From 7d66a075dec741b3a1395991e6709e309d5a3622 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:19:01 +1000 Subject: [PATCH] REVONANO Fixes to AutoTakeoff 1. Fix autoyaw to 0 attitude on takeoff post launch. 2. An error condition abort now properly disarms and continues to contrain thrust and stabi during disarmed state. 3. Increase thrustdown time from 2 to 5 seconds on an error condition (e.g. if 3 m deviation from takeoff position on NE). --- .../inc/vtolautotakeoffcontroller.h | 3 +- .../PathFollower/inc/vtolautotakeofffsm.h | 5 -- .../vtolautotakeoffcontroller.cpp | 40 +++--------- .../PathFollower/vtolautotakeofffsm.cpp | 61 ++++--------------- 4 files changed, 24 insertions(+), 85 deletions(-) diff --git a/flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h b/flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h index 64ec94447..4b52bfa9c 100644 --- a/flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h +++ b/flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h @@ -63,8 +63,7 @@ public: private: void UpdateVelocityDesired(void); - int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); - void setArmedIfChanged(FlightStatusArmedOptions val); + int8_t UpdateStabilizationDesired(void); VtolAutoTakeoffFSM *fsm; VtolPathFollowerSettingsData *vtolPathFollowerSettings; diff --git a/flight/modules/PathFollower/inc/vtolautotakeofffsm.h b/flight/modules/PathFollower/inc/vtolautotakeofffsm.h index fd6832bd7..6c912baec 100644 --- a/flight/modules/PathFollower/inc/vtolautotakeofffsm.h +++ b/flight/modules/PathFollower/inc/vtolautotakeofffsm.h @@ -73,7 +73,6 @@ public: void BoundThrust(float &ulow, float &uhigh); PathFollowerFSMState_T GetCurrentState(void); void ConstrainStabiDesired(StabilizationDesiredData *stabDesired); - void Abort(void); uint8_t PositionHoldState(void); void setControlState(StatusVtolAutoTakeoffControlStateOptions controlState); @@ -138,9 +137,6 @@ protected: void setup_disarmed(void); void run_disarmed(uint8_t); - void setup_abort(void); - void run_abort(uint8_t); - void initFSM(void); void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason); int32_t runState(); @@ -150,7 +146,6 @@ protected: void assessAltitude(void); void setStateTimeout(int32_t count); - void fallback_to_hold(void); static PathFollowerFSM_AutoTakeoffStateHandler_T sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE]; }; diff --git a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp index 2c4bdc569..afac9e7e5 100644 --- a/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp +++ b/flight/modules/PathFollower/vtolautotakeoffcontroller.cpp @@ -30,6 +30,7 @@ extern "C" { #include #include +#include #include #include #include @@ -208,7 +209,7 @@ void VtolAutoTakeoffController::UpdateVelocityDesired() VelocityDesiredSet(&velocityDesired); } -int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction) +int8_t VtolAutoTakeoffController::UpdateStabilizationDesired() { uint8_t result = 1; StabilizationDesiredData stabDesired; @@ -236,13 +237,8 @@ int8_t VtolAutoTakeoffController::UpdateStabilizationDesired(bool yaw_attitude, ManualControlCommandData manualControl; ManualControlCommandGet(&manualControl); - if (yaw_attitude) { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; - stabDesired.Yaw = yaw_direction; - } else { - stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; - } + stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; // default thrust mode to cruise control stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; @@ -259,29 +255,13 @@ void VtolAutoTakeoffController::UpdateAutoPilot() UpdateVelocityDesired(); - // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm - bool yaw_attitude = true; - float yaw = 0.0f; - - fsm->GetYaw(yaw_attitude, yaw); - - int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw); - - if (!result) { - fsm->Abort(); - } - - if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) { - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + int8_t result = UpdateStabilizationDesired(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(pathStatus); } - -void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val) -{ - if (flightStatus->Armed != val) { - flightStatus->Armed = val; - FlightStatusSet(flightStatus); - } -} diff --git a/flight/modules/PathFollower/vtolautotakeofffsm.cpp b/flight/modules/PathFollower/vtolautotakeofffsm.cpp index 4fa320f43..524686d1f 100644 --- a/flight/modules/PathFollower/vtolautotakeofffsm.cpp +++ b/flight/modules/PathFollower/vtolautotakeofffsm.cpp @@ -65,7 +65,7 @@ extern "C" { #define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod) #define TIMEOUT_SLOWSTART (2 * TIMER_COUNT_PER_SECOND) #define TIMEOUT_THRUSTUP (1 * TIMER_COUNT_PER_SECOND) -#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND) +#define TIMEOUT_THRUSTDOWN (5 * TIMER_COUNT_PER_SECOND) #define AUTOTAKEOFFING_SLOWDOWN_HEIGHT -5.0f VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM::sAutoTakeoffStateTable[AUTOTAKEOFF_STATE_SIZE] = { @@ -77,8 +77,7 @@ VtolAutoTakeoffFSM::PathFollowerFSM_AutoTakeoffStateHandler_T VtolAutoTakeoffFSM [AUTOTAKEOFF_STATE_HOLD] = { .setup = &VtolAutoTakeoffFSM::setup_hold, .run = &VtolAutoTakeoffFSM::run_hold }, [AUTOTAKEOFF_STATE_THRUSTDOWN] = { .setup = &VtolAutoTakeoffFSM::setup_thrustdown, .run = &VtolAutoTakeoffFSM::run_thrustdown }, [AUTOTAKEOFF_STATE_THRUSTOFF] = { .setup = &VtolAutoTakeoffFSM::setup_thrustoff, .run = &VtolAutoTakeoffFSM::run_thrustoff }, - [AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed }, - [AUTOTAKEOFF_STATE_ABORT] = { .setup = &VtolAutoTakeoffFSM::setup_abort, .run = &VtolAutoTakeoffFSM::run_abort } + [AUTOTAKEOFF_STATE_DISARMED] = { .setup = &VtolAutoTakeoffFSM::setup_disarmed, .run = &VtolAutoTakeoffFSM::run_disarmed } }; // pointer to a singleton instance @@ -161,21 +160,12 @@ void VtolAutoTakeoffFSM::Activate() setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); } -void VtolAutoTakeoffFSM::Abort(void) -{ - setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); -} - PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void) { switch (mAutoTakeoffData->currentState) { case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE: return PFFSM_STATE_INACTIVE; - break; - case STATUSVTOLAUTOTAKEOFF_STATE_ABORT: - return PFFSM_STATE_ABORT; - break; case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED: return PFFSM_STATE_DISARMED; @@ -339,7 +329,7 @@ void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOption setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); break; case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT: - setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); + setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); break; } } @@ -540,51 +530,26 @@ void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout // STATE: DISARMED void VtolAutoTakeoffFSM::setup_disarmed(void) { - // nothing to do - mAutoTakeoffData->flConstrainThrust = false; - mAutoTakeoffData->flZeroStabiHorizontal = false; + mAutoTakeoffData->thrustLimit = -1.0f; + mAutoTakeoffData->flConstrainThrust = true; + mAutoTakeoffData->flZeroStabiHorizontal = true; mAutoTakeoffData->observationCount = 0; mAutoTakeoffData->boundThrustMin = -0.1f; mAutoTakeoffData->boundThrustMax = 0.0f; + + if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + } } void VtolAutoTakeoffFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout) { + if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + } #ifdef DEBUG_GROUNDIMPACT if (mAutoTakeoffData->observationCount++ > 100) { setState(AUTOTAKEOFF_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); } #endif } - -void VtolAutoTakeoffFSM::fallback_to_hold(void) -{ - PositionStateData positionState; - - PositionStateGet(&positionState); - pathDesired->End.North = positionState.North; - pathDesired->End.East = positionState.East; - pathDesired->End.Down = positionState.Down; - pathDesired->Start.North = positionState.North; - pathDesired->Start.East = positionState.East; - pathDesired->Start.Down = positionState.Down; - pathDesired->StartingVelocity = 0.0f; - pathDesired->EndingVelocity = 0.0f; - pathDesired->Mode = PATHDESIRED_MODE_GOTOENDPOINT; - - PathDesiredSet(pathDesired); -} - -// abort repeatedly overwrites pathfollower's objective on a landing abort and -// continues to do so until a flight mode change. -void VtolAutoTakeoffFSM::setup_abort(void) -{ - mAutoTakeoffData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; - mAutoTakeoffData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; - mAutoTakeoffData->flConstrainThrust = false; - mAutoTakeoffData->flZeroStabiHorizontal = false; - fallback_to_hold(); -} - -void VtolAutoTakeoffFSM::run_abort(__attribute__((unused)) uint8_t flTimeout) -{}