mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
some changes to altitudehold, hope i get that done today...
This commit is contained in:
parent
3e1b0fd114
commit
ff5927bc43
@ -38,7 +38,6 @@
|
||||
#include "accessorydesired.h"
|
||||
#include "actuatordesired.h"
|
||||
#include "altitudeholddesired.h"
|
||||
#include "altholdsmoothed.h"
|
||||
#include "flighttelemetrystats.h"
|
||||
#include "flightstatus.h"
|
||||
#include "sanitycheck.h"
|
||||
@ -872,8 +871,8 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
|
||||
StabilizationSettingsData stabSettings;
|
||||
StabilizationSettingsGet(&stabSettings);
|
||||
|
||||
AltHoldSmoothedData altHold;
|
||||
AltHoldSmoothedGet(&altHold);
|
||||
PositionStateData posState;
|
||||
PositionStateGet(&posState);
|
||||
|
||||
|
||||
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
|
||||
@ -883,23 +882,23 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
|
||||
if (changed) {
|
||||
// After not being in this mode for a while init at current height
|
||||
altitudeHoldDesiredData.Velocity = 0;
|
||||
altitudeHoldDesiredData.Altitude = altHold.Altitude;
|
||||
altitudeHoldDesiredData.Altitude = posState.Down;
|
||||
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.Altitude = altHold.Altitude;
|
||||
altitudeHoldDesiredData.Altitude = posState.Down;
|
||||
} 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.Altitude = altHold.Altitude;
|
||||
altitudeHoldDesiredData.Altitude = posState.Down;
|
||||
} 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
|
||||
// Vario is not "engaged" when throttleRate == 0
|
||||
if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) {
|
||||
;
|
||||
altitudeHoldDesiredData.Velocity = 0;
|
||||
altitudeHoldDesiredData.Altitude = altHold.Altitude;
|
||||
altitudeHoldDesiredData.Altitude = posState.Down;
|
||||
}
|
||||
zeroed = true;
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user