rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

Merge branch 'abeck/nano-rc4-fixes' into rel-nano-15.05

Browse Source
This commit is contained in:
abeck70 2015-05-26 21:31:08 +10:00
commit 647fa80d39
21 changed files with 163 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/modules/ManualControl/armhandler.c
View File

@ -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;
}

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

@ -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;

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

@ -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];
};

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

@ -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];
};

5
flight/modules/PathFollower/inc/vtollandfsm.h
View File

@ -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];
};

1
flight/modules/PathFollower/inc/vtolvelocitycontroller.h
View File

@ -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;

5
flight/modules/PathFollower/pathfollower.cpp
View File

@ -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;
}
}

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

@ -30,6 +30,7 @@ extern "C" {
#include <math.h>
#include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -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);
}
}

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

@ -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)
{}

16
flight/modules/PathFollower/vtolbrakecontroller.cpp
View File

@ -35,6 +35,7 @@ extern "C" {
#include <math.h>
#include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -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);

5
flight/modules/PathFollower/vtolbrakefsm.cpp
View File

@ -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) {

9
flight/modules/PathFollower/vtollandcontroller.cpp
View File

@ -30,6 +30,7 @@ extern "C" {
#include <math.h>
#include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -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);

71
flight/modules/PathFollower/vtollandfsm.cpp
View File

@ -31,6 +31,7 @@ extern "C" {
#include <math.h>
#include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -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)
{}

35
flight/modules/PathFollower/vtolvelocitycontroller.cpp
View File

@ -30,6 +30,7 @@ extern "C" {
#include <math.h>
#include <pid.h>
#include <alarms.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
@ -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);
}

51
flight/targets/boards/revonano/firmware/pios_board.c
View File

@ -36,7 +36,8 @@
#include <pios_oplinkrcvr_priv.h>
#include <taskinfo.h>
#include <pios_ws2811.h>
#include <sanitycheck.h>
#include <actuatorsettings.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <pios_instrumentation.h>
@ -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();
}
/**
* @}
* @}

5
ground/openpilotgcs/src/plugins/config/configoutputwidget.cpp
View File

@ -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'<br>"
"When using Receiver Port setting 'PPM_PIN8+OneShot' "
"<b><font color='%1'>Bank %2</font></b> 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'<br>"));
return;
}
}

31
ground/openpilotgcs/src/plugins/setupwizard/pages/escpage.cpp
View File

@ -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;
}

8
shared/uavobjectdefinition/hwsettings.xml
View File

@ -1,10 +1,6 @@
<xml>
<object name="HwSettings" singleinstance="true" settings="true" category="System">
<description>Selection of optional hardware configurations.</description>
<field name="CC_RcvrPort" units="function" type="enum" elements="1" options="Disabled+OneShot,PWM+NoOneShot,PPM+NoOneShot,PPM+PWM+NoOneShot,PPM+Outputs+NoOneShot,PPM_PIN8+OneShot, Outputs+OneShot" defaultvalue="PWM+NoOneShot"/>
<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,DebugConsole,ComBridge,OsdHk" defaultvalue="Telemetry"/>
<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,PPM,DSM,DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RV_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+Outputs,Outputs" defaultvalue="PWM"/>
<field name="RV_AuxPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,DSM,ComAux,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RV_AuxSBusPort" units="function" type="enum" elements="1" options="Disabled,S.Bus,DSM,ComAux,ComBridge,OsdHk" defaultvalue="Disabled"/>
@ -28,9 +24,7 @@
<field name="ADCRouting" units="" type="enum" elementnames="adc0,adc1,adc2,adc3,adc4,adc5,adc6,adc7" options="Disabled,BatteryVoltage,BatteryCurrent,AnalogAirspeed,Generic" defaultvalue="Disabled"/>
<field name="DSMxBind" units="" type="uint8" elements="1" defaultvalue="0"/>
<field name="WS2811LED_Out" units="" type="enum" elements="1" options="ServoOut1,ServoOut2,ServoOut3,ServoOut4,ServoOut5,ServoOut6,FlexiIOPin3,FlexiIOPin4,Disabled" defaultvalue="Disabled"
limits="%0905NE:ServoOut2:ServoOut3:ServoOut4:ServoOut5:ServoOut6:FlexiIOPin3:FlexiIOPin4,\
%0401NE:ServoOut1:ServoOut2:ServoOut3:ServoOut4:ServoOut5:ServoOut6:FlexiIOPin3:FlexiIOPin4,\
%0402NE:ServoOut1:ServoOut2:ServoOut3:ServoOut4:ServoOut5:ServoOut6:FlexiIOPin3:FlexiIOPin4;"
limits="%0905NE:ServoOut2:ServoOut3:ServoOut4:ServoOut5:ServoOut6:FlexiIOPin3:FlexiIOPin4;"
/>
<access gcs="readwrite" flight="readwrite"/>

2
shared/uavobjectdefinition/statusvtolautotakeoff.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="StatusVtolAutoTakeoff" singleinstance="true" settings="false" category="Navigation">
<description>Status of a AutoTakeoff autopilot</description>
<field name="State" units="" type="enum" elements="1" options="Inactive,CheckState,SlowStart,ThrustUp,Takeoff,Hold,ThrustDown,ThrustOff,Disarmed, Abort" default="0"/>
<field name="State" units="" type="enum" elements="1" options="Inactive,CheckState,SlowStart,ThrustUp,Takeoff,Hold,ThrustDown,ThrustOff,Disarmed" default="0"/>
<field name="AltitudeAtState" units="m" type="float" elements="10" default="0"/>
<field name="StateExitReason" units="" type="enum" elements="10" options="None,ArrivedAtAlt,ZeroThrust,PositionError,Timeout" default="0"/>
<field name="AltitudeState" units="" type="enum" elements="1" options="High,Low" default="0"/>

2
shared/uavobjectdefinition/statusvtolland.xml
View File

@ -2,7 +2,7 @@
<object name="StatusVtolLand" singleinstance="true" settings="false" category="Navigation">
<description>Status of a Vtol landing sequence</description>
<field name="State" units="" type="enum" elements="1" options="Inactive,InitAltHold,WtgForDescentRate,AtDescentRate,WtgForGroundEffect,
GroundEffect,ThrustDown,ThrustOff,Disarmed, Abort" default="0"/>
GroundEffect,ThrustDown,ThrustOff,Disarmed" default="0"/>
<field name="AltitudeAtState" units="m" type="float" elements="10" default="0"/>
<field name="StateExitReason" units="" type="enum" elements="10" options="None,DescentRateOk,OnGround,BounceVelocity,BounceAccel,LowDescentRate,ZeroThrust,PositionError,Timeout" default="0"/>
<field name="targetDescentRate" units="m" type="float" elements="1"/>

3
shared/uavobjectdefinition/vtolpathfollowersettings.xml
View File

@ -18,7 +18,6 @@
<field name="EmergencyFallbackAttitude" units="deg" type="float" elementnames="Roll,Pitch" defaultvalue="0,-20.0"/>
<field name="EmergencyFallbackYawRate" units="(deg/s)/deg" type="float" elementnames="kP,Max" defaultvalue="2.0, 30.0"/>
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
<field name="VelocityRoamMaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="50"/>
<field name="BrakeRate" units="m/s2" type="float" elements="1" defaultvalue="2.5"/>
<field name="BrakeMaxPitch" units="deg" type="float" elements="1" defaultvalue="25"/>
@ -26,6 +25,8 @@
<field name="BrakeVelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="0"/>
<field name="LandVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.42, 3.0, 0.02, 0.95"/>
<field name="AutoTakeoffVerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="0.42, 3.0, 0.02, 0.95"/>
<field name="VelocityRoamMaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
<field name="VelocityRoamHorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,Beta" defaultvalue="8.0, 0.5, 0.001, 0.95"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>