From 89b40f31731433c2dbed99fe32dbfb10a2dac194 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 08:50:33 +1000 Subject: [PATCH 1/9] REVONANO: Landing and disarming fixes Also prevent stabi control in land's disarm state --- flight/modules/ManualControl/armhandler.c | 12 ------------ flight/modules/PathFollower/vtollandfsm.cpp | 7 ++++++- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index f83b4644e..fe80141a6 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -346,18 +346,6 @@ static bool forcedDisArm(void) return true; } -#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES - // check landing state if active - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) { - StatusVtolLandData statusland; - StatusVtolLandGet(&statusland); - if (statusland.State == STATUSVTOLLAND_STATE_DISARMED) { - return true; - } - } -#endif return false; } diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp index f51b76fe0..f9d7a5064 100644 --- a/flight/modules/PathFollower/vtollandfsm.cpp +++ b/flight/modules/PathFollower/vtollandfsm.cpp @@ -31,6 +31,7 @@ extern "C" { #include #include +#include #include #include #include @@ -638,6 +639,7 @@ void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout) void VtolLandFSM::setup_thrustoff(void) { mLandData->thrustLimit = -1.0f; + mLandData->flZeroStabiHorizontal = true; mLandData->flConstrainThrust = true; mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMax = 0.0f; @@ -653,10 +655,13 @@ void VtolLandFSM::setup_disarmed(void) { // nothing to do mLandData->flConstrainThrust = false; - mLandData->flZeroStabiHorizontal = false; + mLandData->flZeroStabiHorizontal = true; mLandData->observationCount = 0; mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMax = 0.0f; + if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + } } void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout) From 50d60649aa3e584aeceb5f35280572caa7dcf1ba Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:13:13 +1000 Subject: [PATCH 2/9] REVONANO Fix BrakeController to use vtol VerticalVelPID instead of LandVerticalVelPID --- flight/modules/PathFollower/inc/vtolbrakefsm.h | 2 -- .../modules/PathFollower/vtolbrakecontroller.cpp | 16 ++++++++++------ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/flight/modules/PathFollower/inc/vtolbrakefsm.h b/flight/modules/PathFollower/inc/vtolbrakefsm.h index 6e5151977..ac10faa0d 100644 --- a/flight/modules/PathFollower/inc/vtolbrakefsm.h +++ b/flight/modules/PathFollower/inc/vtolbrakefsm.h @@ -66,7 +66,6 @@ public: void Activate(void); void Update(void); PathFollowerFSMState_T GetCurrentState(void); - void Abort(void); uint8_t PositionHoldState(void); protected: @@ -103,7 +102,6 @@ protected: // void updateVtolBrakeFSMStatus(); void setStateTimeout(int32_t count); - void fallback_to_hold(void); static PathFollowerFSM_BrakeStateHandler_T sBrakeStateTable[BRAKE_STATE_SIZE]; }; diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp index 271ee907c..960e330de 100644 --- a/flight/modules/PathFollower/vtolbrakecontroller.cpp +++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp @@ -35,6 +35,7 @@ extern "C" { #include #include +#include #include #include #include @@ -138,10 +139,10 @@ void VtolBrakeController::SettingsUpdated(void) controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeMaxPitch, vtolPathFollowerSettings->BrakeVelocityFeedforward); - controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp, - vtolPathFollowerSettings->LandVerticalVelPID.Ki, - vtolPathFollowerSettings->LandVerticalVelPID.Kd, - vtolPathFollowerSettings->LandVerticalVelPID.Beta, + controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp, + vtolPathFollowerSettings->VerticalVelPID.Ki, + vtolPathFollowerSettings->VerticalVelPID.Kd, + vtolPathFollowerSettings->VerticalVelPID.Beta, dT, 10.0f * vtolPathFollowerSettings->VerticalVelMax); // avoid constraining initial fast entry into brake controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP); @@ -364,8 +365,11 @@ void VtolBrakeController::UpdateAutoPilot() int8_t result = UpdateStabilizationDesired(); - if (!result) { - fsm->Abort(); // enter PH + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(pathStatus); From 7d66a075dec741b3a1395991e6709e309d5a3622 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:19:01 +1000 Subject: [PATCH 3/9] 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) -{} From c5715b26af4d7c793b58f1d60110f52c2138186c Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:21:53 +1000 Subject: [PATCH 4/9] REVONANO Landing fixes 1. Forced disarm via guidance alarm critical on disarm state 2. Continue to zero stabi including yaw during disarm state just in case it doesn't actually disarm. --- flight/modules/PathFollower/inc/vtollandfsm.h | 5 -- flight/modules/PathFollower/vtollandfsm.cpp | 66 +++++-------------- 2 files changed, 16 insertions(+), 55 deletions(-) diff --git a/flight/modules/PathFollower/inc/vtollandfsm.h b/flight/modules/PathFollower/inc/vtollandfsm.h index dec143c41..4c582a00a 100644 --- a/flight/modules/PathFollower/inc/vtollandfsm.h +++ b/flight/modules/PathFollower/inc/vtollandfsm.h @@ -47,7 +47,6 @@ typedef enum { LAND_STATE_THRUSTDOWN, // Thrust down sequence LAND_STATE_THRUSTOFF, // Thrust is now off LAND_STATE_DISARMED, // Disarmed - LAND_STATE_ABORT, // Abort on error triggerig fallback to hold LAND_STATE_SIZE } PathFollowerFSM_LandState_T; @@ -75,7 +74,6 @@ public: void ConstrainStabiDesired(StabilizationDesiredData *stabDesired); float BoundVelocityDown(float); void CheckPidScaler(pid_scaler *scaler); - void Abort(void); protected: @@ -130,8 +128,6 @@ protected: void run_thrustoff(uint8_t); void setup_disarmed(void); void run_disarmed(uint8_t); - void setup_abort(void); - void run_abort(uint8_t); void initFSM(void); void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason); int32_t runState(); @@ -140,7 +136,6 @@ protected: void assessAltitude(void); void setStateTimeout(int32_t count); - void fallback_to_hold(void); static PathFollowerFSM_LandStateHandler_T sLandStateTable[LAND_STATE_SIZE]; }; diff --git a/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp index f9d7a5064..267299bc6 100644 --- a/flight/modules/PathFollower/vtollandfsm.cpp +++ b/flight/modules/PathFollower/vtollandfsm.cpp @@ -97,8 +97,7 @@ VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAN [LAND_STATE_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_groundeffect, .run = &VtolLandFSM::run_groundeffect }, [LAND_STATE_THRUSTDOWN] = { .setup = &VtolLandFSM::setup_thrustdown, .run = &VtolLandFSM::run_thrustdown }, [LAND_STATE_THRUSTOFF] = { .setup = &VtolLandFSM::setup_thrustoff, .run = &VtolLandFSM::run_thrustoff }, - [LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed }, - [LAND_STATE_ABORT] = { .setup = &VtolLandFSM::setup_abort, .run = &VtolLandFSM::run_abort } + [LAND_STATE_DISARMED] = { .setup = &VtolLandFSM::setup_disarmed, .run = &VtolLandFSM::run_disarmed } }; // pointer to a singleton instance @@ -177,25 +176,16 @@ void VtolLandFSM::Activate() #endif } else { // move to error state and callback to position hold - setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); + setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE); } } -void VtolLandFSM::Abort(void) -{ - setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE); -} - PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void) { switch (mLandData->currentState) { case STATUSVTOLLAND_STATE_INACTIVE: return PFFSM_STATE_INACTIVE; - break; - case STATUSVTOLLAND_STATE_ABORT: - return PFFSM_STATE_ABORT; - break; case STATUSVTOLLAND_STATE_DISARMED: return PFFSM_STATE_DISARMED; @@ -432,7 +422,7 @@ void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout) } if (flTimeout) { - setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); + setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT); } } @@ -638,11 +628,11 @@ void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout) // STATE: THRUSTOFF void VtolLandFSM::setup_thrustoff(void) { - mLandData->thrustLimit = -1.0f; + mLandData->thrustLimit = -1.0f; mLandData->flZeroStabiHorizontal = true; - mLandData->flConstrainThrust = true; - mLandData->boundThrustMin = -0.1f; - mLandData->boundThrustMax = 0.0f; + mLandData->flConstrainThrust = true; + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = 0.0f; } void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout) @@ -659,48 +649,24 @@ void VtolLandFSM::setup_disarmed(void) mLandData->observationCount = 0; mLandData->boundThrustMin = -0.1f; mLandData->boundThrustMax = 0.0f; + + // force disarm unless in pathplanner mode + // to clear, a new pathfollower mode must be selected that is not land, + // and also a non-pathfollower mode selection will set this to uninitialised. if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { - AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL); } } void VtolLandFSM::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 (mLandData->observationCount++ > 100) { setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE); } #endif } - -void VtolLandFSM::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 VtolLandFSM::setup_abort(void) -{ - mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min; - mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max; - mLandData->flConstrainThrust = false; - mLandData->flZeroStabiHorizontal = false; - fallback_to_hold(); -} - -void VtolLandFSM::run_abort(__attribute__((unused)) uint8_t flTimeout) -{} From cdf1b88cc05269148215d8ca9722cfbf70c5f8bc Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:24:06 +1000 Subject: [PATCH 5/9] REVONANO VelocityRoam fixes 1. New VelocityRoamHorizontalVelPID to allow tuning of VR independent of pathfollower nav modes --- .../PathFollower/vtolvelocitycontroller.cpp | 35 ++++++------------- .../vtolpathfollowersettings.xml | 3 +- 2 files changed, 12 insertions(+), 26 deletions(-) diff --git a/flight/modules/PathFollower/vtolvelocitycontroller.cpp b/flight/modules/PathFollower/vtolvelocitycontroller.cpp index 30cd02107..3934335ce 100644 --- a/flight/modules/PathFollower/vtolvelocitycontroller.cpp +++ b/flight/modules/PathFollower/vtolvelocitycontroller.cpp @@ -30,6 +30,7 @@ extern "C" { #include #include +#include #include #include #include @@ -104,10 +105,10 @@ void VtolVelocityController::SettingsUpdated(void) { const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f; - controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp, - vtolPathFollowerSettings->HorizontalVelPID.Ki, - vtolPathFollowerSettings->HorizontalVelPID.Kd, - vtolPathFollowerSettings->HorizontalVelPID.Beta, + controlNE.UpdateParameters(vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Kp, + vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Ki, + vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Kd, + vtolPathFollowerSettings->VelocityRoamHorizontalVelPID.Beta, dT, vtolPathFollowerSettings->HorizontalVelMax); @@ -201,28 +202,12 @@ void VtolVelocityController::UpdateAutoPilot() float yaw = 0.0f; int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw); - - if (!result) { - fallback_to_hold(); + 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 VtolVelocityController::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); -} diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 5445f1ed0..ebd9b9655 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -18,7 +18,6 @@ - @@ -26,6 +25,8 @@ + + From bddb9377a4283ff818704442d150b531a80c0dc5 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:25:07 +1000 Subject: [PATCH 6/9] REVONANO Land and Takeoff uav changes removing abort state. --- flight/modules/PathFollower/vtollandcontroller.cpp | 9 ++++++--- shared/uavobjectdefinition/statusvtolautotakeoff.xml | 2 +- shared/uavobjectdefinition/statusvtolland.xml | 2 +- 3 files changed, 8 insertions(+), 5 deletions(-) diff --git a/flight/modules/PathFollower/vtollandcontroller.cpp b/flight/modules/PathFollower/vtollandcontroller.cpp index 97a7d466f..c2546ed37 100644 --- a/flight/modules/PathFollower/vtollandcontroller.cpp +++ b/flight/modules/PathFollower/vtollandcontroller.cpp @@ -30,6 +30,7 @@ extern "C" { #include #include +#include #include #include #include @@ -266,9 +267,11 @@ void VtolLandController::UpdateAutoPilot() fsm->GetYaw(yaw_attitude, yaw); int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw); - - if (!result) { - fsm->Abort(); + if (result) { + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); + } else { + pathStatus->Status = PATHSTATUS_STATUS_CRITICAL; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING); } PathStatusSet(pathStatus); diff --git a/shared/uavobjectdefinition/statusvtolautotakeoff.xml b/shared/uavobjectdefinition/statusvtolautotakeoff.xml index 0d0d92a85..e36d9a902 100644 --- a/shared/uavobjectdefinition/statusvtolautotakeoff.xml +++ b/shared/uavobjectdefinition/statusvtolautotakeoff.xml @@ -1,7 +1,7 @@ Status of a AutoTakeoff autopilot - + diff --git a/shared/uavobjectdefinition/statusvtolland.xml b/shared/uavobjectdefinition/statusvtolland.xml index 36b04a600..74b71a18f 100644 --- a/shared/uavobjectdefinition/statusvtolland.xml +++ b/shared/uavobjectdefinition/statusvtolland.xml @@ -2,7 +2,7 @@ Status of a Vtol landing sequence + GroundEffect,ThrustDown,ThrustOff,Disarmed" default="0"/> From 4a78602da2d2835bdfb185515c3e9c4ee18c59d2 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Mon, 25 May 2015 22:26:06 +1000 Subject: [PATCH 7/9] REVONANO Guidance alarm clearing on change of mode --- flight/modules/PathFollower/inc/vtolvelocitycontroller.h | 1 - flight/modules/PathFollower/pathfollower.cpp | 5 +++++ flight/modules/PathFollower/vtolbrakefsm.cpp | 5 ----- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/flight/modules/PathFollower/inc/vtolvelocitycontroller.h b/flight/modules/PathFollower/inc/vtolvelocitycontroller.h index c5209a3d1..8d1108087 100644 --- a/flight/modules/PathFollower/inc/vtolvelocitycontroller.h +++ b/flight/modules/PathFollower/inc/vtolvelocitycontroller.h @@ -62,7 +62,6 @@ public: private: void UpdateVelocityDesired(void); int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); - void fallback_to_hold(void); VtolPathFollowerSettingsData *vtolPathFollowerSettings; PIDControlNE controlNE; diff --git a/flight/modules/PathFollower/pathfollower.cpp b/flight/modules/PathFollower/pathfollower.cpp index 4c688e7e8..f96328965 100644 --- a/flight/modules/PathFollower/pathfollower.cpp +++ b/flight/modules/PathFollower/pathfollower.cpp @@ -242,6 +242,7 @@ static void pathFollowerSetActiveController(void) if (activeController == 0) { // Initialise pathFollowerInitializeControllersForFrameType(); + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK); switch (frameType) { case FRAME_TYPE_MULTIROTOR: @@ -273,6 +274,7 @@ static void pathFollowerSetActiveController(void) break; default: activeController = 0; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); break; } break; @@ -289,6 +291,7 @@ static void pathFollowerSetActiveController(void) break; default: activeController = 0; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); break; } break; @@ -305,12 +308,14 @@ static void pathFollowerSetActiveController(void) break; default: activeController = 0; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); break; } break; default: activeController = 0; + AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED); break; } } diff --git a/flight/modules/PathFollower/vtolbrakefsm.cpp b/flight/modules/PathFollower/vtolbrakefsm.cpp index 6401e0a2f..addf95806 100644 --- a/flight/modules/PathFollower/vtolbrakefsm.cpp +++ b/flight/modules/PathFollower/vtolbrakefsm.cpp @@ -130,11 +130,6 @@ void VtolBrakeFSM::Activate() setState(BRAKE_STATE_BRAKE, FSMBRAKESTATUS_STATEEXITREASON_NONE); } -void VtolBrakeFSM::Abort(void) -{ - setState(BRAKE_STATE_HOLD, FSMBRAKESTATUS_STATEEXITREASON_NONE); -} - PathFollowerFSMState_T VtolBrakeFSM::GetCurrentState(void) { switch (mBrakeData->currentState) { From 85510483541f2c6a3b2648bdc1678829779e6e47 Mon Sep 17 00:00:00 2001 From: samguns Date: Tue, 26 May 2015 15:01:01 +0800 Subject: [PATCH 8/9] REVONANO 1. Sanity check for PWMSync & OneShot while using PWM receiver. 2. Remove CC in hwsettings.xml --- .../boards/revonano/firmware/pios_board.c | 51 ++++++++++++++++++- .../src/plugins/config/configoutputwidget.cpp | 5 +- .../src/plugins/setupwizard/pages/escpage.cpp | 31 ++++++++++- shared/uavobjectdefinition/hwsettings.xml | 8 +-- 4 files changed, 82 insertions(+), 13 deletions(-) diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 41a5da831..dcf373a51 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -36,7 +36,8 @@ #include #include #include - +#include +#include #ifdef PIOS_INCLUDE_INSTRUMENTATION #include @@ -52,6 +53,8 @@ */ #include "../board_hw_defs.c" +static SystemAlarmsExtendedAlarmStatusOptions RevoNanoConfigHook(); +static void ActuatorSettingsUpdatedCb(UAVObjEvent *ev); /** * Sensor configurations */ @@ -764,8 +767,54 @@ void PIOS_Board_Init(void) } } #endif + + // Attach the board config check hook + SANITYCHECK_AttachHook(&RevoNanoConfigHook); + // trigger a config check if actuatorsettings are updated + ActuatorSettingsInitialize(); + ActuatorSettingsConnectCallback(ActuatorSettingsUpdatedCb); } +SystemAlarmsExtendedAlarmStatusOptions RevoNanoConfigHook() +{ + // inhibit usage of oneshot for non supported RECEIVER port modes + uint8_t recmode; + + HwSettingsRM_RcvrPortGet(&recmode); + uint8_t modes[ACTUATORSETTINGS_BANKMODE_NUMELEM]; + ActuatorSettingsBankModeGet(modes); + + switch ((HwSettingsRM_RcvrPortOptions)recmode) { + // Those modes allows oneshot usage + case HWSETTINGS_RM_RCVRPORT_DISABLED: + case HWSETTINGS_RM_RCVRPORT_PPM: + case HWSETTINGS_RM_RCVRPORT_PPMOUTPUTS: + case HWSETTINGS_RM_RCVRPORT_OUTPUTS: + return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; + + // inhibit oneshot for the following modes + case HWSETTINGS_RM_RCVRPORT_PWM: + for (uint8_t i = 0; i < ACTUATORSETTINGS_BANKMODE_NUMELEM; i++) { + if (modes[i] == ACTUATORSETTINGS_BANKMODE_PWMSYNC || + modes[i] == ACTUATORSETTINGS_BANKMODE_ONESHOT125) { + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; + } + } + + return SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE; + + default: + break; + } + + return SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT;; +} + +// trigger a configuration check if ActuatorSettings are changed. +void ActuatorSettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + configuration_check(); +} /** * @} * @} diff --git a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp index 6f149dfba..2ba04e8b3 100644 --- a/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp @@ -472,10 +472,7 @@ void ConfigOutputWidget::updateWarnings(UAVObject *) if (systemAlarms.Alarm[SystemAlarms::ALARM_SYSTEMCONFIGURATION] > SystemAlarms::ALARM_WARNING) { switch (systemAlarms.ExtendedAlarmStatus[SystemAlarms::EXTENDEDALARMSTATUS_SYSTEMCONFIGURATION]) { case SystemAlarms::EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT: - setWarning(tr("OneShot and PWMSync output only works with Receiver Port settings marked with '+OneShot'
" - "When using Receiver Port setting 'PPM_PIN8+OneShot' " - "Bank %2 must be set to PWM") - .arg(m_banks.at(3).color().name()).arg(m_banks.at(3).label()->text())); + setWarning(tr("OneShot and PWMSync output DO NOT work with Receiver Port is 'PWM'
")); return; } } diff --git a/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp b/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp index d56a2ec1b..f6010f72d 100644 --- a/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp +++ b/ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp @@ -73,5 +73,34 @@ void EscPage::initializePage() bool EscPage::isSynchOrOneShotAvailable() { - return true; + bool available = true; + + switch (getWizard()->getControllerType()) { + case SetupWizard::CONTROLLER_NANO: + switch (getWizard()->getVehicleType()) { + case SetupWizard::VEHICLE_MULTI: + switch (getWizard()->getVehicleSubType()) { + case SetupWizard::MULTI_ROTOR_TRI_Y: + case SetupWizard::MULTI_ROTOR_QUAD_X: + case SetupWizard::MULTI_ROTOR_QUAD_H: + case SetupWizard::MULTI_ROTOR_QUAD_PLUS: + available = getWizard()->getInputType() != SetupWizard::INPUT_PWM; + break; + + default: + available = false; + break; + } + break; + + default: + break; + } + break; + + default: + break; + } + + return available; } diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index c7584fea9..d5d991141 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,10 +1,6 @@ Selection of optional hardware configurations. - - - - @@ -28,9 +24,7 @@ From 0ea8260a876528422ba427a222329a48377d7648 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Tue, 26 May 2015 17:29:51 +1000 Subject: [PATCH 9/9] REVONANO Truely disarm on pathfollower guidance alarm --- flight/modules/ManualControl/armhandler.c | 1 + 1 file changed, 1 insertion(+) diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index fe80141a6..dedd4b03c 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -117,6 +117,7 @@ void armHandler(bool newinit, FrameType_t frameType) if (forcedDisArm()) { // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) + armState = ARM_STATE_DISARMED; setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); return; }