diff --git a/flight/Modules/Stabilization/stabilization.c b/flight/Modules/Stabilization/stabilization.c index a43162e71..3c3821eff 100644 --- a/flight/Modules/Stabilization/stabilization.c +++ b/flight/Modules/Stabilization/stabilization.c @@ -79,6 +79,7 @@ uint8_t max_axislock_rate = 0; float weak_leveling_kp = 0; uint8_t weak_leveling_max = 0; bool lowThrottleZeroIntegral; +bool lowThrottleZeroAxis[3]; float vbar_decay = 0.991f; struct pid pids[PID_MAX]; @@ -357,6 +358,18 @@ static void stabilizationTask(void* parameters) actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.Throttle = stabDesired.Throttle; + // Suppress desired output while disarmed or throttle low, for configured axis + if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || stabDesired.Throttle < 0) { + if(lowThrottleZeroAxis[0]) + actuatorDesired.Roll = 0; + + if(lowThrottleZeroAxis[1]) + actuatorDesired.Pitch = 0; + + if(lowThrottleZeroAxis[2]) + actuatorDesired.Yaw = 0; + } + if(PARSE_FLIGHT_MODE(flightStatus.FlightMode) != FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); } else { @@ -460,6 +473,11 @@ static void SettingsUpdatedCb(UAVObjEvent * ev) // Whether to zero the PID integrals while throttle is low lowThrottleZeroIntegral = settings.LowThrottleZeroIntegral == STABILIZATIONSETTINGS_LOWTHROTTLEZEROINTEGRAL_TRUE; + + // Whether to suppress (zero) the StabilizationDesired output for each axis while disarmed or throttle is low + lowThrottleZeroAxis[0] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_ROLL] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[1] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_PITCH] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_TRUE; + lowThrottleZeroAxis[2] = settings.LowThrottleZeroAxis[STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_YAW] == STABILIZATIONSETTINGS_LOWTHROTTLEZEROAXIS_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 diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index b1b541310..6a8edbad7 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -35,6 +35,8 @@ <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"/> + <access gcs="readwrite" flight="readwrite"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>