diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index f83b4644e..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; } @@ -346,18 +347,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/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/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/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/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/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) -{} 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); 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) { 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/flight/modules/PathFollower/vtollandfsm.cpp b/flight/modules/PathFollower/vtollandfsm.cpp index f51b76fe0..267299bc6 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 @@ -96,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 @@ -176,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; @@ -431,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); } } @@ -637,10 +628,11 @@ void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout) // STATE: THRUSTOFF void VtolLandFSM::setup_thrustoff(void) { - mLandData->thrustLimit = -1.0f; - mLandData->flConstrainThrust = true; - mLandData->boundThrustMin = -0.1f; - mLandData->boundThrustMax = 0.0f; + mLandData->thrustLimit = -1.0f; + mLandData->flZeroStabiHorizontal = true; + mLandData->flConstrainThrust = true; + mLandData->boundThrustMin = -0.1f; + mLandData->boundThrustMax = 0.0f; } void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout) @@ -653,49 +645,28 @@ 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; + + // 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); + } } 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) -{} 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/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 @@ 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"/> 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 @@ + +