From 92beb54e2dff6c922eb68246cb68d333b8b4fd05 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Wed, 15 Jan 2014 18:43:47 +0100 Subject: [PATCH] OP-1022 some small cosmetic fixes for AltitudeHold as suggested by Alessio --- flight/libraries/sanitycheck.c | 2 ++ flight/modules/AltitudeHold/altitudehold.c | 16 ++++++++-------- flight/modules/ManualControl/manualcontrol.c | 16 ++++++++-------- .../boards/revoproto/firmware/pios_board.c | 2 +- .../uavobjectdefinition/altitudeholddesired.xml | 4 ++-- 5 files changed, 21 insertions(+), 19 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index ab53e2302..8d876a258 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -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) { diff --git a/flight/modules/AltitudeHold/altitudehold.c b/flight/modules/AltitudeHold/altitudehold.c index 6a81518d6..3d71ee2b2 100644 --- a/flight/modules/AltitudeHold/altitudehold.c +++ b/flight/modules/AltitudeHold/altitudehold.c @@ -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 diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 7dcfb32ea..35cfc525c 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -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; } diff --git a/flight/targets/boards/revoproto/firmware/pios_board.c b/flight/targets/boards/revoproto/firmware/pios_board.c index b3a76c386..ed142f951 100644 --- a/flight/targets/boards/revoproto/firmware/pios_board.c +++ b/flight/targets/boards/revoproto/firmware/pios_board.c @@ -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 */ diff --git a/shared/uavobjectdefinition/altitudeholddesired.xml b/shared/uavobjectdefinition/altitudeholddesired.xml index 0dfe36ceb..1b349766f 100644 --- a/shared/uavobjectdefinition/altitudeholddesired.xml +++ b/shared/uavobjectdefinition/altitudeholddesired.xml @@ -1,8 +1,8 @@ Holds the desired altitude (from manual control) as well as the desired attitude to pass through - - + +