rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {

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

@ -146,13 +146,13 @@ static void altitudeHoldTask(void)
float velocityStateDown;
VelocityStateDownGet(&velocityStateDown);
switch (altitudeHoldDesired.ThrottleMode) {
case ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE:
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE:
// 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;
case ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.ThrottleCommand;
case ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY:
altitudeHoldStatus.VelocityDesired = altitudeHoldDesired.SetPoint;
break;
default:
altitudeHoldStatus.VelocityDesired = 0;
@ -162,9 +162,9 @@ static void altitudeHoldTask(void)
AltitudeHoldStatusSet(&altitudeHoldStatus);
float throttle;
switch (altitudeHoldDesired.ThrottleMode) {
case ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE:
throttle = altitudeHoldDesired.ThrottleCommand;
switch (altitudeHoldDesired.ControlMode) {
case ALTITUDEHOLDDESIRED_CONTROLMODE_THROTTLE:
throttle = altitudeHoldDesired.SetPoint;
break;
default:
// velocity control loop

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

@ -882,22 +882,22 @@ static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
AltitudeHoldSettingsCutThrottleWhenZeroGet(&cutOff);
if (cutOff && cmd->Throttle < 0) {
// Cut throttle if desired
altitudeHoldDesiredData.ThrottleCommand = cmd->Throttle;
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_THROTTLE;
altitudeHoldDesiredData.SetPoint = cmd->Throttle;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_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
// 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.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY;
altitudeHoldDesiredData.SetPoint = -((throttleExp * powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - throttleExp) * (cmd->Throttle - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * throttleRate);
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} 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.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_VELOCITY;
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.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY;
newaltitude = true;
} else if (newaltitude == true) {
altitudeHoldDesiredData.ThrottleCommand = posState.Down;
altitudeHoldDesiredData.ThrottleMode = ALTITUDEHOLDDESIRED_THROTTLEMODE_ALTITUDE;
altitudeHoldDesiredData.SetPoint = posState.Down;
altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE;
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)
#include "pios_ms5611.h"
static const struct pios_ms5611_cfg pios_ms5611_cfg = {
.oversampling = MS5611_OSR_512,
.oversampling = MS5611_OSR_4096,
};
#endif /* PIOS_INCLUDE_MS5611 */

4
shared/uavobjectdefinition/altitudeholddesired.xml
View File

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