diff --git a/flight/libraries/pid/pidcontroldown.cpp b/flight/libraries/pid/pidcontroldown.cpp index 04ae1cddf..7f06eac93 100644 --- a/flight/libraries/pid/pidcontroldown.cpp +++ b/flight/libraries/pid/pidcontroldown.cpp @@ -59,7 +59,7 @@ extern "C" { PIDControlDown::PIDControlDown() : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f), mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f), - mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false) + mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true) { Deactivate(); } diff --git a/flight/libraries/pid/pidcontroldown.h b/flight/libraries/pid/pidcontroldown.h index b7c215430..854daa955 100644 --- a/flight/libraries/pid/pidcontroldown.h +++ b/flight/libraries/pid/pidcontroldown.h @@ -58,6 +58,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(); @@ -77,7 +85,6 @@ private: float mPositionState; float mMinThrust; float mMaxThrust; - uint8_t mActive; struct NeutralThrustEstimation { uint32_t count; @@ -90,6 +97,8 @@ private: bool have_correction; }; struct NeutralThrustEstimation neutralThrustEst; + bool mActive; + bool mAllowNeutralThrustCalc; }; #endif // PIDCONTROLDOWN_H diff --git a/flight/modules/PathFollower/vtolflycontroller.cpp b/flight/modules/PathFollower/vtolflycontroller.cpp index 0160d0e79..20a387277 100644 --- a/flight/modules/PathFollower/vtolflycontroller.cpp +++ b/flight/modules/PathFollower/vtolflycontroller.cpp @@ -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/Stabilization/altitudeloop.cpp b/flight/modules/Stabilization/altitudeloop.cpp index 4c406f868..2cf141c82 100644 --- a/flight/modules/Stabilization/altitudeloop.cpp +++ b/flight/modules/Stabilization/altitudeloop.cpp @@ -141,17 +141,20 @@ static void altitudeHoldTask(void) controlDown.UpdatePositionState(positionStateDown); controlDown.ControlPosition(); altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEHOLD; thrustDemand = controlDown.GetDownCommand(); } break; case ALTITUDEVARIO: altitudeHoldStatus.VelocityDesired = controlDown.GetVelocityDesired(); + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_ALTITUDEVARIO; thrustDemand = controlDown.GetDownCommand(); break; case DIRECT: altitudeHoldStatus.VelocityDesired = 0.0f; + altitudeHoldStatus.State = ALTITUDEHOLDSTATUS_STATE_DIRECT; break; } @@ -202,6 +205,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) // 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/shared/uavobjectdefinition/altitudeholdstatus.xml b/shared/uavobjectdefinition/altitudeholdstatus.xml index f298a45ab..293c5df34 100644 --- a/shared/uavobjectdefinition/altitudeholdstatus.xml +++ b/shared/uavobjectdefinition/altitudeholdstatus.xml @@ -2,6 +2,7 @@ Status Data from Altitude Hold Control Loops +