rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-16 08:29:15 +01:00
Code Issues Releases Activity

OP-1740: Force latest Pathfollower changes to use uavobj enums.

Browse Source
This commit is contained in:
Richard von Lehe 2015-04-26 17:46:49 -05:00
parent f3611591e0
commit d22b96d24b
4 changed files with 32 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/modules/PathFollower/inc/vtolautotakeoffcontroller.h
View File

@ -64,7 +64,7 @@ public:
private: private:
void UpdateVelocityDesired(void); void UpdateVelocityDesired(void);
int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction); int8_t UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction);
void setArmedIfChanged(uint8_t val); void setArmedIfChanged(FlightStatusArmedOptions val);
VtolAutoTakeoffFSM *fsm; VtolAutoTakeoffFSM *fsm;
VtolPathFollowerSettingsData *vtolPathFollowerSettings; VtolPathFollowerSettingsData *vtolPathFollowerSettings;

4
flight/modules/PathFollower/inc/vtolautotakeofffsm.h
View File

@ -82,7 +82,7 @@ protected:
// FSM instance data type // FSM instance data type
typedef struct { typedef struct {
StatusVtolAutoTakeoffData fsmAutoTakeoffStatus; StatusVtolAutoTakeoffData fsmAutoTakeoffStatus;
PathFollowerFSM_AutoTakeoffState_T currentState; StatusVtolAutoTakeoffStateOptions currentState;
TakeOffLocationData takeOffLocation; TakeOffLocationData takeOffLocation;
uint32_t stateRunCount; uint32_t stateRunCount;
uint32_t stateTimeoutCount; uint32_t stateTimeoutCount;
@ -142,7 +142,7 @@ protected:
void run_abort(uint8_t); void run_abort(uint8_t);
void initFSM(void); void initFSM(void);
void setState(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason); void setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason);
int32_t runState(); int32_t runState();
int32_t runAlways(); int32_t runAlways();

2
flight/modules/PathFollower/vtolautotakeoffcontroller.cpp
View File

@ -299,7 +299,7 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
PathStatusSet(pathStatus); PathStatusSet(pathStatus);
} }
void VtolAutoTakeoffController::setArmedIfChanged(uint8_t val) void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val)
{ {
if (flightStatus->Armed != val) { if (flightStatus->Armed != val) {
flightStatus->Armed = val; flightStatus->Armed = val;

56
flight/modules/PathFollower/vtolautotakeofffsm.cpp
View File

@ -128,23 +128,23 @@ void VtolAutoTakeoffFSM::Inactive(void)
void VtolAutoTakeoffFSM::initFSM(void) void VtolAutoTakeoffFSM::initFSM(void)
{ {
if (vtolPathFollowerSettings != 0) { if (vtolPathFollowerSettings != 0) {
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} else { } else {
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE; mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
} }
} }
void VtolAutoTakeoffFSM::Activate() void VtolAutoTakeoffFSM::Activate()
{ {
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T)); memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE; mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->flLowAltitude = true; mAutoTakeoffData->flLowAltitude = true;
mAutoTakeoffData->flAltitudeHold = false; mAutoTakeoffData->flAltitudeHold = false;
mAutoTakeoffData->boundThrustMin = 0.0f; mAutoTakeoffData->boundThrustMin = 0.0f;
mAutoTakeoffData->boundThrustMax = 0.0f; mAutoTakeoffData->boundThrustMax = 0.0f;
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation)); TakeOffLocationGet(&(mAutoTakeoffData->takeOffLocation));
mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[AUTOTAKEOFF_STATE_INACTIVE] = 0.0f; mAutoTakeoffData->fsmAutoTakeoffStatus.AltitudeAtState[STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE] = 0.0f;
mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED; mAutoTakeoffData->fsmAutoTakeoffStatus.ControlState = STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED;
assessAltitude(); assessAltitude();
@ -153,31 +153,31 @@ void VtolAutoTakeoffFSM::Activate()
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) { if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return; return;
} }
// initially inactive and wait for a change in controlstate. // initially inactive and wait for a change in controlstate.
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} }
void VtolAutoTakeoffFSM::Abort(void) void VtolAutoTakeoffFSM::Abort(void)
{ {
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} }
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void) PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{ {
switch (mAutoTakeoffData->currentState) { switch (mAutoTakeoffData->currentState) {
case AUTOTAKEOFF_STATE_INACTIVE: case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE; return PFFSM_STATE_INACTIVE;
break; break;
case AUTOTAKEOFF_STATE_ABORT: case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT; return PFFSM_STATE_ABORT;
break; break;
case AUTOTAKEOFF_STATE_DISARMED: case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
return PFFSM_STATE_DISARMED; return PFFSM_STATE_DISARMED;
break; break;
@ -248,7 +248,7 @@ void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDes
// Set the new state and perform setup for subsequent state run calls // Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive // This is called by state run functions on event detection that drive
// state transitions. // state transitions.
void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason) void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
{ {
mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason; mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
@ -257,7 +257,7 @@ void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, S
} }
mAutoTakeoffData->currentState = newState; mAutoTakeoffData->currentState = newState;
if (newState != AUTOTAKEOFF_STATE_INACTIVE) { if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
PositionStateData positionState; PositionStateData positionState;
PositionStateGet(&positionState); PositionStateGet(&positionState);
float takeOffDown = 0.0f; float takeOffDown = 0.0f;
@ -324,22 +324,22 @@ void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOption
switch (controlState) { switch (controlState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
setState(AUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT: case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break; break;
} }
} }
@ -367,7 +367,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) { if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return; return;
} }
@ -377,7 +377,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
mAutoTakeoffData->boundThrustMin = -0.1f; mAutoTakeoffData->boundThrustMin = -0.1f;
mAutoTakeoffData->boundThrustMax = 0.0f; mAutoTakeoffData->boundThrustMax = 0.0f;
setState(AUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT); setState(STATUSVTOLAUTOTAKEOFF_STATE_SLOWSTART, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
} }
// STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect. // STATE: SLOWSTART spools up motors to vtol min over 2 seconds for effect.
@ -415,7 +415,7 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t flTimeout
} }
if (flTimeout) { if (flTimeout) {
setState(AUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT); setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTUP, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
} }
} }
@ -442,7 +442,7 @@ void VtolAutoTakeoffFSM::run_thrustup(__attribute__((unused)) uint8_t flTimeout)
} }
if (flTimeout) { if (flTimeout) {
setState(AUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT); setState(STATUSVTOLAUTOTAKEOFF_STATE_TAKEOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
} }
} }
@ -458,7 +458,7 @@ void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) { if (stabDesired.Thrust < 0.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST); setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
return; return;
} }
@ -470,11 +470,11 @@ void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
float down_error = pathDesired->End.Down - positionState.Down; float down_error = pathDesired->End.Down - positionState.Down;
float positionError = sqrtf(north_error * north_error + east_error * east_error); float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 3.0f) { if (positionError > 3.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR); setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
return; return;
} }
if (fabsf(down_error) < 0.5f) { if (fabsf(down_error) < 0.5f) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT); setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
return; return;
} }
} }
@ -515,11 +515,11 @@ void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeou
StabilizationDesiredData stabDesired; StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired); StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) { if (stabDesired.Thrust < 0.0f || mAutoTakeoffData->thrustLimit < 0.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST); setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
} }
if (flTimeout) { if (flTimeout) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT); setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_TIMEOUT);
} }
} }
@ -534,7 +534,7 @@ void VtolAutoTakeoffFSM::setup_thrustoff(void)
void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout) void VtolAutoTakeoffFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{ {
setState(AUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE); setState(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} }
// STATE: DISARMED // STATE: DISARMED