rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1216 moved safety zeroing for channels from Stabilization to Actuators (so it also affects manual mode)

Browse Source
This commit is contained in:
Corvus Corax 2014-03-02 15:25:49 +01:00
parent 3ea9042279
commit f61231f610
4 changed files with 23 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/modules/Actuator/actuator.c
View File

@ -167,7 +167,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
MixerStatusData mixerStatus;
FlightStatusData flightStatus;
SystemSettingsThrustControlOptions thrustType;
SystemSettingsAllowReverseThrottleOptions noClamping;
SystemSettingsAllowReverseThrottleOptions allowReverseThrottle;
float throttleDesired;
float collectiveDesired;
@ -225,7 +225,7 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
SystemSettingsThrustControlGet(&thrustType);
SystemSettingsAllowReverseThrottleGet(&noClamping);
SystemSettingsAllowReverseThrottleGet(&allowReverseThrottle);
// read in throttle and collective -demultiplex thrust
switch (thrustType) {
@ -242,8 +242,25 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
ManualControlCommandCollectiveGet(&collectiveDesired);
}
if (noClamping == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_FALSE) {
throttleDesired = (throttleDesired < 0) ? -1 : throttleDesired;
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
// safety settings
if (allowReverseThrottle == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_FALSE && (throttleDesired < 0 || !armed)) {
throttleDesired = -1;
} else if (allowReverseThrottle == SYSTEMSETTINGS_ALLOWREVERSETHROTTLE_TRUE && !armed) {
throttleDesired = 0;
}
if (throttleDesired < 0 || !armed) {
// force set all other controls to zero if throttle is cut (previously set in Stabilization)
if (actuatorSettings.LowThrottleZeroAxis.Roll == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Roll = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Pitch == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Pitch = 0;
}
if (actuatorSettings.LowThrottleZeroAxis.Yaw == ACTUATORSETTINGS_LOWTHROTTLEZEROAXIS_TRUE) {
desired.Yaw = 0;
}
}
#ifdef DIAG_MIXERSTATUS
@ -263,7 +280,6 @@ static void actuatorTask(__attribute__((unused)) void *parameters)
AlarmsClear(SYSTEMALARMS_ALARM_ACTUATOR);
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = throttleDesired >= 0.00f;
bool spinWhileArmed = actuatorSettings.MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;

23
flight/modules/Stabilization/stabilization.c
View File

@ -104,7 +104,6 @@ uint8_t max_axislock_rate = 0;
float weak_leveling_kp = 0;
uint8_t weak_leveling_max = 0;
bool lowThrottleZeroIntegral;
bool lowThrottleZeroAxis[MAX_AXES];
float vbar_decay = 0.991f;
struct pid pids[PID_MAX];
@ -597,21 +596,6 @@ static void stabilizationTask(__attribute__((unused)) void *parameters)
actuatorDesired.UpdateTime = dT * 1000;
actuatorDesired.Thrust = stabDesired.Thrust;
// Suppress desired output while disarmed
if (flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED) {
if (lowThrottleZeroAxis[ROLL]) {
actuatorDesired.Roll = 0.0f;
}
if (lowThrottleZeroAxis[PITCH]) {
actuatorDesired.Pitch = 0.0f;
}
if (lowThrottleZeroAxis[YAW]) {
actuatorDesired.Yaw = 0.0f;
}
}
// modify thrust according to 1/cos(bank angle)
// to maintain same altitdue with changing bank angle
// but without manually adjusting thrust
@ -910,12 +894,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
weak_leveling_max = settings.MaxWeakLevelingRate;
// Whether to zero the PID integrals while thrust is low
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or thrust is low
lowThrottleZeroAxis[ROLL] = settings.LowThrottleZeroAxis.Roll == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[PITCH] = settings.LowThrottleZeroAxis.Pitch == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroAxis[YAW] = settings.LowThrottleZeroAxis.Yaw == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE;
lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE;
// The dT has some jitter iteration to iteration that we don't want to
// make thie result unpredictable. Still, it's nicer to specify the constant

1
shared/uavobjectdefinition/actuatorsettings.xml
View File

@ -8,6 +8,7 @@
<field name="ChannelType" units="" type="enum" elements="12" options="PWM,MK,ASTEC4,PWM Alarm Buzzer,Arming led,Info led" defaultvalue="PWM"/>
<field name="ChannelAddr" units="" type="uint8" elements="12" defaultvalue="0,1,2,3,4,5,6,7,8,9,10,11"/>
<field name="MotorsSpinWhileArmed" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -41,8 +41,6 @@
<field name="LowThrottleZeroIntegral" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
<field name="LowThrottleZeroAxis" units="" type="enum" elementnames="Roll,Pitch,Yaw" options="FALSE,TRUE" defaultvalue="FALSE,FALSE,FALSE"/>
<field name="ScaleToAirspeed" units="m/s" type="float" elements="1" defaultvalue="0"/>
<field name="ScaleToAirspeedLimits" units="" type="float" elementnames="Min,Max" defaultvalue="0.05,3"/>