diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 58704e0fc..b888c0476 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -860,15 +860,16 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) // After not being in this mode for a while init at current height AltHoldSmoothedData altHold; AltHoldSmoothedGet(&altHold); + altitudeHoldDesiredData.Velocity = 0; altitudeHoldDesiredData.Altitude = altHold.Altitude; zeroed = false; } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 - altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = (throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; } else if (cmd->Throttle < DEADBAND_LOW && zeroed) { - altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; + altitudeHoldDesiredData.Velocity = -(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate; altitudeHoldDesiredData.Altitude += altitudeHoldDesiredData.Velocity * dT; } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { // Require the stick to enter the dead band before they can move height