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

OP-1022 some small cosmetic fixes for AltitudeHold as suggested by Alessio

Browse Source
This commit is contained in:
Corvus Corax 2014-01-15 18:43:47 +01:00
parent 07d5c8e4d2
commit 92beb54e2d
5 changed files with 21 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/libraries/sanitycheck.c
View File

@ -114,6 +114,8 @@ int32_t configuration_check()
if (coptercontrol) { if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR; severity = SYSTEMALARMS_ALARM_ERROR;
} }
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break; break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL: case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) { if (coptercontrol) {

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

@ -146,13 +146,13 @@ static void altitudeHoldTask(void)
float velocityStateDown; float velocityStateDown;
VelocityStateDownGet(&velocityStateDown); VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ThrottleMode) { switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE: case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
// altitude control loop // altitude control loop
altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.ThrottleCommand, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS); altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, 1.0f, altitudeHoldDesired.SetPoint, positionStateDown, 1000.0f / DESIRED_UPDATE_RATE_MS);
break; break;
case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY: case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand; altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
break; break;
default: default:
altitudeHoldStatus.VelocityDesired = 0; altitudeHoldStatus.VelocityDesired = 0;
@ -162,9 +162,9 @@ static void altitudeHoldTask(void)
AltitudeHoldStatusSet(&altitudeHoldStatus); AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle; float throttle;
switch (altitudeHoldDesired.ThrottleMode) { switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE: case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.ThrottleCommand; throttle = altitudeHoldDesired.SetPoint;
break; break;
default: default:
// velocity control loop // velocity control loop

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

@ -882,22 +882,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff); AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) { if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired // Cut throttle if desired
altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle; altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE; altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE;
newaltitude = true; newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle > DEADBAND_HIGH) { } 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.ThrottleCommand = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate); altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true; newaltitude = true;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) { } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Throttle < DEADBAND_LOW) {
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); altitudeHoldDesiredData.SetPoint = -(-(throttleExp * powf((DEADBAND_LOW - (cmd->Throttle < 0 ? 0 : cmd->Throttle)) / DEADBAND_LOW, 3) + (255 - throttleExp) * (DEADBAND_LOW - cmd->Throttle) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY; altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true; newaltitude = true;
} else if (newaltitude == true) { } else if (newaltitude == true) {
altitudeHoldDesiredData.ThrottleCommand = posState.Down; altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE; altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
newaltitude = false; newaltitude = false;
} }

2
flight/targets/boards/revoproto/firmware/pios_board.c
View File

@ -129,7 +129,7 @@ static const struct pios_hmc5883_cfg pios_hmc5883_cfg = {
#if defined(PIOS_INCLUDE_MS5611) #if defined(PIOS_INCLUDE_MS5611)
#include "pios_ms5611.h" #include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = { static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512, .oversampling = MS5611_OSR_4096,
}; };
#endif /* PIOS_INCLUDE_MS5611 */ #endif /* PIOS_INCLUDE_MS5611 */

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="ThrottleCommand" units="" type="float" elements="1"/> <field name="SetPoint" units="" type="float" elements="1"/>
<field name="ThrottleMode" units="" type="enum" elements="1" options="Altitude,Velocity,Throttle" /> <field name="ControlMode" 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"/>