rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

OP-1022 overhaul of altitudehold and manualcontrol - cleanup,

Browse Source 
reimplemented safe throttle cutoff and altitudevario features
This commit is contained in:
Corvus Corax 2014-01-11 10:51:47 +01:00
parent 4c078f3dd8
commit ca607ad924
4 changed files with 96 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
flight/modules/AltitudeHold/altitudehold.c
View File

@ -146,13 +146,29 @@ static void altitudeHoldTask(void)
float velocityStateDown; float velocityStateDown;
VelocityStateDownGet(&velocityStateDown); VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ThrottleMode) {
case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE:
// altitude control loop // altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.Altitude, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.ThrottleCommand, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
break;
case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
break;
}
AltitudeHoldStatusSet(&altitudeHoldStatus); AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle;
switch (altitudeHoldDesired.ThrottleMode) {
case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE:
throttle = altitudeHoldDesired.ThrottleCommand;
break;
default:
// velocity control loop // velocity control loop
float throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); throttle = startThrottle - pid_apply_setpoint(&pid1, 1.0f, altitudeHoldStatus.VelocityDesired, velocityStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
// compensate throttle for current attitude // compensate throttle for current attitude
// Rbe[2][2] in the rotation matrix is the rotated down component of a // Rbe[2][2] in the rotation matrix is the rotated down component of a
@ -175,6 +191,9 @@ static void altitudeHoldTask(void)
if (throttle <= 0.0f) { if (throttle <= 0.0f) {
throttle = 0.0f; throttle = 0.0f;
} }
break;
}
StabilizationDesiredData stab; StabilizationDesiredData stab;
StabilizationDesiredGet(&stab); StabilizationDesiredGet(&stab);
stab.Roll = altitudeHoldDesired.Roll; stab.Roll = altitudeHoldDesired.Roll;

57
flight/modules/ManualControl/manualcontrol.c
View File

@ -722,8 +722,7 @@ static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualCont
if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) {
stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
} } else {
else {
stabilization.StabilizationMode.Yaw = stab_settings[2]; stabilization.StabilizationMode.Yaw = stab_settings[2];
stabilization.Yaw = stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
@ -850,30 +849,20 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// Stop updating AltitudeHoldDesired triggering a failsafe condition.
if (cmd->Throttle < 0) {
return;
}
// this is the max speed in m/s at the extents of throttle // this is the max speed in m/s at the extents of throttle
uint8_t throttleRate; uint8_t throttleRate;
uint8_t throttleExp; uint8_t throttleExp;
static uint8_t flightMode; static uint8_t flightMode;
static bool zeroed = false; static bool newaltitude = true;
FlightStatusFlightModeGet(&flightMode); FlightStatusFlightModeGet(&flightMode);
AltitudeHoldDesiredData altitudeHoldDesiredData; AltitudeHoldDesiredData altitudeHoldDesiredData;
AltitudeHoldDesiredGet(&altitudeHoldDesiredData); AltitudeHoldDesiredGet(&altitudeHoldDesiredData);
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) {
throttleExp = 128;
throttleRate = 0;
} else {
AltitudeHoldSettingsThrottleExpGet(&throttleExp); AltitudeHoldSettingsThrottleExpGet(&throttleExp);
AltitudeHoldSettingsThrottleRateGet(&throttleRate); AltitudeHoldSettingsThrottleRateGet(&throttleRate);
}
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
@ -881,33 +870,35 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
PositionStateData posState; PositionStateData posState;
PositionStateGet(&posState); PositionStateGet(&posState);
altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw;
if (changed) { if (changed) {
// After not being in this mode for a while init at current height newaltitude = true;
altitudeHoldDesiredData.Velocity = 0; }
altitudeHoldDesiredData.Altitude = posState.Down;
zeroed = false; uint8_t cutOff;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle;
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE;
newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) {
// being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 // 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 // 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.ThrottleCommand = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.Altitude = posState.Down; altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) { newaltitude = true;
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); } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
altitudeHoldDesiredData.Altitude = posState.Down; altitudeHoldDesiredData.ThrottleCommand = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH && (throttleRate != 0)) { altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY;
// Require the stick to enter the dead band before they can move height newaltitude = true;
// Vario is not "engaged" when throttleRate == 0 } else if (newaltitude == true) {
if (fabsf(altitudeHoldDesiredData.Velocity) > 1e-3f) { altitudeHoldDesiredData.ThrottleCommand = posState.Down;
; altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE;
altitudeHoldDesiredData.Velocity = 0; newaltitude = false;
altitudeHoldDesiredData.Altitude = posState.Down;
}
zeroed = true;
} }
AltitudeHoldDesiredSet(&altitudeHoldDesiredData); AltitudeHoldDesiredSet(&altitudeHoldDesiredData);

4
shared/uavobjectdefinition/altitudeholddesired.xml
View File

@ -1,8 +1,8 @@
<xml> <xml>
<object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control"> <object name="AltitudeHoldDesired" singleinstance="true" settings="false" category="Control">
<description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description> <description>Holds the desired altitude (from manual control) as well as the desired attitude to pass through</description>
<field name="Altitude" units="m" type="float" elements="1"/> <field name="ThrottleCommand" units="" type="float" elements="1"/>
<field name="Velocity" units="m/s" type="float" elements="1"/> <field name="ThrottleMode" units="" type="enum" elements="1" options="Altitude,Velocity,Throttle" />
<field name="Roll" units="deg" type="float" elements="1"/> <field name="Roll" units="deg" type="float" elements="1"/>
<field name="Pitch" units="deg" type="float" elements="1"/> <field name="Pitch" units="deg" type="float" elements="1"/>
<field name="Yaw" units="deg/s" type="float" elements="1"/> <field name="Yaw" units="deg/s" type="float" elements="1"/>

1
shared/uavobjectdefinition/altitudeholdsettings.xml
View File

@ -3,6 +3,7 @@
<description>Settings for the @ref AltitudeHold module</description> <description>Settings for the @ref AltitudeHold module</description>
<field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" /> <field name="AltitudePI" units="(m/s)/m" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.8,0,0" />
<field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" /> <field name="VelocityPI" units="(m/s^2)/(m/s)" type="float" elementnames="Kp,Ki,Ilimit" defaultvalue="0.2,0.0002,2.0" />
<field name="CutThrottleWhenZero" units="bool" type="enum" elements="1" options="False,True" defaultvalue="True" />
<field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" /> <field name="ThrottleExp" units="" type="uint8" elements="1" defaultvalue="128" />
<field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5" /> <field name="ThrottleRate" units="m/s" type="uint8" elements="1" defaultvalue="5" />
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>