rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Merge branch 'next' of ssh://git.openpilot.org/OpenPilot into abeck/OP-1858-autotakeoff

Browse Source
This commit is contained in:
abeck70 2015-05-04 21:15:24 +10:00
commit 16cc9f1512
52 changed files with 1771 additions and 1099 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/libraries/plans.c
View File

@ -157,7 +157,7 @@ void plan_setup_returnToBase()
pathDesired.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f;
uint8_t ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f;

22
flight/libraries/sanitycheck.c
View File

@ -74,7 +74,7 @@ int32_t configuration_check()
// Classify navigation capability
#ifdef REVOLUTION
RevoSettingsInitialize();
uint8_t revoFusion;
RevoSettingsFusionAlgorithmOptions revoFusion;
RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion;
switch (revoFusion) {
@ -104,8 +104,8 @@ int32_t configuration_check()
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
FlightModeSettingsFlightModePositionGet(modes);
@ -209,7 +209,7 @@ int32_t configuration_check()
}
}
uint8_t checks_disabled;
FlightModeSettingsDisableSanityChecksOptions checks_disabled;
FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
severity = SYSTEMALARMS_ALARM_WARNING;
@ -237,22 +237,22 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
// Get the different axis modes for this switch position
switch (index) {
case 1:
FlightModeSettingsStabilization1SettingsArrayGet(modes);
FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
break;
case 2:
FlightModeSettingsStabilization2SettingsArrayGet(modes);
FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
break;
case 3:
FlightModeSettingsStabilization3SettingsArrayGet(modes);
FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
break;
case 4:
FlightModeSettingsStabilization4SettingsArrayGet(modes);
FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
break;
case 5:
FlightModeSettingsStabilization5SettingsArrayGet(modes);
FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
break;
case 6:
FlightModeSettingsStabilization6SettingsArrayGet(modes);
FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
break;
default:
return false;
@ -326,7 +326,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
FrameType_t GetCurrentFrameType()
{
uint8_t airframe_type;
SystemSettingsAirframeTypeOptions airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch ((SystemSettingsAirframeTypeOptions)airframe_type) {

6
flight/modules/Airspeed/airspeed.c
View File

@ -98,7 +98,7 @@ int32_t AirspeedInitialize()
#else
HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesArrayGet(optionalModules);
@ -110,7 +110,7 @@ int32_t AirspeedInitialize()
}
#endif
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingOptions adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adcRouting);
// Determine if the barometric airspeed sensor is routed to an ADC pin
@ -169,6 +169,8 @@ static void airspeedTask(__attribute__((unused)) void *parameters)
imu_airspeedInitialize(&airspeedSettings);
}
break;
default:
break;
}
}
switch (airspeedSettings.AirspeedSensorType) {

6
flight/modules/GPS/GPS.c
View File

@ -153,7 +153,7 @@ int32_t GPSStart(void)
int32_t GPSInitialize(void)
{
gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol;
GPSSettingsDataProtocolOptions gpsProtocol;
#ifdef MODULE_GPS_BUILTIN
gpsEnabled = true;
@ -338,7 +338,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while.
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusOptions status = GPSPOSITIONSENSOR_STATUS_NOGPS;
GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else {
@ -439,7 +439,7 @@ static void updateHwSettings()
{
if (gpsPort) {
// Retrieve settings
uint8_t speed;
HwSettingsGPSSpeedOptions speed;
HwSettingsGPSSpeedGet(&speed);
// Set port speed

2
flight/modules/Logging/Logging.c
View File

@ -134,7 +134,7 @@ static void ControlUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
}
DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed;
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format();

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

@ -182,6 +182,8 @@ void armHandler(bool newinit, FrameType_t frameType)
case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * acc.AccessoryVal;
break;
default:
break;
}
bool manualArm = false;

4
flight/modules/ManualControl/manualcontrol.c
View File

@ -182,7 +182,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
@ -486,7 +486,7 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight
{
uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {

6
flight/modules/ManualControl/pathfollowerhandler.c
View File

@ -55,9 +55,9 @@ void pathFollowerHandler(bool newinit)
plan_initialize();
}
uint8_t flightMode;
uint8_t assistedControlFlightMode;
uint8_t flightModeAssist;
FlightStatusFlightModeOptions flightMode;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
FlightStatusFlightModeAssistOptions flightModeAssist;
FlightStatusFlightModeGet(&flightMode);
FlightStatusFlightModeAssistGet(&flightModeAssist);
FlightStatusAssistedControlStateGet(&assistedControlFlightMode);

14
flight/modules/ManualControl/stabilizedhandler.c
View File

@ -96,27 +96,27 @@ void stabilizedHandler(bool newinit)
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization2SettingsToArray(settings.Stabilization2Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization3SettingsToArray(settings.Stabilization3Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
stab_settings = FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization4SettingsToArray(settings.Stabilization4Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
stab_settings = FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization5SettingsToArray(settings.Stabilization5Settings);
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
stab_settings = FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization6SettingsToArray(settings.Stabilization6Settings);
break;
default:
// Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
stab_settings = (uint8_t *)FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings);
return;
}

12
flight/modules/ManualControl/takeofflocationhandler.c
View File

@ -42,8 +42,8 @@ void takeOffLocationHandlerInit()
{
TakeOffLocationInitialize();
// check whether there is a preset/valid takeoff location
uint8_t mode;
uint8_t status;
TakeOffLocationModeOptions mode;
TakeOffLocationStatusOptions status;
TakeOffLocationModeGet(&mode);
TakeOffLocationStatusGet(&status);
// preset with invalid location will actually behave like FirstTakeoff
@ -61,8 +61,8 @@ void takeOffLocationHandlerInit()
*/
void takeOffLocationHandler()
{
uint8_t armed;
uint8_t status;
FlightStatusArmedOptions armed;
TakeOffLocationStatusOptions status;
FlightStatusArmedGet(&armed);
@ -77,7 +77,7 @@ void takeOffLocationHandler()
case FLIGHTSTATUS_ARMED_ARMING:
case FLIGHTSTATUS_ARMED_ARMED:
if (!locationSet || status != TAKEOFFLOCATION_STATUS_VALID) {
uint8_t mode;
TakeOffLocationModeOptions mode;
TakeOffLocationModeGet(&mode);
if ((mode != TAKEOFFLOCATION_MODE_PRESET) || (status == TAKEOFFLOCATION_STATUS_INVALID)) {
@ -91,7 +91,7 @@ void takeOffLocationHandler()
case FLIGHTSTATUS_ARMED_DISARMED:
// unset if location is to be acquired at each arming
if (locationSet) {
uint8_t mode;
TakeOffLocationModeOptions mode;
TakeOffLocationModeGet(&mode);
if (mode == TAKEOFFLOCATION_MODE_ARMINGLOCATION) {
locationSet = false;

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

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

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

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

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

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

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

@ -82,7 +82,7 @@ protected:
// FSM instance data type
typedef struct {
StatusVtolLandData fsmLandStatus;
PathFollowerFSM_LandState_T currentState;
StatusVtolLandStateOptions currentState;
TakeOffLocationData takeOffLocation;
uint32_t stateRunCount;
uint32_t stateTimeoutCount;
@ -133,7 +133,7 @@ protected:
void setup_abort(void);
void run_abort(uint8_t);
void initFSM(void);
void setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason);
void setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason);
int32_t runState();
int32_t runAlways();
void updateVtolLandFSMStatus();

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

@ -278,7 +278,7 @@ void VtolAutoTakeoffController::UpdateAutoPilot()
PathStatusSet(pathStatus);
}
void VtolAutoTakeoffController::setArmedIfChanged(uint8_t val)
void VtolAutoTakeoffController::setArmedIfChanged(FlightStatusArmedOptions val)
{
if (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)
{
if (vtolPathFollowerSettings != 0) {
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
} else {
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
}
}
void VtolAutoTakeoffFSM::Activate()
{
memset(mAutoTakeoffData, 0, sizeof(VtolAutoTakeoffFSMData_T));
mAutoTakeoffData->currentState = AUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->currentState = STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE;
mAutoTakeoffData->flLowAltitude = true;
mAutoTakeoffData->flAltitudeHold = false;
mAutoTakeoffData->boundThrustMin = 0.0f;
mAutoTakeoffData->boundThrustMax = 0.0f;
mAutoTakeoffData->flZeroStabiHorizontal = true; // turn off positional controllers
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;
assessAltitude();
@ -153,31 +153,31 @@ void VtolAutoTakeoffFSM::Activate()
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return;
}
// 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)
{
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolAutoTakeoffFSM::GetCurrentState(void)
{
switch (mAutoTakeoffData->currentState) {
case AUTOTAKEOFF_STATE_INACTIVE:
case STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case AUTOTAKEOFF_STATE_ABORT:
case STATUSVTOLAUTOTAKEOFF_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case AUTOTAKEOFF_STATE_DISARMED:
case STATUSVTOLAUTOTAKEOFF_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
break;
@ -248,7 +248,7 @@ void VtolAutoTakeoffFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDes
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
void VtolAutoTakeoffFSM::setState(StatusVtolAutoTakeoffStateOptions newState, StatusVtolAutoTakeoffStateExitReasonOptions reason)
{
mAutoTakeoffData->fsmAutoTakeoffStatus.StateExitReason[mAutoTakeoffData->currentState] = reason;
@ -257,7 +257,7 @@ void VtolAutoTakeoffFSM::setState(PathFollowerFSM_AutoTakeoffState_T newState, S
}
mAutoTakeoffData->currentState = newState;
if (newState != AUTOTAKEOFF_STATE_INACTIVE) {
if (newState != STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
@ -324,22 +324,22 @@ void VtolAutoTakeoffFSM::setControlState(StatusVtolAutoTakeoffControlStateOption
switch (controlState) {
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORARMED:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_REQUIREUNARMEDFIRST:
setState(AUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_INACTIVE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE:
setState(AUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_CHECKSTATE, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_POSITIONHOLD:
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
case STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_ABORT:
setState(AUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_ABORT, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
break;
}
}
@ -367,7 +367,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust > vtolPathFollowerSettings->ThrustLimits.Min) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
return;
}
@ -377,7 +377,7 @@ void VtolAutoTakeoffFSM::setup_checkstate(void)
mAutoTakeoffData->boundThrustMin = -0.1f;
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.
@ -415,7 +415,7 @@ void VtolAutoTakeoffFSM::run_slowstart(__attribute__((unused)) uint8_t 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) {
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);
if (stabDesired.Thrust < 0.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTOFF, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ZEROTHRUST);
return;
}
@ -470,11 +470,11 @@ void VtolAutoTakeoffFSM::run_takeoff(__attribute__((unused)) uint8_t flTimeout)
float down_error = pathDesired->End.Down - positionState.Down;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 3.0f) {
setState(AUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
setState(STATUSVTOLAUTOTAKEOFF_STATE_THRUSTDOWN, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_POSITIONERROR);
return;
}
if (fabsf(down_error) < 0.5f) {
setState(AUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
setState(STATUSVTOLAUTOTAKEOFF_STATE_HOLD, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_ARRIVEDATALT);
return;
}
}
@ -515,11 +515,11 @@ void VtolAutoTakeoffFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeou
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
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) {
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)
{
setState(AUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
setState(STATUSVTOLAUTOTAKEOFF_STATE_DISARMED, STATUSVTOLAUTOTAKEOFF_STATEEXITREASON_NONE);
}
// STATE: DISARMED

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

@ -312,8 +312,10 @@ int8_t VtolBrakeController::UpdateStabilizationDesired(void)
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
default:
break;
}
stabDesired.StabilizationMode.Thrust = thrustMode;
stabDesired.StabilizationMode.Thrust = (StabilizationDesiredStabilizationModeOptions)thrustMode;
}
// set the thrust value

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

@ -1,275 +1,275 @@
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtollandcontroller.h"
#include "pathfollowerfsm.h"
#include "vtollandfsm.h"
#include "pidcontroldown.h"
// Private constants
// pointer to a singleton instance
VtolLandController *VtolLandController::p_inst = 0;
VtolLandController::VtolLandController()
: fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
{}
// Called when mode first engaged
void VtolLandController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolLandController::IsActive(void)
{
return mActive;
}
uint8_t VtolLandController::Mode(void)
{
return PATHDESIRED_MODE_LAND;
}
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
void VtolLandController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
}
void VtolLandController::Deactivate(void)
{
if (mActive) {
mActive = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
void VtolLandController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
// The following is not currently used in the landing control.
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
fsm->SettingsUpdated();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
if (fsm == 0) {
fsm = (PathFollowerFSM *)VtolLandFSM::instance();
VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
controlDown.Initialize(fsm);
}
return 0;
}
void VtolLandController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// Implement optional horizontal position hold.
if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
(flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
// landing flight mode has stored original horizontal position in pathdesired
PositionStateData positionState;
PositionStateGet(&positionState);
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
}
velocityDesired.Down = controlDown.GetVelocityDesired();
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
pathStatus->fractional_progress = 1.0f;
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
stabDesired.Thrust = controlDown.GetDownCommand();
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
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;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolLandController::UpdateAutoPilot()
{
fsm->Update();
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = false;
float yaw = 0.0f;
fsm->GetYaw(yaw_attitude, yaw);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
PathStatusSet(pathStatus);
}
/*
******************************************************************************
*
* @file vtollandcontroller.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Vtol landing controller loop
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <pathsummary.h>
}
// C++ includes
#include "vtollandcontroller.h"
#include "pathfollowerfsm.h"
#include "vtollandfsm.h"
#include "pidcontroldown.h"
// Private constants
// pointer to a singleton instance
VtolLandController *VtolLandController::p_inst = 0;
VtolLandController::VtolLandController()
: fsm(NULL), vtolPathFollowerSettings(NULL), mActive(false)
{}
// Called when mode first engaged
void VtolLandController::Activate(void)
{
if (!mActive) {
mActive = true;
SettingsUpdated();
fsm->Activate();
controlDown.Activate();
controlNE.Activate();
}
}
uint8_t VtolLandController::IsActive(void)
{
return mActive;
}
uint8_t VtolLandController::Mode(void)
{
return PATHDESIRED_MODE_LAND;
}
// Objective updated in pathdesired, e.g. same flight mode but new target velocity
void VtolLandController::ObjectiveUpdated(void)
{
// Set the objective's target velocity
controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_DOWN]);
controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_NORTH],
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_VELOCITY_VELOCITYVECTOR_EAST]);
controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
}
void VtolLandController::Deactivate(void)
{
if (mActive) {
mActive = false;
fsm->Inactive();
controlDown.Deactivate();
controlNE.Deactivate();
}
}
void VtolLandController::SettingsUpdated(void)
{
const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
vtolPathFollowerSettings->HorizontalVelPID.Ki,
vtolPathFollowerSettings->HorizontalVelPID.Kd,
vtolPathFollowerSettings->HorizontalVelPID.ILimit,
dT,
vtolPathFollowerSettings->HorizontalVelMax);
controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
controlDown.UpdateParameters(vtolPathFollowerSettings->LandVerticalVelPID.Kp,
vtolPathFollowerSettings->LandVerticalVelPID.Ki,
vtolPathFollowerSettings->LandVerticalVelPID.Kd,
vtolPathFollowerSettings->LandVerticalVelPID.Beta,
dT,
vtolPathFollowerSettings->VerticalVelMax);
// The following is not currently used in the landing control.
controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
VtolSelfTuningStatsData vtolSelfTuningStats;
VtolSelfTuningStatsGet(&vtolSelfTuningStats);
controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
// initialise limits on thrust but note the FSM can override.
controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
fsm->SettingsUpdated();
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
if (fsm == 0) {
fsm = (PathFollowerFSM *)VtolLandFSM::instance();
VtolLandFSM::instance()->Initialize(ptr_vtolPathFollowerSettings, pathDesired, flightStatus);
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
controlDown.Initialize(fsm);
}
return 0;
}
void VtolLandController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlDown.UpdateVelocityState(velocityState.Down);
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
// Implement optional horizontal position hold.
if ((((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS]) == PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH) ||
(flightStatus->ControlChain.PathPlanner == FLIGHTSTATUS_CONTROLCHAIN_TRUE)) {
// landing flight mode has stored original horizontal position in pathdesired
PositionStateData positionState;
PositionStateGet(&positionState);
controlNE.UpdatePositionState(positionState.North, positionState.East);
controlNE.ControlPosition();
}
velocityDesired.Down = controlDown.GetVelocityDesired();
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
if (fsm->GetCurrentState() == PFFSM_STATE_DISARMED) {
pathStatus->fractional_progress = 1.0f;
}
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = velocityDesired.Down - velocityState.Down;
VelocityDesiredSet(&velocityDesired);
}
int8_t VtolLandController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
stabDesired.Thrust = controlDown.GetDownCommand();
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch);
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
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;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
fsm->ConstrainStabiDesired(&stabDesired); // excludes thrust
StabilizationDesiredSet(&stabDesired);
return result;
}
void VtolLandController::UpdateAutoPilot()
{
fsm->Update();
UpdateVelocityDesired();
// yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
bool yaw_attitude = false;
float yaw = 0.0f;
fsm->GetYaw(yaw_attitude, yaw);
int8_t result = UpdateStabilizationDesired(yaw_attitude, yaw);
if (!result) {
fsm->Abort();
}
PathStatusSet(pathStatus);
}

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

@ -1,701 +1,701 @@
/*
******************************************************************************
*
* @file vtollandfsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* VtolLandController.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolland.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtollandfsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define MIN_LANDRATE 0.1f
#define MAX_LANDRATE 0.6f
#define LOW_ALT_DESCENT_REDUCTION_FACTOR 0.7f // TODO Need to make the transition smooth
#define LANDRATE_LOWLIMIT_FACTOR 0.5f
#define LANDRATE_HILIMIT_FACTOR 1.5f
#define TIMEOUT_INIT_ALTHOLD (3 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_WTG_FOR_DESCENTRATE (10 * TIMER_COUNT_PER_SECOND)
#define WTG_FOR_DESCENTRATE_COUNT_LIMIT 10
#define TIMEOUT_AT_DESCENTRATE 10
#define TIMEOUT_GROUNDEFFECT (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND)
#define LANDING_PID_SCALAR_P 2.0f
#define LANDING_PID_SCALAR_I 10.0f
#define LANDING_SLOWDOWN_HEIGHT -5.0f
#define BOUNCE_VELOCITY_TRIGGER_LIMIT -0.3f
#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
#define BOUNCE_TRIGGER_COUNT 4
#define GROUNDEFFECT_SLOWDOWN_FACTOR 0.3f
#define GROUNDEFFECT_SLOWDOWN_COUNT 4
VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
[LAND_STATE_INACTIVE] = { .setup = &VtolLandFSM::setup_inactive, .run = 0 },
[LAND_STATE_INIT_ALTHOLD] = { .setup = &VtolLandFSM::setup_init_althold, .run = &VtolLandFSM::run_init_althold },
[LAND_STATE_WTG_FOR_DESCENTRATE] = { .setup = &VtolLandFSM::setup_wtg_for_descentrate, .run = &VtolLandFSM::run_wtg_for_descentrate },
[LAND_STATE_AT_DESCENTRATE] = { .setup = &VtolLandFSM::setup_at_descentrate, .run = &VtolLandFSM::run_at_descentrate },
[LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
[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 }
};
// pointer to a singleton instance
VtolLandFSM *VtolLandFSM::p_inst = 0;
VtolLandFSM::VtolLandFSM()
: mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
if (mLandData == 0) {
mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
PIOS_Assert(mLandData);
}
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
initFSM();
return 0;
}
void VtolLandFSM::Inactive(void)
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
initFSM();
}
// Initialise the FSM
void VtolLandFSM::initFSM(void)
{
if (vtolPathFollowerSettings != 0) {
setState(LAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
} else {
mLandData->currentState = LAND_STATE_INACTIVE;
}
}
void VtolLandFSM::Activate()
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
mLandData->currentState = LAND_STATE_INACTIVE;
mLandData->flLowAltitude = false;
mLandData->flAltitudeHold = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
TakeOffLocationGet(&(mLandData->takeOffLocation));
mLandData->fsmLandStatus.AltitudeAtState[LAND_STATE_INACTIVE] = 0.0f;
assessAltitude();
if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
#ifndef DEBUG_GROUNDIMPACT
setState(LAND_STATE_INIT_ALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
#else
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
#endif
} else {
// move to error state and callback to position hold
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
}
void VtolLandFSM::Abort(void)
{
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
PathFollowerFSMState_T VtolLandFSM::GetCurrentState(void)
{
switch (mLandData->currentState) {
case LAND_STATE_INACTIVE:
return PFFSM_STATE_INACTIVE;
break;
case LAND_STATE_ABORT:
return PFFSM_STATE_ABORT;
break;
case LAND_STATE_DISARMED:
return PFFSM_STATE_DISARMED;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolLandFSM::Update()
{
runState();
if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
runAlways();
}
}
int32_t VtolLandFSM::runState(void)
{
uint8_t flTimeout = false;
mLandData->stateRunCount++;
if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sLandStateTable[mLandData->currentState].run) {
(this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
}
return 0;
}
int32_t VtolLandFSM::runAlways(void)
{
void assessAltitude(void);
return 0;
}
// PathFollower implements the PID scheme and has a objective
// set by a PathDesired object. Based on the mode, pathfollower
// uses FSM's as helper functions that manage state and event detection.
// PathFollower calls into FSM methods to alter its commands.
void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
{
ulow = mLandData->boundThrustMin;
uhigh = mLandData->boundThrustMax;
if (mLandData->flConstrainThrust) {
uhigh = mLandData->thrustLimit;
}
}
void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
{
if (mLandData->flZeroStabiHorizontal && stabDesired) {
stabDesired->Pitch = 0.0f;
stabDesired->Roll = 0.0f;
stabDesired->Yaw = 0.0f;
}
}
void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
{
if (mLandData->flLowAltitude) {
local_scaler->p = LANDING_PID_SCALAR_P;
local_scaler->i = LANDING_PID_SCALAR_I;
}
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolLandFSM::setState(PathFollowerFSM_LandState_T newState, StatusVtolLandStateExitReasonOptions reason)
{
mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
if (mLandData->currentState == newState) {
return;
}
mLandData->currentState = newState;
if (newState != LAND_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
}
// Restart state timer counter
mLandData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mLandData->stateTimeoutCount = 0;
if (sLandStateTable[mLandData->currentState].setup) {
(this->*sLandStateTable[mLandData->currentState].setup)();
}
updateVtolLandFSMStatus();
}
// Timeout utility function for use by state init implementations
void VtolLandFSM::setStateTimeout(int32_t count)
{
mLandData->stateTimeoutCount = count;
}
void VtolLandFSM::updateVtolLandFSMStatus()
{
mLandData->fsmLandStatus.State = mLandData->currentState;
if (mLandData->flLowAltitude) {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
} else {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
}
StatusVtolLandSet(&mLandData->fsmLandStatus);
}
float VtolLandFSM::BoundVelocityDown(float velocity_down)
{
velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
if (mLandData->flLowAltitude) {
velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
}
mLandData->fsmLandStatus.targetDescentRate = velocity_down;
if (mLandData->flAltitudeHold) {
return 0.0f;
} else {
return velocity_down;
}
}
void VtolLandFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
mLandData->flLowAltitude = false;
} else {
mLandData->flLowAltitude = true;
}
}
// FSM Setup and Run method implementation
// State: INACTIVE
void VtolLandFSM::setup_inactive(void)
{
// Re-initialise local variables
mLandData->flZeroStabiHorizontal = false;
mLandData->flConstrainThrust = false;
}
// State: INIT ALTHOLD
void VtolLandFSM::setup_init_althold(void)
{
setStateTimeout(TIMEOUT_INIT_ALTHOLD);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = true;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_init_althold(uint8_t flTimeout)
{
if (flTimeout) {
mLandData->flAltitudeHold = false;
setState(LAND_STATE_WTG_FOR_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: WAITING FOR DESCENT RATE
void VtolLandFSM::setup_wtg_for_descentrate(void)
{
setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = false;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
{
// Look at current actual thrust...are we already shutdown??
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// We don't expect PID to get exactly the target descent rate, so have a lower
// water mark but need to see 5 observations to be confident that we have semi-stable
// descent achieved
// we need to see velocity down within a range of control before we proceed, without which we
// really don't have confidence to allow later states to run.
if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
setState(LAND_STATE_AT_DESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
return;
}
}
if (flTimeout) {
setState(LAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: AT DESCENT RATE
void VtolLandFSM::setup_at_descentrate(void)
{
setStateTimeout(TIMEOUT_AT_DESCENTRATE);
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->sum1 += velocityState.Down;
mLandData->sum2 += stabDesired.Thrust;
mLandData->observationCount++;
if (flTimeout) {
mLandData->fsmLandStatus.averageDescentRate = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
// As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
// detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
}
}
// State: WAITING FOR GROUND EFFECT
void VtolLandFSM::setup_wtg_for_groundeffect(void)
{
// No timeout
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
// detect material downrating in thrust for 1 second.
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
AccelStateData accelState;
AccelStateGet(&accelState);
// +ve 9.8 expected
float g_e;
HomeLocationg_eGet(&g_e);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// detect bounce
uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
if (flBounce) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
}
// invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
// a relative acceleration term.
float bounceAccel = -accelState.z - g_e;
uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
if (flBounceAccel) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
}
if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
mLandData->observation2Count++;
if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
setState(LAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
return;
}
} else {
mLandData->observation2Count = 0;
}
// detect low descent rate
uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
if (flDescentRateLow) {
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
mLandData->observationCount++;
if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
#ifndef DEBUG_GROUNDIMPACT
setState(LAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
#endif
return;
}
} else {
mLandData->observationCount = 0;
}
updateVtolLandFSMStatus();
}
// STATE: GROUNDEFFET
void VtolLandFSM::setup_groundeffect(void)
{
setStateTimeout(TIMEOUT_GROUNDEFFECT);
mLandData->flZeroStabiHorizontal = false;
PositionStateData positionState;
PositionStateGet(&positionState);
mLandData->expectedLandPositionNorth = positionState.North;
mLandData->expectedLandPositionEast = positionState.East;
mLandData->flConstrainThrust = false;
// now that we have ground effect limit max thrust to neutral
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) {
setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
return;
}
// Stay in this state until we get a low altitude flag.
if (mLandData->flLowAltitude == false) {
// worst case scenario is that we land and the pid brings thrust down to zero.
return;
}
// detect broad sideways drift. If for some reason we have a hard landing that the bounce detection misses, this will kick in
PositionStateData positionState;
PositionStateGet(&positionState);
float north_error = mLandData->expectedLandPositionNorth - positionState.North;
float east_error = mLandData->expectedLandPositionEast - positionState.East;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 1.5f) {
setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
return;
}
if (flTimeout) {
setState(LAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTDOWN
void VtolLandFSM::setup_thrustdown(void)
{
setStateTimeout(TIMEOUT_THRUSTDOWN);
mLandData->flZeroStabiHorizontal = true;
mLandData->flConstrainThrust = true;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->thrustLimit = stabDesired.Thrust;
mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
{
// reduce thrust setpoint step by step
mLandData->thrustLimit -= mLandData->sum1;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(LAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTOFF
void VtolLandFSM::setup_thrustoff(void)
{
mLandData->thrustLimit = -1.0f;
mLandData->flConstrainThrust = true;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{
setState(LAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
// STATE: DISARMED
void VtolLandFSM::setup_disarmed(void)
{
// nothing to do
mLandData->flConstrainThrust = false;
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
{
#ifdef DEBUG_GROUNDIMPACT
if (mLandData->observationCount++ > 100) {
setState(LAND_STATE_WTG_FOR_GROUNDEFFECT, 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)
{}
/*
******************************************************************************
*
* @file vtollandfsm.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief This landing state machine is a helper state machine to the
* VtolLandController.
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
extern "C" {
#include <openpilot.h>
#include <callbackinfo.h>
#include <math.h>
#include <pid.h>
#include <CoordinateConversions.h>
#include <sin_lookup.h>
#include <pathdesired.h>
#include <paths.h>
#include "plans.h"
#include <sanitycheck.h>
#include <homelocation.h>
#include <accelstate.h>
#include <vtolpathfollowersettings.h>
#include <flightstatus.h>
#include <flightmodesettings.h>
#include <pathstatus.h>
#include <positionstate.h>
#include <velocitystate.h>
#include <velocitydesired.h>
#include <stabilizationdesired.h>
#include <airspeedstate.h>
#include <attitudestate.h>
#include <takeofflocation.h>
#include <poilocation.h>
#include <manualcontrolcommand.h>
#include <systemsettings.h>
#include <stabilizationbank.h>
#include <stabilizationdesired.h>
#include <vtolselftuningstats.h>
#include <statusvtolland.h>
#include <pathsummary.h>
}
// C++ includes
#include <vtollandfsm.h>
// Private constants
#define TIMER_COUNT_PER_SECOND (1000 / vtolPathFollowerSettings->UpdatePeriod)
#define MIN_LANDRATE 0.1f
#define MAX_LANDRATE 0.6f
#define LOW_ALT_DESCENT_REDUCTION_FACTOR 0.7f // TODO Need to make the transition smooth
#define LANDRATE_LOWLIMIT_FACTOR 0.5f
#define LANDRATE_HILIMIT_FACTOR 1.5f
#define TIMEOUT_INIT_ALTHOLD (3 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_WTG_FOR_DESCENTRATE (10 * TIMER_COUNT_PER_SECOND)
#define WTG_FOR_DESCENTRATE_COUNT_LIMIT 10
#define TIMEOUT_AT_DESCENTRATE 10
#define TIMEOUT_GROUNDEFFECT (1 * TIMER_COUNT_PER_SECOND)
#define TIMEOUT_THRUSTDOWN (2 * TIMER_COUNT_PER_SECOND)
#define LANDING_PID_SCALAR_P 2.0f
#define LANDING_PID_SCALAR_I 10.0f
#define LANDING_SLOWDOWN_HEIGHT -5.0f
#define BOUNCE_VELOCITY_TRIGGER_LIMIT -0.3f
#define BOUNCE_ACCELERATION_TRIGGER_LIMIT -9.0f // -6.0 found to be too sensitive
#define BOUNCE_TRIGGER_COUNT 4
#define GROUNDEFFECT_SLOWDOWN_FACTOR 0.3f
#define GROUNDEFFECT_SLOWDOWN_COUNT 4
VtolLandFSM::PathFollowerFSM_LandStateHandler_T VtolLandFSM::sLandStateTable[LAND_STATE_SIZE] = {
[LAND_STATE_INACTIVE] = { .setup = &VtolLandFSM::setup_inactive, .run = 0 },
[LAND_STATE_INIT_ALTHOLD] = { .setup = &VtolLandFSM::setup_init_althold, .run = &VtolLandFSM::run_init_althold },
[LAND_STATE_WTG_FOR_DESCENTRATE] = { .setup = &VtolLandFSM::setup_wtg_for_descentrate, .run = &VtolLandFSM::run_wtg_for_descentrate },
[LAND_STATE_AT_DESCENTRATE] = { .setup = &VtolLandFSM::setup_at_descentrate, .run = &VtolLandFSM::run_at_descentrate },
[LAND_STATE_WTG_FOR_GROUNDEFFECT] = { .setup = &VtolLandFSM::setup_wtg_for_groundeffect, .run = &VtolLandFSM::run_wtg_for_groundeffect },
[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 }
};
// pointer to a singleton instance
VtolLandFSM *VtolLandFSM::p_inst = 0;
VtolLandFSM::VtolLandFSM()
: mLandData(0), vtolPathFollowerSettings(0), pathDesired(0), flightStatus(0)
{}
// Private types
// Private functions
// Public API methods
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t VtolLandFSM::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings,
PathDesiredData *ptr_pathDesired,
FlightStatusData *ptr_flightStatus)
{
PIOS_Assert(ptr_vtolPathFollowerSettings);
PIOS_Assert(ptr_pathDesired);
PIOS_Assert(ptr_flightStatus);
if (mLandData == 0) {
mLandData = (VtolLandFSMData_T *)pios_malloc(sizeof(VtolLandFSMData_T));
PIOS_Assert(mLandData);
}
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
pathDesired = ptr_pathDesired;
flightStatus = ptr_flightStatus;
initFSM();
return 0;
}
void VtolLandFSM::Inactive(void)
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
initFSM();
}
// Initialise the FSM
void VtolLandFSM::initFSM(void)
{
if (vtolPathFollowerSettings != 0) {
setState(STATUSVTOLLAND_STATE_INACTIVE, STATUSVTOLLAND_STATEEXITREASON_NONE);
} else {
mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
}
}
void VtolLandFSM::Activate()
{
memset(mLandData, 0, sizeof(VtolLandFSMData_T));
mLandData->currentState = STATUSVTOLLAND_STATE_INACTIVE;
mLandData->flLowAltitude = false;
mLandData->flAltitudeHold = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->fsmLandStatus.calculatedNeutralThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
TakeOffLocationGet(&(mLandData->takeOffLocation));
mLandData->fsmLandStatus.AltitudeAtState[STATUSVTOLLAND_STATE_INACTIVE] = 0.0f;
assessAltitude();
if (pathDesired->Mode == PATHDESIRED_MODE_LAND) {
#ifndef DEBUG_GROUNDIMPACT
setState(STATUSVTOLLAND_STATE_INITALTHOLD, STATUSVTOLLAND_STATEEXITREASON_NONE);
#else
setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_NONE);
#endif
} else {
// move to error state and callback to position hold
setState(STATUSVTOLLAND_STATE_ABORT, 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;
break;
default:
return PFFSM_STATE_ACTIVE;
break;
}
}
void VtolLandFSM::Update()
{
runState();
if (GetCurrentState() != PFFSM_STATE_INACTIVE) {
runAlways();
}
}
int32_t VtolLandFSM::runState(void)
{
uint8_t flTimeout = false;
mLandData->stateRunCount++;
if (mLandData->stateTimeoutCount > 0 && mLandData->stateRunCount > mLandData->stateTimeoutCount) {
flTimeout = true;
}
// If the current state has a static function, call it
if (sLandStateTable[mLandData->currentState].run) {
(this->*sLandStateTable[mLandData->currentState].run)(flTimeout);
}
return 0;
}
int32_t VtolLandFSM::runAlways(void)
{
void assessAltitude(void);
return 0;
}
// PathFollower implements the PID scheme and has a objective
// set by a PathDesired object. Based on the mode, pathfollower
// uses FSM's as helper functions that manage state and event detection.
// PathFollower calls into FSM methods to alter its commands.
void VtolLandFSM::BoundThrust(float &ulow, float &uhigh)
{
ulow = mLandData->boundThrustMin;
uhigh = mLandData->boundThrustMax;
if (mLandData->flConstrainThrust) {
uhigh = mLandData->thrustLimit;
}
}
void VtolLandFSM::ConstrainStabiDesired(StabilizationDesiredData *stabDesired)
{
if (mLandData->flZeroStabiHorizontal && stabDesired) {
stabDesired->Pitch = 0.0f;
stabDesired->Roll = 0.0f;
stabDesired->Yaw = 0.0f;
}
}
void VtolLandFSM::CheckPidScaler(pid_scaler *local_scaler)
{
if (mLandData->flLowAltitude) {
local_scaler->p = LANDING_PID_SCALAR_P;
local_scaler->i = LANDING_PID_SCALAR_I;
}
}
// Set the new state and perform setup for subsequent state run calls
// This is called by state run functions on event detection that drive
// state transitions.
void VtolLandFSM::setState(StatusVtolLandStateOptions newState, StatusVtolLandStateExitReasonOptions reason)
{
mLandData->fsmLandStatus.StateExitReason[mLandData->currentState] = reason;
if (mLandData->currentState == newState) {
return;
}
mLandData->currentState = newState;
if (newState != STATUSVTOLLAND_STATE_INACTIVE) {
PositionStateData positionState;
PositionStateGet(&positionState);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
mLandData->fsmLandStatus.AltitudeAtState[newState] = positionState.Down - takeOffDown;
assessAltitude();
}
// Restart state timer counter
mLandData->stateRunCount = 0;
// Reset state timeout to disabled/zero
mLandData->stateTimeoutCount = 0;
if (sLandStateTable[mLandData->currentState].setup) {
(this->*sLandStateTable[mLandData->currentState].setup)();
}
updateVtolLandFSMStatus();
}
// Timeout utility function for use by state init implementations
void VtolLandFSM::setStateTimeout(int32_t count)
{
mLandData->stateTimeoutCount = count;
}
void VtolLandFSM::updateVtolLandFSMStatus()
{
mLandData->fsmLandStatus.State = mLandData->currentState;
if (mLandData->flLowAltitude) {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_LOW;
} else {
mLandData->fsmLandStatus.AltitudeState = STATUSVTOLLAND_ALTITUDESTATE_HIGH;
}
StatusVtolLandSet(&mLandData->fsmLandStatus);
}
float VtolLandFSM::BoundVelocityDown(float velocity_down)
{
velocity_down = boundf(velocity_down, MIN_LANDRATE, MAX_LANDRATE);
if (mLandData->flLowAltitude) {
velocity_down *= LOW_ALT_DESCENT_REDUCTION_FACTOR;
}
mLandData->fsmLandStatus.targetDescentRate = velocity_down;
if (mLandData->flAltitudeHold) {
return 0.0f;
} else {
return velocity_down;
}
}
void VtolLandFSM::assessAltitude(void)
{
float positionDown;
PositionStateDownGet(&positionDown);
float takeOffDown = 0.0f;
if (mLandData->takeOffLocation.Status == TAKEOFFLOCATION_STATUS_VALID) {
takeOffDown = mLandData->takeOffLocation.Down;
}
float positionDownRelativeToTakeoff = positionDown - takeOffDown;
if (positionDownRelativeToTakeoff < LANDING_SLOWDOWN_HEIGHT) {
mLandData->flLowAltitude = false;
} else {
mLandData->flLowAltitude = true;
}
}
// FSM Setup and Run method implementation
// State: INACTIVE
void VtolLandFSM::setup_inactive(void)
{
// Re-initialise local variables
mLandData->flZeroStabiHorizontal = false;
mLandData->flConstrainThrust = false;
}
// State: INIT ALTHOLD
void VtolLandFSM::setup_init_althold(void)
{
setStateTimeout(TIMEOUT_INIT_ALTHOLD);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->fsmLandStatus.targetDescentRate = BoundVelocityDown(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN]);
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = true;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_init_althold(uint8_t flTimeout)
{
if (flTimeout) {
mLandData->flAltitudeHold = false;
setState(STATUSVTOLLAND_STATE_WTGFORDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: WAITING FOR DESCENT RATE
void VtolLandFSM::setup_wtg_for_descentrate(void)
{
setStateTimeout(TIMEOUT_WTG_FOR_DESCENTRATE);
// get target descent velocity
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->flConstrainThrust = false;
mLandData->flAltitudeHold = false;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_descentrate(uint8_t flTimeout)
{
// Look at current actual thrust...are we already shutdown??
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// We don't expect PID to get exactly the target descent rate, so have a lower
// water mark but need to see 5 observations to be confident that we have semi-stable
// descent achieved
// we need to see velocity down within a range of control before we proceed, without which we
// really don't have confidence to allow later states to run.
if (velocityState.Down > (LANDRATE_LOWLIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate) &&
velocityState.Down < (LANDRATE_HILIMIT_FACTOR * mLandData->fsmLandStatus.targetDescentRate)) {
if (mLandData->observationCount++ > WTG_FOR_DESCENTRATE_COUNT_LIMIT) {
setState(STATUSVTOLLAND_STATE_ATDESCENTRATE, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
return;
}
}
if (flTimeout) {
setState(STATUSVTOLLAND_STATE_ABORT, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// State: AT DESCENT RATE
void VtolLandFSM::setup_at_descentrate(void)
{
setStateTimeout(TIMEOUT_AT_DESCENTRATE);
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.averageDescentRate = MIN_LANDRATE;
mLandData->fsmLandStatus.averageDescentThrust = vtolPathFollowerSettings->ThrustLimits.Neutral;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_at_descentrate(uint8_t flTimeout)
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->sum1 += velocityState.Down;
mLandData->sum2 += stabDesired.Thrust;
mLandData->observationCount++;
if (flTimeout) {
mLandData->fsmLandStatus.averageDescentRate = boundf((mLandData->sum1 / (float)(mLandData->observationCount)), 0.5f * MIN_LANDRATE, 1.5f * MAX_LANDRATE);
mLandData->fsmLandStatus.averageDescentThrust = boundf((mLandData->sum2 / (float)(mLandData->observationCount)), vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
// We need to calculate a neutral limit to use later to constrain upper thrust range during states where we are close to the ground
// As our battery gets flat, ThrustLimits.Neutral needs to constrain us too much and we get too fast a descent rate. We can
// detect this by the fact that the descent rate will exceed the target and the required thrust will exceed the neutral value
mLandData->fsmLandStatus.calculatedNeutralThrust = mLandData->fsmLandStatus.averageDescentRate / mLandData->fsmLandStatus.targetDescentRate * mLandData->fsmLandStatus.averageDescentThrust;
mLandData->fsmLandStatus.calculatedNeutralThrust = boundf(mLandData->fsmLandStatus.calculatedNeutralThrust, vtolPathFollowerSettings->ThrustLimits.Neutral, vtolPathFollowerSettings->ThrustLimits.Max);
setState(STATUSVTOLLAND_STATE_WTGFORGROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_DESCENTRATEOK);
}
}
// State: WAITING FOR GROUND EFFECT
void VtolLandFSM::setup_wtg_for_groundeffect(void)
{
// No timeout
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->observation2Count = 0;
mLandData->sum1 = 0.0f;
mLandData->sum2 = 0.0f;
mLandData->flConstrainThrust = false;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
mLandData->boundThrustMin = vtolPathFollowerSettings->ThrustLimits.Min;
mLandData->boundThrustMax = vtolPathFollowerSettings->ThrustLimits.Max;
}
void VtolLandFSM::run_wtg_for_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
// detect material downrating in thrust for 1 second.
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
AccelStateData accelState;
AccelStateGet(&accelState);
// +ve 9.8 expected
float g_e;
HomeLocationg_eGet(&g_e);
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
// detect bounce
uint8_t flBounce = (velocityState.Down < BOUNCE_VELOCITY_TRIGGER_LIMIT);
if (flBounce) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = velocityState.Down;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceVelocity = 0.0f;
}
// invert sign of accel to the standard convention of down is +ve and subtract the gravity to get
// a relative acceleration term.
float bounceAccel = -accelState.z - g_e;
uint8_t flBounceAccel = (bounceAccel < BOUNCE_ACCELERATION_TRIGGER_LIMIT);
if (flBounceAccel) {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = bounceAccel;
} else {
mLandData->fsmLandStatus.WtgForGroundEffect.BounceAccel = 0.0f;
}
if (flBounce) { // || flBounceAccel) { // accel trigger can occur due to vibration and is too sensitive
mLandData->observation2Count++;
if (mLandData->observation2Count > BOUNCE_TRIGGER_COUNT) {
setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, (flBounce ? STATUSVTOLLAND_STATEEXITREASON_BOUNCEVELOCITY : STATUSVTOLLAND_STATEEXITREASON_BOUNCEACCEL));
return;
}
} else {
mLandData->observation2Count = 0;
}
// detect low descent rate
uint8_t flDescentRateLow = (velocityState.Down < (GROUNDEFFECT_SLOWDOWN_FACTOR * mLandData->fsmLandStatus.averageDescentRate));
if (flDescentRateLow) {
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
mLandData->observationCount++;
if (mLandData->observationCount > GROUNDEFFECT_SLOWDOWN_COUNT) {
#ifndef DEBUG_GROUNDIMPACT
setState(STATUSVTOLLAND_STATE_GROUNDEFFECT, STATUSVTOLLAND_STATEEXITREASON_LOWDESCENTRATE);
#endif
return;
}
} else {
mLandData->observationCount = 0;
}
updateVtolLandFSMStatus();
}
// STATE: GROUNDEFFET
void VtolLandFSM::setup_groundeffect(void)
{
setStateTimeout(TIMEOUT_GROUNDEFFECT);
mLandData->flZeroStabiHorizontal = false;
PositionStateData positionState;
PositionStateGet(&positionState);
mLandData->expectedLandPositionNorth = positionState.North;
mLandData->expectedLandPositionEast = positionState.East;
mLandData->flConstrainThrust = false;
// now that we have ground effect limit max thrust to neutral
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_groundeffect(__attribute__((unused)) uint8_t flTimeout)
{
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f) {
setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
return;
}
// Stay in this state until we get a low altitude flag.
if (mLandData->flLowAltitude == false) {
// worst case scenario is that we land and the pid brings thrust down to zero.
return;
}
// detect broad sideways drift. If for some reason we have a hard landing that the bounce detection misses, this will kick in
PositionStateData positionState;
PositionStateGet(&positionState);
float north_error = mLandData->expectedLandPositionNorth - positionState.North;
float east_error = mLandData->expectedLandPositionEast - positionState.East;
float positionError = sqrtf(north_error * north_error + east_error * east_error);
if (positionError > 1.5f) {
setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_POSITIONERROR);
return;
}
if (flTimeout) {
setState(STATUSVTOLLAND_STATE_THRUSTDOWN, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTDOWN
void VtolLandFSM::setup_thrustdown(void)
{
setStateTimeout(TIMEOUT_THRUSTDOWN);
mLandData->flZeroStabiHorizontal = true;
mLandData->flConstrainThrust = true;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
mLandData->thrustLimit = stabDesired.Thrust;
mLandData->sum1 = stabDesired.Thrust / (float)TIMEOUT_THRUSTDOWN;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = mLandData->fsmLandStatus.calculatedNeutralThrust;
}
void VtolLandFSM::run_thrustdown(__attribute__((unused)) uint8_t flTimeout)
{
// reduce thrust setpoint step by step
mLandData->thrustLimit -= mLandData->sum1;
StabilizationDesiredData stabDesired;
StabilizationDesiredGet(&stabDesired);
if (stabDesired.Thrust < 0.0f || mLandData->thrustLimit < 0.0f) {
setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_ZEROTHRUST);
}
if (flTimeout) {
setState(STATUSVTOLLAND_STATE_THRUSTOFF, STATUSVTOLLAND_STATEEXITREASON_TIMEOUT);
}
}
// STATE: THRUSTOFF
void VtolLandFSM::setup_thrustoff(void)
{
mLandData->thrustLimit = -1.0f;
mLandData->flConstrainThrust = true;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_thrustoff(__attribute__((unused)) uint8_t flTimeout)
{
setState(STATUSVTOLLAND_STATE_DISARMED, STATUSVTOLLAND_STATEEXITREASON_NONE);
}
// STATE: DISARMED
void VtolLandFSM::setup_disarmed(void)
{
// nothing to do
mLandData->flConstrainThrust = false;
mLandData->flZeroStabiHorizontal = false;
mLandData->observationCount = 0;
mLandData->boundThrustMin = -0.1f;
mLandData->boundThrustMax = 0.0f;
}
void VtolLandFSM::run_disarmed(__attribute__((unused)) uint8_t flTimeout)
{
#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/PathPlanner/pathplanner.c
View File

@ -49,6 +49,7 @@
#include <sanitycheck.h>
#include <vtolpathfollowersettings.h>
#include <statusvtolautotakeoff.h>
#include <statusvtolland.h>
#include <manualcontrolcommand.h>
// Private constants
@ -79,6 +80,7 @@ static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired);
static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
static void planner_setup_pathdesired(PathDesiredData *pathDesired, uint8_t);
@ -130,6 +132,8 @@ int32_t PathPlannerInitialize()
VelocityStateInitialize();
WaypointInitialize();
WaypointActiveInitialize();
StatusVtolAutoTakeoffInitialize();
StatusVtolLandInitialize();
pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
@ -142,7 +146,7 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
uint8_t TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
@ -337,6 +341,9 @@ void updatePathDesired()
case PATHACTION_MODE_AUTOTAKEOFF:
planner_setup_pathdesired_takeoff(&pathDesired);
break;
case PATHACTION_MODE_LAND:
planner_setup_pathdesired_land(&pathDesired);
break;
default:
planner_setup_pathdesired(&pathDesired, autotakeoff);
break;
@ -510,6 +517,32 @@ static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
pathDesired->UID = waypointActive.Index;
}
static void planner_setup_pathdesired_land(PathDesiredData *pathDesired)
{
PositionStateData positionState;
PositionStateGet(&positionState);
float velocity_down;
FlightModeSettingsLandingVelocityGet(&velocity_down);
pathDesired->Start.North = positionState.North;
pathDesired->Start.East = positionState.East;
pathDesired->Start.Down = positionState.Down;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_NORTH] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_EAST] = 0.0f;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_VELOCITYVECTOR_DOWN] = velocity_down;
pathDesired->End.North = positionState.North;
pathDesired->End.East = positionState.East;
pathDesired->End.Down = positionState.Down;
pathDesired->StartingVelocity = 0.0f;
pathDesired->EndingVelocity = 0.0f;
pathDesired->Mode = PATHDESIRED_MODE_LAND;
pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_LAND_OPTIONS] = (float)PATHDESIRED_MODEPARAMETER_LAND_OPTION_HORIZONTAL_PH;
}
// helper function to go to a specific waypoint
static void setWaypoint(uint16_t num)

3
flight/modules/Receiver/receiver.c
View File

@ -612,6 +612,9 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL:
group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL;
break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break;

2
flight/modules/Stabilization/innerloop.c
View File

@ -350,7 +350,7 @@ static void stabilizationInnerloopTask()
}
{
uint8_t armed;
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);

2
flight/modules/Stabilization/outerloop.c
View File

@ -264,7 +264,7 @@ static void stabilizationOuterloopTask()
RateDesiredSet(&rateDesired);
{
uint8_t armed;
FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed);
float throttleDesired;
ManualControlCommandThrottleGet(&throttleDesired);

2
flight/modules/System/systemmod.c
View File

@ -101,7 +101,9 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C
static bool mallocFailed;
static HwSettingsData bootHwSettings;
static FrameType_t bootFrameType;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
static struct PIOS_FLASHFS_Stats fsStats;
#endif
// Private functions
static void objectUpdatedCb(UAVObjEvent *ev);

2
flight/modules/Telemetry/telemetry.c
View File

@ -888,7 +888,7 @@ static void updateSettings(channelContext *channel)
if (port) {
// Retrieve settings
uint8_t speed;
HwSettingsTelemetrySpeedOptions speed;
HwSettingsTelemetrySpeedGet(&speed);
// Set port speed

365
flight/pios/common/pios_srxl.c Normal file
View File

@ -0,0 +1,365 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
* @brief Code to read Multiplex SRXL receiver serial stream
* @{
*
* @file pios_srxl.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Code to read Multiplex SRXL receiver serial stream
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "pios.h"
#ifdef PIOS_INCLUDE_SRXL
#include "pios_srxl_priv.h"
// #define PIOS_INSTRUMENT_MODULE
#include <pios_instrumentation_helper.h>
PERF_DEFINE_COUNTER(crcFailureCount);
PERF_DEFINE_COUNTER(failsafeCount);
PERF_DEFINE_COUNTER(successfulCount);
PERF_DEFINE_COUNTER(messageUnrollTimer);
PERF_DEFINE_COUNTER(messageReceiveRate);
PERF_DEFINE_COUNTER(receivedBytesCount);
PERF_DEFINE_COUNTER(frameStartCount);
PERF_DEFINE_COUNTER(frameAbortCount);
PERF_DEFINE_COUNTER(completeMessageCount);
/* Forward Declarations */
static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel);
static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
uint16_t *headroom,
bool *need_yield);
static void PIOS_SRXL_Supervisor(uint32_t srxl_id);
/* Local Variables */
const struct pios_rcvr_driver pios_srxl_rcvr_driver = {
.read = PIOS_SRXL_Get,
};
enum pios_srxl_dev_magic {
PIOS_SRXL_DEV_MAGIC = 0x55545970,
};
struct pios_srxl_state {
uint16_t channel_data[PIOS_SRXL_NUM_INPUTS];
uint8_t received_data[SRXL_FRAME_LENGTH];
uint8_t receive_timer;
uint8_t failsafe_timer;
uint8_t frame_found;
uint8_t byte_count;
uint8_t data_bytes;
};
struct pios_srxl_dev {
enum pios_srxl_dev_magic magic;
struct pios_srxl_state state;
};
/* Allocate S.Bus device descriptor */
#if defined(PIOS_INCLUDE_FREERTOS)
static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
{
struct pios_srxl_dev *srxl_dev;
srxl_dev = (struct pios_srxl_dev *)pios_malloc(sizeof(*srxl_dev));
if (!srxl_dev) {
return NULL;
}
srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
return srxl_dev;
}
#else
static struct pios_srxl_dev pios_srxl_devs[PIOS_SRXL_MAX_DEVS];
static uint8_t pios_srxl_num_devs;
static struct pios_srxl_dev *PIOS_SRXL_Alloc(void)
{
struct pios_srxl_dev *srxl_dev;
if (pios_srxl_num_devs >= PIOS_SRXL_MAX_DEVS) {
return NULL;
}
srxl_dev = &pios_srxl_devs[pios_srxl_num_devs++];
srxl_dev->magic = PIOS_SRXL_DEV_MAGIC;
return srxl_dev;
}
#endif /* if defined(PIOS_INCLUDE_FREERTOS) */
/* Validate SRXL device descriptor */
static bool PIOS_SRXL_Validate(struct pios_srxl_dev *srxl_dev)
{
return srxl_dev->magic == PIOS_SRXL_DEV_MAGIC;
}
/* Reset channels in case of lost signal or explicit failsafe receiver flag */
static void PIOS_SRXL_ResetChannels(struct pios_srxl_state *state)
{
for (int i = 0; i < PIOS_SRXL_NUM_INPUTS; i++) {
state->channel_data[i] = PIOS_RCVR_TIMEOUT;
}
}
/* Reset SRXL receiver state */
static void PIOS_SRXL_ResetState(struct pios_srxl_state *state)
{
state->receive_timer = 0;
state->failsafe_timer = 0;
state->frame_found = 0;
state->data_bytes = 0;
PIOS_SRXL_ResetChannels(state);
}
/* Initialize SRXL receiver interface */
int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
const struct pios_com_driver *driver,
uint32_t lower_id)
{
PIOS_DEBUG_Assert(srxl_id);
PIOS_DEBUG_Assert(driver);
struct pios_srxl_dev *srxl_dev;
srxl_dev = (struct pios_srxl_dev *)PIOS_SRXL_Alloc();
if (!srxl_dev) {
goto out_fail;
}
PIOS_SRXL_ResetState(&(srxl_dev->state));
*srxl_id = (uint32_t)srxl_dev;
/* Set comm driver callback */
(driver->bind_rx_cb)(lower_id, PIOS_SRXL_RxInCallback, *srxl_id);
if (!PIOS_RTC_RegisterTickCallback(PIOS_SRXL_Supervisor, *srxl_id)) {
PIOS_DEBUG_Assert(0);
}
PERF_INIT_COUNTER(crcFailureCount, 0x5551);
PERF_INIT_COUNTER(failsafeCount, 0x5552);
PERF_INIT_COUNTER(successfulCount, 0x5553);
PERF_INIT_COUNTER(messageUnrollTimer, 0x5554);
PERF_INIT_COUNTER(messageReceiveRate, 0x5555);
PERF_INIT_COUNTER(receivedBytesCount, 0x5556);
PERF_INIT_COUNTER(frameStartCount, 0x5557);
PERF_INIT_COUNTER(frameAbortCount, 0x5558);
PERF_INIT_COUNTER(completeMessageCount, 0x5559);
return 0;
out_fail:
return -1;
}
/**
* Get the value of an input channel
* \param[in] channel Number of the channel desired (zero based)
* \output PIOS_RCVR_INVALID channel not available
* \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver
* \output >=0 channel value
*/
static int32_t PIOS_SRXL_Get(uint32_t rcvr_id, uint8_t channel)
{
struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)rcvr_id;
if (!PIOS_SRXL_Validate(srxl_dev)) {
return PIOS_RCVR_INVALID;
}
/* return error if channel is not available */
if (channel >= PIOS_SRXL_NUM_INPUTS) {
return PIOS_RCVR_INVALID;
}
return srxl_dev->state.channel_data[channel];
}
static void PIOS_SRXL_UnrollChannels(struct pios_srxl_state *state)
{
PERF_TIMED_SECTION_START(messageUnrollTimer);
uint8_t *received_data = state->received_data;
uint8_t channel;
uint16_t channel_value;
for (channel = 0; channel < (state->data_bytes / 2); channel++) {
channel_value = ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2)]) << 8;
channel_value = channel_value + ((uint16_t)received_data[SRXL_HEADER_LENGTH + (channel * 2) + 1]);
state->channel_data[channel] = (800 + ((channel_value * 1400) >> 12));
}
PERF_TIMED_SECTION_END(messageUnrollTimer);
}
static bool PIOS_SRXL_Validate_Checksum(struct pios_srxl_state *state)
{
// Check the CRC16 checksum. The provided checksum is immediately after the channel data.
// All data including start byte and version byte is included in crc calculation.
uint8_t i = 0;
uint16_t crc = 0;
for (i = 0; i < SRXL_HEADER_LENGTH + state->data_bytes; i++) {
crc = crc ^ (int16_t)state->received_data[i] << 8;
uint8_t j = 0;
for (j = 0; j < 8; j++) {
if (crc & 0x8000) {
crc = crc << 1 ^ 0x1021;
} else {
crc = crc << 1;
}
}
}
uint16_t checksum = (((uint16_t)(state->received_data[i] << 8) |
(uint16_t)state->received_data[i + 1]));
return crc == checksum;
}
/* Update decoder state processing input byte from the SRXL stream */
static void PIOS_SRXL_UpdateState(struct pios_srxl_state *state, uint8_t b)
{
/* should not process any data until new frame is found */
if (!state->frame_found) {
return;
}
if (state->byte_count < (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
if (state->byte_count == 0) {
// Set up the length of the channel data according to version received
PERF_INCREMENT_VALUE(frameStartCount);
if (b == SRXL_V1_HEADER) {
state->data_bytes = SRXL_V1_CHANNEL_DATA_BYTES;
} else if (b == SRXL_V2_HEADER) {
state->data_bytes = SRXL_V2_CHANNEL_DATA_BYTES;
} else {
/* discard the whole frame if the 1st byte is not correct */
state->frame_found = 0;
PERF_INCREMENT_VALUE(frameAbortCount);
return;
}
}
/* store next byte */
state->received_data[state->byte_count] = b;
state->byte_count++;
if (state->byte_count == (SRXL_HEADER_LENGTH + state->data_bytes + SRXL_CHECKSUM_LENGTH)) {
PERF_INCREMENT_VALUE(completeMessageCount);
// We have a complete message, lets decode it
if (PIOS_SRXL_Validate_Checksum(state)) {
/* data looking good */
PIOS_SRXL_UnrollChannels(state);
state->failsafe_timer = 0;
PERF_INCREMENT_VALUE(successfulCount);
} else {
/* discard whole frame */
PERF_INCREMENT_VALUE(crcFailureCount);
}
/* prepare for the next frame */
state->frame_found = 0;
PERF_MEASURE_PERIOD(messageReceiveRate);
}
}
}
/* Comm byte received callback */
static uint16_t PIOS_SRXL_RxInCallback(uint32_t context,
uint8_t *buf,
uint16_t buf_len,
uint16_t *headroom,
bool *need_yield)
{
struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)context;
bool valid = PIOS_SRXL_Validate(srxl_dev);
PIOS_Assert(valid);
struct pios_srxl_state *state = &(srxl_dev->state);
/* process byte(s) and clear receive timer */
for (uint8_t i = 0; i < buf_len; i++) {
PIOS_SRXL_UpdateState(state, buf[i]);
state->receive_timer = 0;
PERF_INCREMENT_VALUE(receivedBytesCount);
}
/* Always signal that we can accept another byte */
if (headroom) {
*headroom = SRXL_FRAME_LENGTH;
}
/* We never need a yield */
*need_yield = false;
/* Always indicate that all bytes were consumed */
return buf_len;
}
/**
* Input data supervisor is called periodically and provides
* two functions: frame syncing and failsafe triggering.
*
* Multiplex SRXL frames come at 14ms (FastResponse ON) or 21ms (FastResponse OFF)
* rate at 115200bps.
* RTC timer is running at 625Hz (1.6ms). So with divider 2 it gives
* 3.2ms pause between frames which is good for both SRXL frame rates.
*
* Data receive function must clear the receive_timer to confirm new
* data reception. If no new data received in 100ms, we must call the
* failsafe function which clears all channels.
*/
static void PIOS_SRXL_Supervisor(uint32_t srxl_id)
{
struct pios_srxl_dev *srxl_dev = (struct pios_srxl_dev *)srxl_id;
bool valid = PIOS_SRXL_Validate(srxl_dev);
PIOS_Assert(valid);
struct pios_srxl_state *state = &(srxl_dev->state);
/* waiting for new frame if no bytes were received in 6.4ms */
if (++state->receive_timer > 4) {
state->frame_found = 1;
state->byte_count = 0;
state->receive_timer = 0;
}
/* activate failsafe if no frames have arrived in 102.4ms */
if (++state->failsafe_timer > 64) {
PIOS_SRXL_ResetChannels(state);
state->failsafe_timer = 0;
PERF_INCREMENT_VALUE(failsafeCount);
}
}
#endif /* PIOS_INCLUDE_SRXL */
/**
* @}
* @}
*/

23
flight/pios/inc/pios_instrumentation.h
View File

@ -127,6 +127,29 @@ static inline void PIOS_Instrumentation_TrackPeriod(pios_counter_t counter_handl
counter->lastUpdateTS = PIOS_DELAY_GetRaw();
}
/**
* Increment a counter with a value
* @param counter_handle handle of the counter to update @see PIOS_Instrumentation_SearchCounter @see PIOS_Instrumentation_CreateCounter
* @param increment the value to increment counter with.
*/
static inline void PIOS_Instrumentation_incrementCounter(pios_counter_t counter_handle, int32_t increment)
{
PIOS_Assert(pios_instrumentation_perf_counters && counter_handle);
vPortEnterCritical();
pios_perf_counter_t *counter = (pios_perf_counter_t *)counter_handle;
counter->value += increment;
counter->max--;
if (counter->value > counter->max) {
counter->max = counter->value;
}
counter->min++;
if (counter->value < counter->min) {
counter->min = counter->value;
}
counter->lastUpdateTS = PIOS_DELAY_GetRaw();
vPortExitCritical();
}
/**
* Initialize the Instrumentation infrastructure
* @param maxCounters maximum number of allowed counters

4
flight/pios/inc/pios_instrumentation_helper.h
View File

@ -98,6 +98,8 @@
#define PERF_TIMED_SECTION_END(x) PIOS_Instrumentation_TimeEnd(x)
#define PERF_MEASURE_PERIOD(x) PIOS_Instrumentation_TrackPeriod(x)
#define PERF_TRACK_VALUE(x, y) PIOS_Instrumentation_updateCounter(x, y)
#define PERF_INCREMENT_VALUE(x) PIOS_Instrumentation_incrementCounter(x, 1)
#define PERF_DECREMENT_VALUE(x) PIOS_Instrumentation_incrementCounter(x, -1)
#else
@ -107,5 +109,7 @@
#define PERF_TIMED_SECTION_END(x)
#define PERF_MEASURE_PERIOD(x)
#define PERF_TRACK_VALUE(x, y)
#define PERF_INCREMENT_VALUE(x)
#define PERF_DECREMENT_VALUE(x)
#endif /* PIOS_INCLUDE_INSTRUMENTATION */
#endif /* PIOS_INSTRUMENTATION_HELPER_H */

43
flight/pios/inc/pios_srxl.h Normal file
View File

@ -0,0 +1,43 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
* @brief Code to read Multiplex SRXL receiver serial stream
* @{
*
* @file pios_srxl.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Code to read Multiplex SRXL receiver serial stream
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_SRXL_H
#define PIOS_SRXL_H
/* Global Types */
/* Public Functions */
#endif /* PIOS_SRXL_H */
/**
* @}
* @}
*/

95
flight/pios/inc/pios_srxl_priv.h Normal file
View File

@ -0,0 +1,95 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_SRXL Multiplex SRXL receiver functions
* @brief Code to read Multiplex SRXL receiver serial stream
* @{
*
* @file pios_srxl_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
* @brief Code to read Multiplex SRXL receiver serial stream
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef PIOS_SRXL_PRIV_H
#define PIOS_SRXL_PRIV_H
#include <pios.h>
#include <pios_stm32.h>
#include <pios_usart_priv.h>
/*
* Multiplex SRXL serial port settings:
* 115200bps inverted serial stream, 8 bits, no parity, 1 stop bits
* frame period is 14ms (FastResponse ON) or 21ms (FastResponse OFF)
*
* Frame structure:
* 1 byte - start and version
* 0xa1 (v1 12-channels)
* 0xa2 (v2 16-channels)
* 24/32 bytes - channel data (4 + 12 bit/channel, 12/16 channels, MSB first)
* 16 bits per channel. 4 first reserved/not used. 12 bits channel data in
* 4095 steps, 0x000(800µs) - 0x800(1500µs) - 0xfff(2200µs)
* 2 bytes checksum (calculated over all bytes including start and version)
*
* Checksum calculation:
* u16 CRC16(u16 crc, u8 value) {
* u8 i;
* crc = crc ^ (s16)value << 8;
* for(i = 0; i < 8; i++) {
* if(crc & 0x8000) {
* crc = crc << 1 ^ 0x1021;
* } else {
* crc = crc << 1;
* }
* }
* return crc;
* }
*/
#define SRXL_V1_HEADER 0xa1
#define SRXL_V2_HEADER 0xa2
#define SRXL_HEADER_LENGTH 1
#define SRXL_CHECKSUM_LENGTH 2
#define SRXL_V1_CHANNEL_DATA_BYTES (12 * 2)
#define SRXL_V2_CHANNEL_DATA_BYTES (16 * 2)
#define SRXL_FRAME_LENGTH (SRXL_HEADER_LENGTH + SRXL_V2_CHANNEL_DATA_BYTES + SRXL_CHECKSUM_LENGTH)
/*
* Multiplex SRXL protocol provides 16 proportional channels.
* Do not change unless driver code is updated accordingly.
*/
#if (PIOS_SRXL_NUM_INPUTS != 16)
#error "Multiplex SRXL protocol provides 16 proportional channels."
#endif
extern const struct pios_rcvr_driver pios_srxl_rcvr_driver;
extern int32_t PIOS_SRXL_Init(uint32_t *srxl_id,
const struct pios_com_driver *driver,
uint32_t lower_id);
#endif /* PIOS_SRXL_PRIV_H */
/**
* @}
* @}
*/

4
flight/pios/pios.h
View File

@ -228,6 +228,10 @@ extern "C" {
#include <pios_sbus.h>
#endif
#ifdef PIOS_INCLUDE_SRXL
#include <pios_srxl.h>
#endif
/* PIOS abstract receiver interface */
#ifdef PIOS_INCLUDE_RCVR
#include <pios_rcvr.h>

1
flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc
View File

@ -20,6 +20,7 @@
# (all architectures)
UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired

49
flight/targets/boards/revolution/board_hw_defs.c
View File

@ -899,7 +899,6 @@ static const struct pios_sbus_cfg pios_sbus_cfg = {
.gpio_clk_periph = RCC_AHB1Periph_GPIOC,
};
#ifdef PIOS_INCLUDE_COM_FLEXI
/*
* FLEXI PORT
@ -1007,6 +1006,54 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
#endif /* PIOS_INCLUDE_DSM */
#if defined(PIOS_INCLUDE_SRXL)
/*
* SRXL USART
*/
#include <pios_srxl_priv.h>
static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = {
.regs = USART3,
.remap = GPIO_AF_USART3,
.init = {
.USART_BaudRate = 115200,
.USART_WordLength = USART_WordLength_8b,
.USART_Parity = USART_Parity_No,
.USART_StopBits = USART_StopBits_1,
.USART_HardwareFlowControl = USART_HardwareFlowControl_None,
.USART_Mode = USART_Mode_Rx,
},
.irq = {
.init = {
.NVIC_IRQChannel = USART3_IRQn,
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH,
.NVIC_IRQChannelSubPriority = 0,
.NVIC_IRQChannelCmd = ENABLE,
},
},
.rx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_11,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_AF,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
.tx = {
.gpio = GPIOB,
.init = {
.GPIO_Pin = GPIO_Pin_10,
.GPIO_Speed = GPIO_Speed_2MHz,
.GPIO_Mode = GPIO_Mode_OUT,
.GPIO_OType = GPIO_OType_PP,
.GPIO_PuPd = GPIO_PuPd_UP
},
},
};
#endif /* PIOS_INCLUDE_SRXL */
/*
* HK OSD
*/

1
flight/targets/boards/revolution/firmware/inc/pios_config.h
View File

@ -103,6 +103,7 @@
/* #define PIOS_INCLUDE_PPM_FLEXI */
#define PIOS_INCLUDE_DSM
#define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_SRXL
#define PIOS_INCLUDE_GCSRCVR
#define PIOS_INCLUDE_OPLINKRCVR

22
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -499,6 +499,27 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RM_FLEXIPORT_OSDHK:
PIOS_Board_configure_com(&pios_usart_hkosd_flexi_cfg, PIOS_COM_HKOSD_RX_BUF_LEN, PIOS_COM_HKOSD_TX_BUF_LEN, &pios_usart_com_driver, &pios_com_hkosd_id);
break;
case HWSETTINGS_RM_FLEXIPORT_SRXL:
#if defined(PIOS_INCLUDE_SRXL)
{
uint32_t pios_usart_srxl_id;
if (PIOS_USART_Init(&pios_usart_srxl_id, &pios_usart_srxl_flexi_cfg)) {
PIOS_Assert(0);
}
uint32_t pios_srxl_id;
if (PIOS_SRXL_Init(&pios_srxl_id, &pios_usart_com_driver, pios_usart_srxl_id)) {
PIOS_Assert(0);
}
uint32_t pios_srxl_rcvr_id;
if (PIOS_RCVR_Init(&pios_srxl_rcvr_id, &pios_srxl_rcvr_driver, pios_srxl_id)) {
PIOS_Assert(0);
}
pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL] = pios_srxl_rcvr_id;
}
#endif
break;
} /* hwsettings_rm_flexiport */
/* Moved this here to allow binding on flexiport */
@ -868,7 +889,6 @@ void PIOS_Board_Init(void)
break;
}
#if defined(PIOS_INCLUDE_GCSRCVR)
GCSReceiverInitialize();
uint32_t pios_gcsrcvr_id;

6
flight/targets/boards/revolution/pios_board.h
View File

@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler;
#define PIOS_SBUS_MAX_DEVS 1
#define PIOS_SBUS_NUM_INPUTS (16 + 2)
// -------------------------
// Receiver Multiplex SRXL input
// -------------------------
#define PIOS_SRXL_MAX_DEVS 1
#define PIOS_SRXL_NUM_INPUTS 16
// -------------------------
// Receiver DSM input
// -------------------------

11
flight/targets/boards/simposix/firmware/pios_board.c
View File

@ -152,7 +152,7 @@ void PIOS_Board_Init(void)
/* Configure IO ports */
/* Configure Telemetry port */
uint8_t hwsettings_rv_telemetryport;
HwSettingsRV_TelemetryPortOptions hwsettings_rv_telemetryport;
HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport);
switch (hwsettings_rv_telemetryport) {
@ -164,10 +164,12 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RV_TELEMETRYPORT_COMAUX:
PIOS_Board_configure_com(&pios_udp_telem_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id);
break;
default:
break;
} /* hwsettings_rv_telemetryport */
/* Configure GPS port */
uint8_t hwsettings_rv_gpsport;
HwSettingsRV_GPSPortOptions hwsettings_rv_gpsport;
HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport);
switch (hwsettings_rv_gpsport) {
case HWSETTINGS_RV_GPSPORT_DISABLED:
@ -184,10 +186,12 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RV_GPSPORT_COMAUX:
PIOS_Board_configure_com(&pios_udp_gps_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id);
break;
default:
break;
} /* hwsettings_rv_gpsport */
/* Configure AUXPort */
uint8_t hwsettings_rv_auxport;
HwSettingsRV_AuxPortOptions hwsettings_rv_auxport;
HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport);
switch (hwsettings_rv_auxport) {
@ -201,6 +205,7 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RV_AUXPORT_COMAUX:
PIOS_Board_configure_com(&pios_udp_aux_cfg, PIOS_COM_AUX_RX_BUF_LEN, PIOS_COM_AUX_TX_BUF_LEN, &pios_udp_com_driver, &pios_com_aux_id);
break;
default:
break;
} /* hwsettings_rv_auxport */
}

4
flight/uavobjects/inc/uavobject.h.template
View File

@ -52,6 +52,8 @@ int32_t $(NAME)Initialize();
UAVObjHandle $(NAME)Handle();
void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId);
$(DATAFIELDINFO)
$(DATASTRUCTURES)
/*
* Packed Object data (unaligned).
@ -86,8 +88,6 @@ static inline int32_t $(NAME)GetMetadata(UAVObjMetadata *dataOut) { return UAVOb
static inline int32_t $(NAME)SetMetadata(const UAVObjMetadata *dataIn) { return UAVObjSetMetadata($(NAME)Handle(), dataIn); }
static inline int8_t $(NAME)ReadOnly() { return UAVObjReadOnly($(NAME)Handle()); }
$(DATAFIELDINFO)
/* Set/Get functions */
$(SETGETFIELDSEXTERN)

3
ground/openpilotgcs/src/plugins/config/inputchannelform.cpp
View File

@ -158,6 +158,9 @@ void InputChannelForm::groupUpdated()
case ManualControlSettings::CHANNELGROUPS_SBUS:
count = 18;
break;
case ManualControlSettings::CHANNELGROUPS_SRXL:
count = 16;
break;
case ManualControlSettings::CHANNELGROUPS_GCS:
count = GCSReceiver::CHANNEL_NUMELEM;
break;

56
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp
View File

@ -68,8 +68,8 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templat
// Write the flight object inialization files
flightInitTemplate.replace(QString("$(OBJINC)"), objInc);
flightInitTemplate.replace(QString("$(OBJINIT)"), flightObjInit);
bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.c",
flightInitTemplate);
bool res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/uavobjectsinit.c",
flightInitTemplate);
if (!res) {
cout << "Error: Could not write flight object init file" << endl;
return false;
@ -77,8 +77,8 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templat
// Write the flight object initialization header
flightInitIncludeTemplate.replace(QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc));
res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.h",
flightInitIncludeTemplate);
res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/uavobjectsinit.h",
flightInitIncludeTemplate);
if (!res) {
cout << "Error: Could not write flight object init header file" << endl;
return false;
@ -87,8 +87,8 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templat
// Write the flight object Makefile
flightMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames);
flightMakeTemplate.replace(QString("$(UAVOBJNAMES)"), objNames);
res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/Makefile.inc",
flightMakeTemplate);
res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/Makefile.inc",
flightMakeTemplate);
if (!res) {
cout << "Error: Could not write flight Makefile" << endl;
return false;
@ -115,13 +115,25 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
replaceCommonTags(outInclude, info);
replaceCommonTags(outCode, info);
// Use the appropriate typedef for enums where we find them. Set up
// that StringList here for use below.
//
QStringList typeList;
for (int n = 0; n < info->fields.length(); ++n) {
if (info->fields[n]->type == FIELDTYPE_ENUM) {
typeList << QString("%1%2Options").arg(info->name).arg(info->fields[n]->name);
} else {
typeList << fieldTypeStrC[info->fields[n]->type];
}
}
// Replace the $(DATAFIELDS) tag
QString type;
QString fields;
QString dataStructures;
for (int n = 0; n < info->fields.length(); ++n) {
// Determine type
type = fieldTypeStrC[info->fields[n]->type];
type = typeList[n];
// Append field
// Check if it a named set and creates structures accordingly
if (info->fields[n]->numElements > 1) {
@ -158,7 +170,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
// Only for enum types
if (info->fields[n]->type == FIELDTYPE_ENUM) {
enums.append(QString("\n// Enumeration options for field %1\n").arg(info->fields[n]->name));
enums.append("typedef enum {\n");
enums.append("typedef enum __attribute__ ((__packed__)) {\n");
// Go through each option
QStringList options = info->fields[n]->options;
for (int m = 0; m < options.length(); ++m) {
@ -262,26 +274,26 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
if (info->fields[n]->numElements == 1) {
/* Set */
setgetfields.append(QString("void %2%3Set(%1 *New%3)\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(typeList[n])
.arg(info->name)
.arg(info->fields[n]->name));
setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n")
.arg(info->name)
.arg(info->fields[n]->name)
.arg(fieldTypeStrC[info->fields[n]->type]));
.arg(typeList[n]));
setgetfields.append(QString("}\n"));
/* GET */
setgetfields.append(QString("void %2%3Get(%1 *New%3)\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(typeList[n])
.arg(info->name)
.arg(info->fields[n]->name));
setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n")
.arg(info->name)
.arg(info->fields[n]->name)
.arg(fieldTypeStrC[info->fields[n]->type]));
.arg(typeList[n]));
setgetfields.append(QString("}\n"));
} else {
// When no struct accessor is available for a field array accessor is the default.
@ -300,7 +312,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name)
.arg(info->fields[n]->name)
.arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type]));
.arg(typeList[n]));
setgetfields.append(QString("}\n"));
/* GET */
@ -313,7 +325,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name)
.arg(info->fields[n]->name)
.arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type]));
.arg(typeList[n]));
setgetfields.append(QString("}\n"));
// Append array suffix to array accessors
@ -323,7 +335,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
// array based field accessor
/* SET */
setgetfields.append(QString("void %2%3%4Set( %1 *New%3 )\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(typeList[n])
.arg(info->name)
.arg(info->fields[n]->name)
.arg(suffix));
@ -332,12 +344,12 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name)
.arg(info->fields[n]->name)
.arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type]));
.arg(typeList[n]));
setgetfields.append(QString("}\n"));
/* GET */
setgetfields.append(QString("void %2%3%4Get( %1 *New%3 )\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(typeList[n])
.arg(info->name)
.arg(info->fields[n]->name)
.arg(suffix));
@ -346,7 +358,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name)
.arg(info->fields[n]->name)
.arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type]));
.arg(typeList[n]));
setgetfields.append(QString("}\n"));
}
}
@ -378,14 +390,14 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
}
/* SET */
setgetfieldsextern.append(QString("extern void %2%3%4Set(%1 *New%3);\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(typeList[n])
.arg(info->name)
.arg(info->fields[n]->name)
.arg(suffix));
/* GET */
setgetfieldsextern.append(QString("extern void %2%3%4Get(%1 *New%3);\n")
.arg(fieldTypeStrC[info->fields[n]->type])
.arg(typeList[n])
.arg(info->name)
.arg(info->fields[n]->name)
.arg(suffix));
@ -394,13 +406,13 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern);
// Write the flight code
bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode);
bool res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode);
if (!res) {
cout << "Error: Could not write flight code files" << endl;
return false;
}
res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude);
res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude);
if (!res) {
cout << "Error: Could not write flight include files" << endl;
return false;

6
ground/uavobjgenerator/generators/gcs/uavobjectgeneratorgcs.cpp
View File

@ -62,7 +62,7 @@ bool UAVObjectGeneratorGCS::generate(UAVObjectParser *parser, QString templatepa
// Write the gcs object inialization files
gcsInitTemplate.replace(QString("$(OBJINC)"), objInc);
gcsInitTemplate.replace(QString("$(OBJINIT)"), gcsObjInit);
bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate);
bool res = writeFileIfDifferent(gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate);
if (!res) {
cout << "Error: Could not write output files" << endl;
return false;
@ -371,12 +371,12 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo *info)
outCode.replace(QString("$(INITFIELDS)"), initfields);
// Write the GCS code
bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode);
bool res = writeFileIfDifferent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode);
if (!res) {
cout << "Error: Could not write gcs output files" << endl;
return false;
}
res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude);
res = writeFileIfDifferent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude);
if (!res) {
cout << "Error: Could not write gcs output files" << endl;
return false;

2
ground/uavobjgenerator/generators/generator_io.cpp
View File

@ -75,7 +75,7 @@ bool writeFile(QString name, QString & str)
/**
* Write contents of string to file if the content changes
*/
bool writeFileIfDiffrent(QString name, QString & str)
bool writeFileIfDifferent(QString name, QString & str)
{
if (str == readFile(name, false)) {
return true;

2
ground/uavobjgenerator/generators/generator_io.h
View File

@ -37,6 +37,6 @@
QString readFile(QString name);
QString readFile(QString name);
bool writeFile(QString name, QString & str);
bool writeFileIfDiffrent(QString name, QString & str);
bool writeFileIfDifferent(QString name, QString & str);
#endif

4
ground/uavobjgenerator/generators/java/uavobjectgeneratorjava.cpp
View File

@ -62,7 +62,7 @@ bool UAVObjectGeneratorJava::generate(UAVObjectParser *parser, QString templatep
// Write the gcs object inialization files
javaInitTemplate.replace(QString("$(OBJINC)"), objInc);
javaInitTemplate.replace(QString("$(OBJINIT)"), javaObjInit);
bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate);
bool res = writeFileIfDifferent(javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate);
if (!res) {
cout << "Error: Could not write output files" << endl;
return false;
@ -238,7 +238,7 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo *info)
outCode.replace(QString("$(INITFIELDS)"), initfields);
// Write the java code
bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode);
bool res = writeFileIfDifferent(javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode);
if (!res) {
cout << "Error: Could not write gcs output files" << endl;
return false;

2
ground/uavobjgenerator/generators/python/uavobjectgeneratorpython.cpp
View File

@ -110,7 +110,7 @@ bool UAVObjectGeneratorPython::process_object(ObjectInfo *info)
outCode.replace(QString("$(DATAFIELDINIT)"), fields);
// Write the Python code
bool res = writeFileIfDiffrent(pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode);
bool res = writeFileIfDifferent(pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode);
if (!res) {
cout << "Error: Could not write Python output files" << endl;
return false;

6
ground/uavobjgenerator/generators/wireshark/uavobjectgeneratorwireshark.cpp
View File

@ -93,8 +93,8 @@ bool UAVObjectGeneratorWireshark::generate(UAVObjectParser *parser, QString temp
/* Write the uavobject dissector's Makefile.common */
wiresharkMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames);
bool res = writeFileIfDiffrent(uavobjectsOutputPath.absolutePath() + "/Makefile.common",
wiresharkMakeTemplate);
bool res = writeFileIfDifferent(uavobjectsOutputPath.absolutePath() + "/Makefile.common",
wiresharkMakeTemplate);
if (!res) {
cout << "Error: Could not write wireshark Makefile" << endl;
return false;
@ -279,7 +279,7 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo *info, QDir outputpa
outCode.replace(QString("$(HEADERFIELDS)"), headerfields);
// Write the flight code
bool res = writeFileIfDiffrent(outputpath.absolutePath() + "/packet-op-uavobjects-" + info->namelc + ".c", outCode);
bool res = writeFileIfDifferent(outputpath.absolutePath() + "/packet-op-uavobjects-" + info->namelc + ".c", outCode);
if (!res) {
cout << "Error: Could not write wireshark code files" << endl;
return false;

5
ground/uavobjgenerator/uavobjectparser.cpp
View File

@ -25,6 +25,8 @@
*/
#include "uavobjectparser.h"
#include <QDomDocument>
#include <QDomElement>
#include <QDebug>
/**
* Constructor
@ -213,9 +215,6 @@ QString UAVObjectParser::parseXML(QString & xml, QString & filename)
// Sort all fields according to size
qStableSort(info->fields.begin(), info->fields.end(), fieldTypeLessThan);
// Sort all fields according to size
qStableSort(info->fields.begin(), info->fields.end(), fieldTypeLessThan);
// Make sure that required elements were found
if (!fieldFound) {
return QString("Object::field element is missing");

2
ground/uavobjgenerator/uavobjectparser.h
View File

@ -30,8 +30,6 @@
#include <QString>
#include <QStringList>
#include <QList>
#include <QDomDocument>
#include <QDomElement>
#include <QDomNode>
#include <QByteArray>

1
make/apps-defs.mk
View File

@ -80,6 +80,7 @@ SRC += $(PIOSCOMMON)/pios_rfm22b.c
SRC += $(PIOSCOMMON)/pios_rfm22b_com.c
SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/pios_sbus.c
SRC += $(PIOSCOMMON)/pios_srxl.c
SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_sensors.c

2
shared/uavobjectdefinition/hwsettings.xml
View File

@ -14,7 +14,7 @@
<field name="RM_RcvrPort" units="function" type="enum" elements="1" options="Disabled,PWM,PPM,PPM+PWM,PPM+Telemetry,PPM+Outputs,Outputs,Telemetry" defaultvalue="PWM"/>
<field name="RM_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,S.Bus,DSM,DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="RM_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,I2C,DSM,SRXL,DebugConsole,ComBridge,OsdHk" defaultvalue="Disabled"/>
<field name="TelemetrySpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200" defaultvalue="57600"/>
<field name="GPSSpeed" units="bps" type="enum" elements="1" options="2400,4800,9600,19200,38400,57600,115200,230400" defaultvalue="57600"/>

2
shared/uavobjectdefinition/manualcontrolsettings.xml
View File

@ -3,7 +3,7 @@
<description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
<field name="ChannelGroups" units="Channel Group" type="enum"
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,GCS,OPLink,None" defaultvalue="None"/>
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,SRXL,GCS,OPLink,None" defaultvalue="None"/>
<field name="ChannelNumber" units="channel" type="uint8" defaultvalue="0"
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/>
<field name="ChannelMin" units="us" type="int16" defaultvalue="1000"

2
shared/uavobjectdefinition/receiveractivity.xml
View File

@ -2,7 +2,7 @@
<object name="ReceiverActivity" singleinstance="true" settings="false" category="System">
<description>Monitors which receiver channels have been active within the last second.</description>
<field name="ActiveGroup" units="Channel Group" type="enum" elements="1"
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,GCS,OPLink,None"
options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,SRXL,GCS,OPLink,None"
defaultvalue="None"/>
<field name="ActiveChannel" units="channel" type="uint8" elements="1"
defaultvalue="255"/>