diff --git a/flight/modules/PathFollower/pidcontroldown.cpp b/flight/libraries/pid/pidcontroldown.cpp similarity index 96% rename from flight/modules/PathFollower/pidcontroldown.cpp rename to flight/libraries/pid/pidcontroldown.cpp index ea01b7e06..7f06eac93 100644 --- a/flight/modules/PathFollower/pidcontroldown.cpp +++ b/flight/libraries/pid/pidcontroldown.cpp @@ -45,7 +45,6 @@ extern "C" { #include } -#include "pathfollowerfsm.h" #include "pidcontroldown.h" #define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f @@ -59,17 +58,17 @@ extern "C" { PIDControlDown::PIDControlDown() : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f), - mFSM(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f), - mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false) + mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f), + mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true) { Deactivate(); } PIDControlDown::~PIDControlDown() {} -void PIDControlDown::Initialize(PathFollowerFSM *fsm) +void PIDControlDown::Initialize(PIDControlDownCallback *callback) { - mFSM = fsm; + mCallback = callback; } void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust) @@ -80,7 +79,6 @@ void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust) void PIDControlDown::Deactivate() { - // pid_zero(&PID); mActive = false; } @@ -333,9 +331,9 @@ void PIDControlDown::UpdateVelocityState(float pv) { mVelocityState = pv; - if (mFSM) { + if (mCallback) { // The FSM controls the actual descent velocity and introduces step changes as required - float velocitySetpointDesired = mFSM->BoundVelocityDown(mVelocitySetpointTarget); + float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget); // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f ); mVelocitySetpointCurrent = velocitySetpointDesired; } else { @@ -354,8 +352,8 @@ float PIDControlDown::GetDownCommand(void) float ulow = mMinThrust; float uhigh = mMaxThrust; - if (mFSM) { - mFSM->BoundThrust(ulow, uhigh); + if (mCallback) { + mCallback->BoundThrust(ulow, uhigh); } float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh); pidStatus.setpoint = mVelocitySetpointCurrent; diff --git a/flight/modules/PathFollower/inc/pidcontroldown.h b/flight/libraries/pid/pidcontroldown.h similarity index 88% rename from flight/modules/PathFollower/inc/pidcontroldown.h rename to flight/libraries/pid/pidcontroldown.h index 9546e2e75..8ea185610 100644 --- a/flight/modules/PathFollower/inc/pidcontroldown.h +++ b/flight/libraries/pid/pidcontroldown.h @@ -34,16 +34,20 @@ extern "C" { #include #include } -#include "pathfollowerfsm.h" +#include "pidcontroldowncallback.h" class PIDControlDown { public: PIDControlDown(); ~PIDControlDown(); - void Initialize(PathFollowerFSM *fsm); + void Initialize(PIDControlDownCallback *callback); void SetThrustLimits(float min_thrust, float max_thrust); void Deactivate(); void Activate(); + bool IsActive() + { + return mActive; + } void UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax); void UpdateNeutralThrust(float neutral); void UpdateVelocitySetpoint(float setpoint); @@ -58,6 +62,14 @@ public: void ControlPositionWithPath(struct path_status *progress); void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); void UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate); + void DisableNeutralThrustCalc() + { + mAllowNeutralThrustCalc = false; + } + void EnableNeutralThrustCalc() + { + mAllowNeutralThrustCalc = true; + } private: void setup_neutralThrustCalc(); @@ -69,7 +81,7 @@ private: float mVelocitySetpointCurrent; float mVelocityState; float mDownCommand; - PathFollowerFSM *mFSM; + PIDControlDownCallback *mCallback; float mNeutral; float mVelocityMax; struct pid PIDpos; @@ -77,7 +89,6 @@ private: float mPositionState; float mMinThrust; float mMaxThrust; - uint8_t mActive; struct NeutralThrustEstimation { uint32_t count; @@ -90,6 +101,8 @@ private: bool have_correction; }; struct NeutralThrustEstimation neutralThrustEst; + bool mActive; + bool mAllowNeutralThrustCalc; }; #endif // PIDCONTROLDOWN_H diff --git a/flight/libraries/pid/pidcontroldowncallback.h b/flight/libraries/pid/pidcontroldowncallback.h new file mode 100644 index 000000000..b6aedf8dd --- /dev/null +++ b/flight/libraries/pid/pidcontroldowncallback.h @@ -0,0 +1,42 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup PID Library + * @brief Thrust control callback pure virtual + * @{ + * + * @file pidcontroldowncallback.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015. + * @brief Interface class for PathFollower FSMs + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIDCONTROLDOWNCALLBACK_H +#define PIDCONTROLDOWNCALLBACK_H + +class PIDControlDownCallback { +public: + // PIDControlDownCalback() {}; + virtual void BoundThrust(__attribute__((unused)) float &ulow, __attribute__((unused)) float &uhigh) = 0; + virtual float BoundVelocityDown(float velocity) = 0; + // virtual ~PIDControlDownCalback() = 0; +}; + +#endif // PIDCONTROLDOWNCALLBACK_H diff --git a/flight/modules/PathFollower/inc/pathfollowerfsm.h b/flight/modules/PathFollower/inc/pathfollowerfsm.h index 9701bc7de..0188cc71c 100644 --- a/flight/modules/PathFollower/inc/pathfollowerfsm.h +++ b/flight/modules/PathFollower/inc/pathfollowerfsm.h @@ -34,6 +34,7 @@ extern "C" { #include } +#include typedef enum { PFFSM_STATE_INACTIVE = 0, // Inactive state is the initialised state on startup @@ -42,7 +43,7 @@ typedef enum { PFFSM_STATE_ABORT // Abort on error } PathFollowerFSMState_T; -class PathFollowerFSM { +class PathFollowerFSM : public PIDControlDownCallback { public: // PathFollowerFSM() {}; virtual void Inactive(void) {} diff --git a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp index 729f65097..2c4bdc569 100644 --- a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp +++ b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp @@ -126,7 +126,7 @@ void VtolAutoTakeoffController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp index 208e862f7..862011057 100644 --- a/flight/modules/PathFollower/vtolbrakecontroller.cpp +++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp @@ -132,7 +132,7 @@ void VtolBrakeController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings->BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings->BrakeHorizontalVelPID.Kd, - vtolPathFollowerSettings->BrakeHorizontalVelPID.ILimit, + vtolPathFollowerSettings->BrakeHorizontalVelPID.Beta, dT, 10.0f * vtolPathFollowerSettings->HorizontalVelMax); // avoid constraining initial fast entry into brake controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); diff --git a/flight/modules/PathFollower/vtolflycontroller.cpp b/flight/modules/PathFollower/vtolflycontroller.cpp index e76c09d54..20a387277 100644 --- a/flight/modules/PathFollower/vtolflycontroller.cpp +++ b/flight/modules/PathFollower/vtolflycontroller.cpp @@ -127,7 +127,7 @@ void VtolFlyController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); @@ -136,7 +136,7 @@ void VtolFlyController::SettingsUpdated(void) controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp, vtolPathFollowerSettings->VerticalVelPID.Ki, vtolPathFollowerSettings->VerticalVelPID.Kd, - vtolPathFollowerSettings->VerticalVelPID.ILimit, // TODO Change to BETA + vtolPathFollowerSettings->VerticalVelPID.Beta, dT, vtolPathFollowerSettings->VerticalVelMax); controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); @@ -145,6 +145,9 @@ void VtolFlyController::SettingsUpdated(void) VtolSelfTuningStatsGet(&vtolSelfTuningStats); controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral); controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max); + + // disable neutral thrust calcs which should only be done in a hold mode. + controlDown.DisableNeutralThrustCalc(); } /** diff --git a/flight/modules/PathFollower/vtollandcontroller.cpp b/flight/modules/PathFollower/vtollandcontroller.cpp index 96b4a40c7..97a7d466f 100644 --- a/flight/modules/PathFollower/vtollandcontroller.cpp +++ b/flight/modules/PathFollower/vtollandcontroller.cpp @@ -120,7 +120,7 @@ void VtolLandController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); diff --git a/flight/modules/PathFollower/vtolvelocitycontroller.cpp b/flight/modules/PathFollower/vtolvelocitycontroller.cpp index 84ba99daa..68018c5ea 100644 --- a/flight/modules/PathFollower/vtolvelocitycontroller.cpp +++ b/flight/modules/PathFollower/vtolvelocitycontroller.cpp @@ -107,7 +107,7 @@ void VtolVelocityController::SettingsUpdated(void) controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, vtolPathFollowerSettings->HorizontalVelPID.Ki, vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.ILimit, + vtolPathFollowerSettings->HorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); @@ -181,7 +181,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused) ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; // default thrust mode to altvario diff --git a/flight/modules/Stabilization/altitudeloop.c b/flight/modules/Stabilization/altitudeloop.cpp similarity index 52% rename from flight/modules/Stabilization/altitudeloop.c rename to flight/modules/Stabilization/altitudeloop.cpp index 489e6e0d1..dab918ae4 100644 --- a/flight/modules/Stabilization/altitudeloop.c +++ b/flight/modules/Stabilization/altitudeloop.cpp @@ -1,7 +1,7 @@ /** ****************************************************************************** * - * @file altitudeloop.c + * @file altitudeloop.cpp * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief This module compared @ref PositionActuatl to @ref ActiveWaypoint * and sets @ref AttitudeDesired. It only does this when the FlightMode field @@ -26,6 +26,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +extern "C" { #include #include @@ -37,6 +38,12 @@ #include #include #include +#include +#include +} + +#include + // Private constants @@ -50,78 +57,134 @@ #define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW #define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL -#define STACK_SIZE_BYTES 512 // Private types // Private variables static DelayedCallbackInfo *altitudeHoldCBInfo; +static PIDControlDown controlDown; static AltitudeHoldSettingsData altitudeHoldSettings; -static struct pid pid0, pid1; static ThrustModeType thrustMode; -static PiOSDeltatimeConfig timeval; -static float thrustSetpoint = 0.0f; -static float thrustDemand = 0.0f; -static float startThrust = 0.5f; +static float thrustDemand = 0.0f; // Private functions -static void altitudeHoldTask(void); static void SettingsUpdatedCb(UAVObjEvent *ev); +static void altitudeHoldTask(void); static void VelocityStateUpdatedCb(UAVObjEvent *ev); /** * Setup mode and setpoint + * + * reinit: true when althold/vario mode selected over a previous alternate thrust mode */ float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit) { static bool newaltitude = true; - if (reinit) { - startThrust = setpoint; - pid_zero(&pid0); - pid_zero(&pid1); + if (reinit || !controlDown.IsActive()) { + controlDown.Activate(); newaltitude = true; + // calculate a thrustDemand on reinit only + altitudeHoldTask(); } const float DEADBAND = 0.20f; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2; const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2; - // this is the max speed in m/s at the extents of thrust - float thrustRate; - uint8_t thrustExp; - - AltitudeHoldSettingsThrustExpGet(&thrustExp); - AltitudeHoldSettingsThrustRateGet(&thrustRate); - - PositionStateData posState; - PositionStateGet(&posState); if (altitudeHoldSettings.CutThrustWhenZero && setpoint <= 0) { // Cut thrust if desired - thrustSetpoint = 0.0f; - thrustDemand = 0.0f; - thrustMode = DIRECT; - newaltitude = true; + controlDown.UpdateVelocitySetpoint(0.0f); + thrustDemand = 0.0f; + thrustMode = DIRECT; + newaltitude = true; + return thrustDemand; } else if (mode == ALTITUDEVARIO && setpoint > 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 - thrustSetpoint = -((thrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); - thrustMode = ALTITUDEVARIO; - newaltitude = true; + controlDown.UpdateVelocitySetpoint(-((altitudeHoldSettings.ThrustExp * powf((setpoint - DEADBAND_HIGH) / (DEADBAND_LOW), 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (setpoint - DEADBAND_HIGH) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate)); + thrustMode = ALTITUDEVARIO; + newaltitude = true; } else if (mode == ALTITUDEVARIO && setpoint < DEADBAND_LOW) { - thrustSetpoint = -(-(thrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255 * thrustRate); - thrustMode = ALTITUDEVARIO; - newaltitude = true; + controlDown.UpdateVelocitySetpoint(-(-(altitudeHoldSettings.ThrustExp * powf((DEADBAND_LOW - (setpoint < 0 ? 0 : setpoint)) / DEADBAND_LOW, 3.0f) + (255.0f - altitudeHoldSettings.ThrustExp) * (DEADBAND_LOW - setpoint) / DEADBAND_LOW) / 255.0f * altitudeHoldSettings.ThrustRate)); + thrustMode = ALTITUDEVARIO; + newaltitude = true; } else if (newaltitude == true) { - thrustSetpoint = posState.Down; - thrustMode = ALTITUDEHOLD; - newaltitude = false; + controlDown.UpdateVelocitySetpoint(0.0f); + PositionStateData posState; + PositionStateGet(&posState); + controlDown.UpdatePositionSetpoint(posState.Down); + thrustMode = ALTITUDEHOLD; + newaltitude = false; } + thrustDemand = boundf(thrustDemand, altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max); + return thrustDemand; } +/** + * Disable the alt hold task loop velocity controller to save cpu cycles + */ +void stabilizationDisableAltitudeHold(void) +{ + controlDown.Deactivate(); +} + + +/** + * Module thread, should not return. + */ +static void altitudeHoldTask(void) +{ + if (!controlDown.IsActive()) { + return; + } + + AltitudeHoldStatusData altitudeHoldStatus; + AltitudeHoldStatusGet(&altitudeHoldStatus); + + float velocityStateDown; + VelocityStateDownGet(&velocityStateDown); + controlDown.UpdateVelocityState(velocityStateDown); + + float local_thrustDemand = 0.0f; + + switch (thrustMode) { + case ALTITUDEHOLD: + { + float positionStateDown; + PositionStateDownGet(&positionStateDown); + controlDown.UpdatePositionState(positionStateDown); + controlDown.ControlPosition(); + altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD; + local_thrustDemand = controlDown.GetDownCommand(); + } + break; + + case ALTITUDEVARIO: + altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO; + local_thrustDemand = controlDown.GetDownCommand(); + break; + + case DIRECT: + altitudeHoldStatus.VelocityDesired = 0.0f; + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT; + break; + } + + AltitudeHoldStatusSet(&altitudeHoldStatus); + thrustDemand = local_thrustDemand; +} + +static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); +} + /** * Initialise the module, called on startup */ @@ -131,82 +194,39 @@ void stabilizationAltitudeloopInit() AltitudeHoldStatusInitialize(); PositionStateInitialize(); VelocityStateInitialize(); + VtolSelfTuningStatsInitialize(); - PIOS_DELTATIME_Init(&timeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); - // Create object queue + AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); + VtolSelfTuningStatsConnectCallback(&SettingsUpdatedCb); + SettingsUpdatedCb(NULL); altitudeHoldCBInfo = PIOS_CALLBACKSCHEDULER_Create(&altitudeHoldTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES); - AltitudeHoldSettingsConnectCallback(&SettingsUpdatedCb); VelocityStateConnectCallback(&VelocityStateUpdatedCb); - - // Start main task - SettingsUpdatedCb(NULL); } -/** - * Module thread, should not return. - */ -static void altitudeHoldTask(void) -{ - AltitudeHoldStatusData altitudeHoldStatus; - - AltitudeHoldStatusGet(&altitudeHoldStatus); - - // do the actual control loop(s) - float positionStateDown; - PositionStateDownGet(&positionStateDown); - float velocityStateDown; - VelocityStateDownGet(&velocityStateDown); - - float dT; - dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); - switch (thrustMode) { - case ALTITUDEHOLD: - { - // altitude control loop - // No scaling. - const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; - altitudeHoldStatus.VelocityDesired = pid_apply_setpoint(&pid0, &scaler, thrustSetpoint, positionStateDown, dT); - } - break; - case ALTITUDEVARIO: - altitudeHoldStatus.VelocityDesired = thrustSetpoint; - break; - default: - altitudeHoldStatus.VelocityDesired = 0; - break; - } - - AltitudeHoldStatusSet(&altitudeHoldStatus); - - switch (thrustMode) { - case DIRECT: - thrustDemand = thrustSetpoint; - break; - default: - { - // velocity control loop - // No scaling. - const pid_scaler scaler = { .p = 1.0f, .i = 1.0f, .d = 1.0f }; - thrustDemand = startThrust - pid_apply_setpoint(&pid1, &scaler, altitudeHoldStatus.VelocityDesired, velocityStateDown, dT); - } - break; - } -} - static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AltitudeHoldSettingsGet(&altitudeHoldSettings); - pid_configure(&pid0, altitudeHoldSettings.AltitudePI.Kp, altitudeHoldSettings.AltitudePI.Ki, 0, altitudeHoldSettings.AltitudePI.Ilimit); - pid_zero(&pid0); - pid_configure(&pid1, altitudeHoldSettings.VelocityPI.Kp, altitudeHoldSettings.VelocityPI.Ki, 0, altitudeHoldSettings.VelocityPI.Ilimit); - pid_zero(&pid1); -} -static void VelocityStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - PIOS_CALLBACKSCHEDULER_Dispatch(altitudeHoldCBInfo); + controlDown.UpdateParameters(altitudeHoldSettings.VerticalVelPID.Kp, + altitudeHoldSettings.VerticalVelPID.Ki, + altitudeHoldSettings.VerticalVelPID.Kd, + altitudeHoldSettings.VerticalVelPID.Beta, + (float)(UPDATE_EXPECTED), + altitudeHoldSettings.ThrustRate); + + controlDown.UpdatePositionalParameters(altitudeHoldSettings.VerticalPosP); + + VtolSelfTuningStatsData vtolSelfTuningStats; + VtolSelfTuningStatsGet(&vtolSelfTuningStats); + controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + altitudeHoldSettings.ThrustLimits.Neutral); + + // initialise limits on thrust but note the FSM can override. + controlDown.SetThrustLimits(altitudeHoldSettings.ThrustLimits.Min, altitudeHoldSettings.ThrustLimits.Max); + + // disable neutral thrust calcs which should only be done in a hold mode. + controlDown.DisableNeutralThrustCalc(); } diff --git a/flight/modules/Stabilization/inc/altitudeloop.h b/flight/modules/Stabilization/inc/altitudeloop.h index d4789c6eb..f29709149 100644 --- a/flight/modules/Stabilization/inc/altitudeloop.h +++ b/flight/modules/Stabilization/inc/altitudeloop.h @@ -37,5 +37,6 @@ typedef enum { ALTITUDEHOLD = 0, ALTITUDEVARIO = 1, DIRECT = 2 } ThrustModeType; void stabilizationAltitudeloopInit(); float stabilizationAltitudeHold(float setpoint, ThrustModeType mode, bool reinit); +void stabilizationDisableAltitudeHold(void); #endif /* ALTITUDELOOP_H */ diff --git a/flight/modules/Stabilization/outerloop.c b/flight/modules/Stabilization/outerloop.c index 7771591c9..1fb2a4061 100644 --- a/flight/modules/Stabilization/outerloop.c +++ b/flight/modules/Stabilization/outerloop.c @@ -103,7 +103,33 @@ static void stabilizationOuterloopTask() float *stabilizationDesiredAxis = &stabilizationDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; int t; - float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + float dT = PIOS_DELTATIME_GetAverageSeconds(&timeval); + + bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST] != previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST]); + previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]; +#ifdef REVOLUTION + if (reinit && (previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE || + previous_mode[STABILIZATIONSTATUS_OUTERLOOP_THRUST] == STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO)) { + // disable the altvario velocity control loop + stabilizationDisableAltitudeHold(); + } +#endif /* REVOLUTION */ + switch (StabilizationStatusOuterLoopToArray(enabled)[STABILIZATIONSTATUS_OUTERLOOP_THRUST]) { +#ifdef REVOLUTION + case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: + rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEHOLD, reinit); + break; + case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO: + rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationAltitudeHold(stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST], ALTITUDEVARIO, reinit); + break; +#endif /* REVOLUTION */ + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: + case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: + default: + rateDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST] = stabilizationDesiredAxis[STABILIZATIONSTATUS_OUTERLOOP_THRUST]; + break; + } + float local_error[3]; { @@ -151,151 +177,134 @@ static void stabilizationOuterloopTask() } - for (t = 0; t < AXES; t++) { - bool reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); + for (t = STABILIZATIONSTATUS_OUTERLOOP_ROLL; t < STABILIZATIONSTATUS_OUTERLOOP_THRUST; t++) { + reinit = (StabilizationStatusOuterLoopToArray(enabled)[t] != previous_mode[t]); previous_mode[t] = StabilizationStatusOuterLoopToArray(enabled)[t]; - if (t < STABILIZATIONSTATUS_OUTERLOOP_THRUST) { - if (reinit) { - stabSettings.outerPids[t].iAccumulator = 0; - } - switch (StabilizationStatusOuterLoopToArray(enabled)[t]) { - case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: - rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); - break; - case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE: - { - float stickinput[3]; - stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); - stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); - stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); - float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; - // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together - rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT), - -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t], - StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t] - ); - // Compute the weighted average rate desired - // Using max() rather than sqrt() for cpu speed; - // - this makes the stick region into a square; - // - this is a feature! - // - hold a roll angle and add just pitch without the stick sensitivity changing - float magnitude = fabsf(stickinput[t]); - if (t < 2) { - magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1])); - } - - // modify magnitude to move the Att to Rate transition to the place - // specified by the user - // we are looking for where the stick angle == transition angle - // and the Att rate equals the Rate rate - // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] - // == Rate x StickAngle [Rate pulling up according to stick angle] - // * StickAngle [X Ratt proportion] - // so 1-x == x*x or x*x+x-1=0 where xE(0,1) - // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 - // and quadratic formula says that is 0.618033989f - // I tested 14.01 and came up with .61 without even remembering this number - // I thought that moving the P,I, and maxangle terms around would change this value - // and that I would have to take these into account, but varying - // all P's and I's by factors of 1/2 to 2 didn't change it noticeably - // and varying maxangle from 4 to 120 didn't either. - // so for now I'm not taking these into account - // while working with this, it occurred to me that Attitude mode, - // set up with maxangle=190 would be similar to Ratt, and it is. - #define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f - - // the following assumes the transition would otherwise be at 0.618033989f - // and THAT assumes that Att ramps up to max roll rate - // when a small number of degrees off of where it should be - - // if below the transition angle (still in attitude mode) - // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ - if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) { - magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position; - } else { - magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position) - * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) - / (1.0f - stabSettings.rattitude_mode_transition_stick_position) - + STICK_VALUE_AT_MODE_TRANSITION; - } - rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate; - } + if (reinit) { + stabSettings.outerPids[t].iAccumulator = 0; + } + switch (StabilizationStatusOuterLoopToArray(enabled)[t]) { + case STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE: + rateDesiredAxis[t] = pid_apply(&stabSettings.outerPids[t], local_error[t], dT); break; - case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING: - // FIXME: local_error[] is rate - attitude for Weak Leveling - // The only ramifications are: - // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS - // Changing Rate mode max rate currently requires a change to Kp - // That would be changed to Attitude mode max angle affecting Kp - // Also does not take dT into account - { - float stickinput[3]; - stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); - stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); - stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); - float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; - float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; - weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); - - // Compute desired rate as input biased towards leveling - rateDesiredAxis[t] = rate_input + weak_leveling; + case STABILIZATIONSTATUS_OUTERLOOP_RATTITUDE: + { + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); + float rateDesiredAxisRate = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; + // limit corrective rate to maximum rates to not give it overly large impact over manual rate when joined together + rateDesiredAxis[t] = boundf(pid_apply(&stabSettings.outerPids[t], local_error[t], dT), + -StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t], + StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t] + ); + // Compute the weighted average rate desired + // Using max() rather than sqrt() for cpu speed; + // - this makes the stick region into a square; + // - this is a feature! + // - hold a roll angle and add just pitch without the stick sensitivity changing + float magnitude = fabsf(stickinput[t]); + if (t < 2) { + magnitude = fmaxf(fabsf(stickinput[0]), fabsf(stickinput[1])); } - break; - case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: - rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes - // now test limits for pitch and/or roll - if (t == 1) { // pitch - if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) { - // attitude exceeds pitch max. - // zero rate desired if also -ve - if (rateDesiredAxis[t] < 0.0f) { - rateDesiredAxis[t] = 0.0f; - } - } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) { - // attitude exceeds pitch max - // zero rate desired if also +ve - if (rateDesiredAxis[t] > 0.0f) { - rateDesiredAxis[t] = 0.0f; - } + + // modify magnitude to move the Att to Rate transition to the place + // specified by the user + // we are looking for where the stick angle == transition angle + // and the Att rate equals the Rate rate + // that's where Rate x (1-StickAngle) [Attitude pulling down max X Ratt proportion] + // == Rate x StickAngle [Rate pulling up according to stick angle] + // * StickAngle [X Ratt proportion] + // so 1-x == x*x or x*x+x-1=0 where xE(0,1) + // (-1+-sqrt(1+4))/2 = (-1+sqrt(5))/2 + // and quadratic formula says that is 0.618033989f + // I tested 14.01 and came up with .61 without even remembering this number + // I thought that moving the P,I, and maxangle terms around would change this value + // and that I would have to take these into account, but varying + // all P's and I's by factors of 1/2 to 2 didn't change it noticeably + // and varying maxangle from 4 to 120 didn't either. + // so for now I'm not taking these into account + // while working with this, it occurred to me that Attitude mode, + // set up with maxangle=190 would be similar to Ratt, and it is. +#define STICK_VALUE_AT_MODE_TRANSITION 0.618033989f + + // the following assumes the transition would otherwise be at 0.618033989f + // and THAT assumes that Att ramps up to max roll rate + // when a small number of degrees off of where it should be + + // if below the transition angle (still in attitude mode) + // '<=' instead of '<' keeps rattitude_mode_transition_stick_position==1.0 from causing DZ + if (magnitude <= stabSettings.rattitude_mode_transition_stick_position) { + magnitude *= STICK_VALUE_AT_MODE_TRANSITION / stabSettings.rattitude_mode_transition_stick_position; + } else { + magnitude = (magnitude - stabSettings.rattitude_mode_transition_stick_position) + * (1.0f - STICK_VALUE_AT_MODE_TRANSITION) + / (1.0f - stabSettings.rattitude_mode_transition_stick_position) + + STICK_VALUE_AT_MODE_TRANSITION; + } + rateDesiredAxis[t] = (1.0f - magnitude) * rateDesiredAxis[t] + magnitude * rateDesiredAxisRate; + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_WEAKLEVELING: + // FIXME: local_error[] is rate - attitude for Weak Leveling + // The only ramifications are: + // Weak Leveling Kp is off by a factor of 3 to 12 and may need a different default in GCS + // Changing Rate mode max rate currently requires a change to Kp + // That would be changed to Attitude mode max angle affecting Kp + // Also does not take dT into account + { + float stickinput[3]; + stickinput[0] = boundf(stabilizationDesiredAxis[0] / stabSettings.stabBank.RollMax, -1.0f, 1.0f); + stickinput[1] = boundf(stabilizationDesiredAxis[1] / stabSettings.stabBank.PitchMax, -1.0f, 1.0f); + stickinput[2] = boundf(stabilizationDesiredAxis[2] / stabSettings.stabBank.YawMax, -1.0f, 1.0f); + float rate_input = stickinput[t] * StabilizationBankManualRateToArray(stabSettings.stabBank.ManualRate)[t]; + float weak_leveling = local_error[t] * stabSettings.settings.WeakLevelingKp; + weak_leveling = boundf(weak_leveling, -stabSettings.settings.MaxWeakLevelingRate, stabSettings.settings.MaxWeakLevelingRate); + + // Compute desired rate as input biased towards leveling + rateDesiredAxis[t] = rate_input + weak_leveling; + } + break; + case STABILIZATIONSTATUS_OUTERLOOP_DIRECTWITHLIMITS: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; // default for all axes + // now test limits for pitch and/or roll + if (t == 1) { // pitch + if (attitudeState.Pitch < -stabSettings.stabBank.PitchMax) { + // attitude exceeds pitch max. + // zero rate desired if also -ve + if (rateDesiredAxis[t] < 0.0f) { + rateDesiredAxis[t] = 0.0f; } - } else if (t == 0) { // roll - if (attitudeState.Roll < -stabSettings.stabBank.RollMax) { - // attitude exceeds roll max. - // zero rate desired if also -ve - if (rateDesiredAxis[t] < 0.0f) { - rateDesiredAxis[t] = 0.0f; - } - } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) { - // attitude exceeds roll max - // zero rate desired if also +ve - if (rateDesiredAxis[t] > 0.0f) { - rateDesiredAxis[t] = 0.0f; - } + } else if (attitudeState.Pitch > stabSettings.stabBank.PitchMax) { + // attitude exceeds pitch max + // zero rate desired if also +ve + if (rateDesiredAxis[t] > 0.0f) { + rateDesiredAxis[t] = 0.0f; } } - break; + } else if (t == 0) { // roll + if (attitudeState.Roll < -stabSettings.stabBank.RollMax) { + // attitude exceeds roll max. + // zero rate desired if also -ve + if (rateDesiredAxis[t] < 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } else if (attitudeState.Roll > stabSettings.stabBank.RollMax) { + // attitude exceeds roll max + // zero rate desired if also +ve + if (rateDesiredAxis[t] > 0.0f) { + rateDesiredAxis[t] = 0.0f; + } + } + } + break; - case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: - default: - rateDesiredAxis[t] = stabilizationDesiredAxis[t]; - break; - } - } else { - switch (StabilizationStatusOuterLoopToArray(enabled)[t]) { -#ifdef REVOLUTION - case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDE: - rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEHOLD, reinit); - break; - case STABILIZATIONSTATUS_OUTERLOOP_ALTITUDEVARIO: - rateDesiredAxis[t] = stabilizationAltitudeHold(stabilizationDesiredAxis[t], ALTITUDEVARIO, reinit); - break; -#endif /* REVOLUTION */ - case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: - default: - rateDesiredAxis[t] = stabilizationDesiredAxis[t]; - break; - } + case STABILIZATIONSTATUS_OUTERLOOP_DIRECT: + default: + rateDesiredAxis[t] = stabilizationDesiredAxis[t]; + break; } } @@ -314,7 +323,7 @@ static void stabilizationOuterloopTask() } } - // update cruisecontrol based on attitude +// update cruisecontrol based on attitude cruisecontrol_compute_factor(&attitudeState, rateDesired.Thrust); stabSettings.monitor.rateupdates = 0; } diff --git a/flight/modules/TxPID/txpid.c b/flight/modules/TxPID/txpid.c index c4e94fbc8..7b6b7b2ee 100644 --- a/flight/modules/TxPID/txpid.c +++ b/flight/modules/TxPID/txpid.c @@ -55,6 +55,9 @@ #include "accessorydesired.h" #include "manualcontrolcommand.h" #include "stabilizationsettings.h" +#ifdef REVOLUTION +#include "altitudeholdsettings.h" +#endif #include "stabilizationbank.h" #include "stabilizationsettingsbank1.h" #include "stabilizationsettingsbank2.h" @@ -95,6 +98,10 @@ int32_t TxPIDInitialize(void) bool txPIDEnabled; HwSettingsOptionalModulesData optionalModules; +#ifdef REVOLUTION + AltitudeHoldSettingsInitialize(); +#endif + HwSettingsInitialize(); HwSettingsOptionalModulesGet(&optionalModules); @@ -188,10 +195,17 @@ static void updatePIDs(UAVObjEvent *ev) } StabilizationSettingsData stab; StabilizationSettingsGet(&stab); +#ifdef REVOLUTION + AltitudeHoldSettingsData altitude; + AltitudeHoldSettingsGet(&altitude); +#endif AccessoryDesiredData accessory; - uint8_t needsUpdateBank = 0; - uint8_t needsUpdateStab = 0; + uint8_t needsUpdateBank = 0; + uint8_t needsUpdateStab = 0; +#ifdef REVOLUTION + uint8_t needsUpdateAltitude = 0; +#endif // Loop through every enabled instance for (uint8_t i = 0; i < TXPIDSETTINGS_PIDS_NUMELEM; i++) { @@ -351,6 +365,23 @@ static void updatePIDs(UAVObjEvent *ev) case TXPIDSETTINGS_PIDS_ACROPLUSFACTOR: needsUpdateBank |= update(&bank.AcroInsanityFactor, value); break; +#ifdef REVOLUTION + case TXPIDSETTINGS_PIDS_ALTITUDEPOSKP: + needsUpdateAltitude |= update(&altitude.VerticalPosP, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKP: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kp, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKI: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Ki, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYKD: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Kd, value); + break; + case TXPIDSETTINGS_PIDS_ALTITUDEVELOCITYBETA: + needsUpdateAltitude |= update(&altitude.VerticalVelPID.Beta, value); + break; +#endif default: PIOS_Assert(0); } @@ -359,6 +390,11 @@ static void updatePIDs(UAVObjEvent *ev) if (needsUpdateStab) { StabilizationSettingsSet(&stab); } +#ifdef REVOLUTION + if (needsUpdateAltitude) { + AltitudeHoldSettingsSet(&altitude); + } +#endif if (needsUpdateBank) { switch (inst.BankNumber) { case 0: diff --git a/flight/targets/boards/discoveryf4bare/firmware/Makefile b/flight/targets/boards/discoveryf4bare/firmware/Makefile index f0d0e483f..4d9c3064c 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/Makefile +++ b/flight/targets/boards/discoveryf4bare/firmware/Makefile @@ -24,6 +24,9 @@ endif include ../board-info.mk include $(ROOT_DIR)/make/firmware-defs.mk +# REVO C++ support +USE_CXX = YES + # ARM DSP library USE_DSP_LIB ?= NO @@ -70,7 +73,7 @@ ifndef TESTAPP ## Application Core SRC += ../pios_usb_board_data.c SRC += $(OPMODULEDIR)/System/systemmod.c - SRC += $(OPSYSTEM)/discoveryf4bare.c + CPPSRC += $(OPSYSTEM)/discoveryf4bare.cpp SRC += $(OPSYSTEM)/pios_board.c SRC += $(FLIGHTLIB)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c @@ -91,6 +94,7 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/insgps13state.c SRC += $(FLIGHTLIB)/auxmagsupport.c SRC += $(FLIGHTLIB)/lednotification.c + CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index b2fe81720..1d7f0e0c2 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -19,6 +19,8 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += statusgrounddrive +UAVOBJSRCFILENAMES += pidstatus UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += statusvtolautotakeoff UAVOBJSRCFILENAMES += vtolselftuningstats diff --git a/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c b/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp similarity index 99% rename from flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c rename to flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp index 19e33890b..268325b26 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.c +++ b/flight/targets/boards/discoveryf4bare/firmware/discoveryf4bare.cpp @@ -31,7 +31,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ - +extern "C" { #include "inc/openpilot.h" #include @@ -74,6 +74,7 @@ static void initTask(void *parameters); /* Prototype of generated InitModules() function */ extern void InitModules(void); +} /** * OpenPilot Main function: diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index c040a0421..a47856179 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -24,6 +24,9 @@ endif include ../board-info.mk include $(ROOT_DIR)/make/firmware-defs.mk +# REVO C++ support +USE_CXX = YES + # ARM DSP library USE_DSP_LIB ?= NO @@ -65,7 +68,7 @@ ifndef TESTAPP ## Application Core SRC += ../pios_usb_board_data.c SRC += $(OPMODULEDIR)/System/systemmod.c - SRC += $(OPSYSTEM)/revolution.c + CPPSRC += $(OPSYSTEM)/revolution.cpp SRC += $(OPSYSTEM)/pios_board.c SRC += $(FLIGHTLIB)/alarms.c SRC += $(OPUAVTALK)/uavtalk.c @@ -84,6 +87,8 @@ ifndef TESTAPP SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c SRC += $(FLIGHTLIB)/auxmagsupport.c + CPPSRC += $(FLIGHTLIB)/mini_cpp.cpp + ## UAVObjects include ./UAVObjects.inc diff --git a/flight/targets/boards/revoproto/firmware/revolution.c b/flight/targets/boards/revoproto/firmware/revolution.cpp similarity index 99% rename from flight/targets/boards/revoproto/firmware/revolution.c rename to flight/targets/boards/revoproto/firmware/revolution.cpp index b1081c4b9..268325b26 100644 --- a/flight/targets/boards/revoproto/firmware/revolution.c +++ b/flight/targets/boards/revoproto/firmware/revolution.cpp @@ -31,6 +31,7 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +extern "C" { #include "inc/openpilot.h" #include @@ -73,6 +74,7 @@ static void initTask(void *parameters); /* Prototype of generated InitModules() function */ extern void InitModules(void); +} /** * OpenPilot Main function: diff --git a/ground/openpilotgcs/src/plugins/config/stabilization.ui b/ground/openpilotgcs/src/plugins/config/stabilization.ui index a2a157089..aa5b3b553 100644 --- a/ground/openpilotgcs/src/plugins/config/stabilization.ui +++ b/ground/openpilotgcs/src/plugins/config/stabilization.ui @@ -7,7 +7,7 @@ 0 0 974 - 857 + 755 @@ -136,8 +136,8 @@ 0 0 - 939 - 776 + 950 + 775 @@ -8244,8 +8244,8 @@ border-radius: 5; 0 0 - 952 - 763 + 950 + 736 @@ -18236,8 +18236,8 @@ border-radius: 5; 0 0 - 952 - 763 + 950 + 671 @@ -24082,8 +24082,8 @@ font:bold; 0 0 - 952 - 763 + 950 + 671 @@ -25209,8 +25209,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:AltitudePI - element:Kp + fieldname:VerticalPosP scale:0.01 haslimits:yes buttongroup:98,10 @@ -25294,15 +25293,18 @@ border-radius: 5; - + - <html><head/><body><p>How fast the vehicle should adjust its neutral throttle estimation. Altitude assumes that when engaged the throttle is in the range required to hover. If the throttle is a lot higher or lower, it needs to adjust this &quot;throttle trim&quot; Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.</p></body></html> + <html><head/><body><p>How fast the vehicle should attain its target velocity. For neutral throttle estimation, the altitude module assumes that when engaged the throttle thrust limit neutral setting is in the range required to hover. If the throttle required is a lot higher or lower, it needs to adjust this &quot;throttle trim&quot;. Higher values make it do this adjustment faster, but this could lead to ugly oscillations. Leave at default unless you know what you are doing.</p></body></html> - 1000 + 100 + + + 1 - 50 + 25 Qt::Horizontal @@ -25313,17 +25315,17 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:VelocityPI + fieldname:VerticalVelPID element:Ki - scale:0.00001 + scale:0.01 haslimits:yes buttongroup:98 - - + + 0 @@ -25355,13 +25357,13 @@ border-radius: 5; 100 - 51 + 25 objname:AltitudeHoldSettings - fieldname:VelocityPI - element:Kp + fieldname:VerticalVelPID + element:Ki scale:0.01 haslimits:yes buttongroup:98,10 @@ -25369,8 +25371,68 @@ border-radius: 5; - - + + + + + 0 + 0 + + + + + 58 + 0 + + + + + 16777215 + 16777215 + + + + + + + Velocity Derivative + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <html><head/><body><p>Small abouts of Kd can reduce oscillations in the velocity controller.</p></body></html> + + + 300 + + + 50 + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Kd + scale:0.0001 + haslimits:yes + buttongroup:98 + + + + + + 0 @@ -25399,7 +25461,7 @@ border-radius: 5; Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - 1000 + 300 51 @@ -25407,9 +25469,172 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:VelocityPI - element:Ki - scale:0.00001 + fieldname:VerticalVelPID + element:Kd + scale:0.0001 + haslimits:yes + buttongroup:98,10 + + + + + + + + + 0 + 0 + + + + + 58 + 0 + + + + + 16777215 + 16777215 + + + + + + + Velocity Beta + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + <html><head/><body><p>The beta value applies a setpoint weighting that reduces the sensitivity to quick changes in the desired velocity. Transitions from altitude hold to descent/climb can be made smooth applying a Beta value of around 80 to 90%.</p></body></html> + + + 70 + + + 100 + + + 90 + + + 90 + + + Qt::Horizontal + + + QSlider::TicksBelow + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Beta + scale:0.01 + haslimits:yes + buttongroup:98 + + + + + + + + + 0 + 0 + + + + + 50 + 22 + + + + + 50 + 22 + + + + Qt::StrongFocus + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 70 + + + 100 + + + 90 + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Beta + scale:0.01 + haslimits:yes + buttongroup:98,10 + + + + + + + + + 0 + 0 + + + + + 50 + 22 + + + + + 50 + 22 + + + + Qt::StrongFocus + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 100 + + + 50 + + + + objname:AltitudeHoldSettings + fieldname:VerticalVelPID + element:Kp + scale:0.01 haslimits:yes buttongroup:98,10 @@ -25419,7 +25644,7 @@ border-radius: 5; - <html><head/><body><p>How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.</p></body></html> + <html><head/><body><p>How fast the vehicle should climb or descent to compensate a certain altitude difference. Higher values could result in more accurate altitude hold but also more violent control actions, lower values are safer and ensure smoother flight. The default value should be fine for the majority of crafts.</p></body></html> 100 @@ -25434,10 +25659,9 @@ border-radius: 5; QSlider::TicksBelow - + objname:AltitudeHoldSettings - fieldname:AltitudePI - element:Kp + fieldname:VerticalPosP scale:0.01 haslimits:yes buttongroup:98 @@ -25446,7 +25670,7 @@ border-radius: 5; - + <html><head/><body><p>How much the vehicle should throttle up or down to compensate or achieve a certain vertical speed. Higher values lead to more aggressive throttle changes and could lead to oscillations. This is the most likely candidate to change depending on the crafts engine thrust. Heavy craft with weak engines might require higher values.</p></body></html> @@ -25465,7 +25689,7 @@ border-radius: 5; objname:AltitudeHoldSettings - fieldname:VelocityPI + fieldname:VerticalVelPID element:Kp scale:0.01 haslimits:yes @@ -27173,10 +27397,10 @@ Useful if you have accidentally changed some settings. pushButton_7 AltKpSlider AltKp - AltKiSlider - AltKi - AltKdSlider - AltKd + VelKpSlider + VelKp + VelKiSlider + VelKi pushButton_8 AltThrExpSlider_2 AltThrExp_2 diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 744827d2b..d197221a8 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -33,6 +33,10 @@ FLIGHTLIBINC = $(FLIGHTLIB)/inc OPUAVOBJINC = $(OPUAVOBJ)/inc OPUAVTALKINC = $(OPUAVTALK)/inc +## PID +PIDLIB =$(FLIGHTLIB)/pid +PIDLIBINC =$(FLIGHTLIB)/pid + ## Math MATHLIB = $(FLIGHTLIB)/math MATHLIBINC = $(FLIGHTLIB)/math @@ -88,7 +92,10 @@ SRC += $(PIOSCOMMON)/pios_sensors.c SRC += $(FLIGHTLIB)/sanitycheck.c SRC += $(FLIGHTLIB)/CoordinateConversions.c SRC += $(MATHLIB)/sin_lookup.c + +## PID library functions SRC += $(MATHLIB)/pid.c +CPPSRC += $(PIDLIB)/pidcontroldown.cpp ## PIOS Hardware (Common) SRC += $(PIOSCOMMON)/pios_flashfs_logfs.c @@ -167,6 +174,7 @@ EXTRAINCDIRS += $(FLIGHTLIBINC) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(OPSYSTEMINC) EXTRAINCDIRS += $(MATHLIBINC) +EXTRAINCDIRS += $(PIDLIBINC) EXTRAINCDIRS += $(OPUAVOBJINC) EXTRAINCDIRS += $(OPUAVTALKINC) EXTRAINCDIRS += $(OPUAVSYNTHDIR) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index aadf90e8f..e85ff09e6 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -1,14 +1,15 @@ - Settings for the @ref AltitudeHold module - - + Settings for the @ref AltitudeHold module - - - - - - + + + + + + + + + diff --git a/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml index f298a45ab..0918785c5 100644 --- a/shared/uavobjectdefinition/altitudeholdstatus.xml +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -2,6 +2,8 @@ Status Data from Altitude Hold Control Loops + + diff --git a/shared/uavobjectdefinition/txpidsettings.xml b/shared/uavobjectdefinition/txpidsettings.xml index 28035452b..6d59ae5a9 100644 --- a/shared/uavobjectdefinition/txpidsettings.xml +++ b/shared/uavobjectdefinition/txpidsettings.xml @@ -20,7 +20,7 @@ Roll+Pitch Attitude.Kp, Roll+Pitch Attitude.Ki, Roll+Pitch Attitude.ILimit, Roll+Pitch Attitude.Resp, Yaw Attitude.Kp, Yaw Attitude.Ki, Yaw Attitude.ILimit, Yaw Attitude.Resp, Roll.Expo, Pitch.Expo, Roll+Pitch.Expo, Yaw.Expo, - GyroTau,AcroPlusFactor" + GyroTau,AcroPlusFactor,Altitude Pos.Kp,Altitude Velocity.Kp,Altitude Velocity.Ki,Altitude Velocity.Kd,Altitude Velocity.Beta" defaultvalue="Disabled"/> diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index c98ad4254..4f6758035 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -7,8 +7,8 @@ - - + + @@ -21,10 +21,10 @@ - + - - + +