rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +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.StartingVelocity = 0.0f;
pathDesired.EndingVelocity = 0.0f; pathDesired.EndingVelocity = 0.0f;
uint8_t ReturnToBaseNextCommand; FlightModeSettingsReturnToBaseNextCommandOptions ReturnToBaseNextCommand;
FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand); FlightModeSettingsReturnToBaseNextCommandGet(&ReturnToBaseNextCommand);
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand; pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] = (float)ReturnToBaseNextCommand;
pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_UNUSED1] = 0.0f; 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 // Classify navigation capability
#ifdef REVOLUTION #ifdef REVOLUTION
RevoSettingsInitialize(); RevoSettingsInitialize();
uint8_t revoFusion; RevoSettingsFusionAlgorithmOptions revoFusion;
RevoSettingsFusionAlgorithmGet(&revoFusion); RevoSettingsFusionAlgorithmGet(&revoFusion);
bool navCapableFusion; bool navCapableFusion;
switch (revoFusion) { switch (revoFusion) {
@ -104,8 +104,8 @@ int32_t configuration_check()
// For each available flight mode position sanity check the available // For each available flight mode position sanity check the available
// modes // modes
uint8_t num_modes; uint8_t num_modes;
uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes); ManualControlSettingsFlightModeNumberGet(&num_modes);
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap); StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
FlightModeSettingsFlightModePositionGet(modes); FlightModeSettingsFlightModePositionGet(modes);
@ -209,7 +209,7 @@ int32_t configuration_check()
} }
} }
uint8_t checks_disabled; FlightModeSettingsDisableSanityChecksOptions checks_disabled;
FlightModeSettingsDisableSanityChecksGet(&checks_disabled); FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) { if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
severity = SYSTEMALARMS_ALARM_WARNING; 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 // Get the different axis modes for this switch position
switch (index) { switch (index) {
case 1: case 1:
FlightModeSettingsStabilization1SettingsArrayGet(modes); FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
break; break;
case 2: case 2:
FlightModeSettingsStabilization2SettingsArrayGet(modes); FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
break; break;
case 3: case 3:
FlightModeSettingsStabilization3SettingsArrayGet(modes); FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
break; break;
case 4: case 4:
FlightModeSettingsStabilization4SettingsArrayGet(modes); FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
break; break;
case 5: case 5:
FlightModeSettingsStabilization5SettingsArrayGet(modes); FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
break; break;
case 6: case 6:
FlightModeSettingsStabilization6SettingsArrayGet(modes); FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
break; break;
default: default:
return false; return false;
@ -326,7 +326,7 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter
FrameType_t GetCurrentFrameType() FrameType_t GetCurrentFrameType()
{ {
uint8_t airframe_type; SystemSettingsAirframeTypeOptions airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type); SystemSettingsAirframeTypeGet(&airframe_type);
switch ((SystemSettingsAirframeTypeOptions)airframe_type) { switch ((SystemSettingsAirframeTypeOptions)airframe_type) {

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

@ -98,7 +98,7 @@ int32_t AirspeedInitialize()
#else #else
HwSettingsInitialize(); HwSettingsInitialize();
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM]; HwSettingsOptionalModulesOptions optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesArrayGet(optionalModules); HwSettingsOptionalModulesArrayGet(optionalModules);
@ -110,7 +110,7 @@ int32_t AirspeedInitialize()
} }
#endif #endif
uint8_t adcRouting[HWSETTINGS_ADCROUTING_NUMELEM]; HwSettingsADCRoutingOptions adcRouting[HWSETTINGS_ADCROUTING_NUMELEM];
HwSettingsADCRoutingArrayGet(adcRouting); HwSettingsADCRoutingArrayGet(adcRouting);
// Determine if the barometric airspeed sensor is routed to an ADC pin // 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); imu_airspeedInitialize(&airspeedSettings);
} }
break; break;
default:
break;
} }
} }
switch (airspeedSettings.AirspeedSensorType) { switch (airspeedSettings.AirspeedSensorType) {

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

@ -153,7 +153,7 @@ int32_t GPSStart(void)
int32_t GPSInitialize(void) int32_t GPSInitialize(void)
{ {
gpsPort = PIOS_COM_GPS; gpsPort = PIOS_COM_GPS;
uint8_t gpsProtocol; GPSSettingsDataProtocolOptions gpsProtocol;
#ifdef MODULE_GPS_BUILTIN #ifdef MODULE_GPS_BUILTIN
gpsEnabled = true; gpsEnabled = true;
@ -338,7 +338,7 @@ static void gpsTask(__attribute__((unused)) void *parameters)
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) { (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
// we have not received any valid GPS sentences for a while. // 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. // 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); GPSPositionSensorStatusSet(&status);
AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_GPS, SYSTEMALARMS_ALARM_ERROR);
} else { } else {
@ -439,7 +439,7 @@ static void updateHwSettings()
{ {
if (gpsPort) { if (gpsPort) {
// Retrieve settings // Retrieve settings
uint8_t speed; HwSettingsGPSSpeedOptions speed;
HwSettingsGPSSpeedGet(&speed); HwSettingsGPSSpeedGet(&speed);
// Set port 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); DebugLogEntrySet(entry);
} else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) { } else if (control.Operation == DEBUGLOGCONTROL_OPERATION_FORMATFLASH) {
uint8_t armed; FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed); FlightStatusArmedGet(&armed);
if (armed == FLIGHTSTATUS_ARMED_DISARMED) { if (armed == FLIGHTSTATUS_ARMED_DISARMED) {
PIOS_DEBUGLOG_Format(); 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: case FLIGHTMODESETTINGS_ARMING_ACCESSORY2:
armingInputLevel = -1.0f * acc.AccessoryVal; armingInputLevel = -1.0f * acc.AccessoryVal;
break; break;
default:
break;
} }
bool manualArm = false; bool manualArm = false;

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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. // and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO; thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break; break;
default:
break;
} }
stabDesired.StabilizationMode.Thrust = thrustMode; stabDesired.StabilizationMode.Thrust = (StabilizationDesiredStabilizationModeOptions)thrustMode;
} }
// set the thrust value // set the thrust value

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

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

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

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

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

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

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

@ -264,7 +264,7 @@ static void stabilizationOuterloopTask()
RateDesiredSet(&rateDesired); RateDesiredSet(&rateDesired);
{ {
uint8_t armed; FlightStatusArmedOptions armed;
FlightStatusArmedGet(&armed); FlightStatusArmedGet(&armed);
float throttleDesired; float throttleDesired;
ManualControlCommandThrottleGet(&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 bool mallocFailed;
static HwSettingsData bootHwSettings; static HwSettingsData bootHwSettings;
static FrameType_t bootFrameType; static FrameType_t bootFrameType;
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)
static struct PIOS_FLASHFS_Stats fsStats; static struct PIOS_FLASHFS_Stats fsStats;
#endif
// Private functions // Private functions
static void objectUpdatedCb(UAVObjEvent *ev); static void objectUpdatedCb(UAVObjEvent *ev);

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

@ -888,7 +888,7 @@ static void updateSettings(channelContext *channel)
if (port) { if (port) {
// Retrieve settings // Retrieve settings
uint8_t speed; HwSettingsTelemetrySpeedOptions speed;
HwSettingsTelemetrySpeedGet(&speed); HwSettingsTelemetrySpeedGet(&speed);
// Set port 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(); 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 * Initialize the Instrumentation infrastructure
* @param maxCounters maximum number of allowed counters * @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_TIMED_SECTION_END(x) PIOS_Instrumentation_TimeEnd(x)
#define PERF_MEASURE_PERIOD(x) PIOS_Instrumentation_TrackPeriod(x) #define PERF_MEASURE_PERIOD(x) PIOS_Instrumentation_TrackPeriod(x)
#define PERF_TRACK_VALUE(x, y) PIOS_Instrumentation_updateCounter(x, y) #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 #else
@ -107,5 +109,7 @@
#define PERF_TIMED_SECTION_END(x) #define PERF_TIMED_SECTION_END(x)
#define PERF_MEASURE_PERIOD(x) #define PERF_MEASURE_PERIOD(x)
#define PERF_TRACK_VALUE(x, y) #define PERF_TRACK_VALUE(x, y)
#define PERF_INCREMENT_VALUE(x)
#define PERF_DECREMENT_VALUE(x)
#endif /* PIOS_INCLUDE_INSTRUMENTATION */ #endif /* PIOS_INCLUDE_INSTRUMENTATION */
#endif /* PIOS_INSTRUMENTATION_HELPER_H */ #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> #include <pios_sbus.h>
#endif #endif
#ifdef PIOS_INCLUDE_SRXL
#include <pios_srxl.h>
#endif
/* PIOS abstract receiver interface */ /* PIOS abstract receiver interface */
#ifdef PIOS_INCLUDE_RCVR #ifdef PIOS_INCLUDE_RCVR
#include <pios_rcvr.h> #include <pios_rcvr.h>

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

@ -20,6 +20,7 @@
# (all architectures) # (all architectures)
UAVOBJSRCFILENAMES = UAVOBJSRCFILENAMES =
UAVOBJSRCFILENAMES += statusvtolland UAVOBJSRCFILENAMES += statusvtolland
UAVOBJSRCFILENAMES += statusvtolautotakeoff
UAVOBJSRCFILENAMES += vtolselftuningstats UAVOBJSRCFILENAMES += vtolselftuningstats
UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accelgyrosettings
UAVOBJSRCFILENAMES += accessorydesired 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, .gpio_clk_periph = RCC_AHB1Periph_GPIOC,
}; };
#ifdef PIOS_INCLUDE_COM_FLEXI #ifdef PIOS_INCLUDE_COM_FLEXI
/* /*
* FLEXI PORT * FLEXI PORT
@ -1007,6 +1006,54 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = {
#endif /* PIOS_INCLUDE_DSM */ #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 * 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_PPM_FLEXI */
#define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_DSM
#define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SBUS
#define PIOS_INCLUDE_SRXL
#define PIOS_INCLUDE_GCSRCVR #define PIOS_INCLUDE_GCSRCVR
#define PIOS_INCLUDE_OPLINKRCVR #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: 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); 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; 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 */ } /* hwsettings_rm_flexiport */
/* Moved this here to allow binding on flexiport */ /* Moved this here to allow binding on flexiport */
@ -868,7 +889,6 @@ void PIOS_Board_Init(void)
break; break;
} }
#if defined(PIOS_INCLUDE_GCSRCVR) #if defined(PIOS_INCLUDE_GCSRCVR)
GCSReceiverInitialize(); GCSReceiverInitialize();
uint32_t pios_gcsrcvr_id; 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_MAX_DEVS 1
#define PIOS_SBUS_NUM_INPUTS (16 + 2) #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 // 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 IO ports */
/* Configure Telemetry port */ /* Configure Telemetry port */
uint8_t hwsettings_rv_telemetryport; HwSettingsRV_TelemetryPortOptions hwsettings_rv_telemetryport;
HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport); HwSettingsRV_TelemetryPortGet(&hwsettings_rv_telemetryport);
switch (hwsettings_rv_telemetryport) { switch (hwsettings_rv_telemetryport) {
@ -164,10 +164,12 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RV_TELEMETRYPORT_COMAUX: 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); 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; break;
default:
break;
} /* hwsettings_rv_telemetryport */ } /* hwsettings_rv_telemetryport */
/* Configure GPS port */ /* Configure GPS port */
uint8_t hwsettings_rv_gpsport; HwSettingsRV_GPSPortOptions hwsettings_rv_gpsport;
HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport); HwSettingsRV_GPSPortGet(&hwsettings_rv_gpsport);
switch (hwsettings_rv_gpsport) { switch (hwsettings_rv_gpsport) {
case HWSETTINGS_RV_GPSPORT_DISABLED: case HWSETTINGS_RV_GPSPORT_DISABLED:
@ -184,10 +186,12 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RV_GPSPORT_COMAUX: 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); 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; break;
default:
break;
} /* hwsettings_rv_gpsport */ } /* hwsettings_rv_gpsport */
/* Configure AUXPort */ /* Configure AUXPort */
uint8_t hwsettings_rv_auxport; HwSettingsRV_AuxPortOptions hwsettings_rv_auxport;
HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport); HwSettingsRV_AuxPortGet(&hwsettings_rv_auxport);
switch (hwsettings_rv_auxport) { switch (hwsettings_rv_auxport) {
@ -201,6 +205,7 @@ void PIOS_Board_Init(void)
case HWSETTINGS_RV_AUXPORT_COMAUX: 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); 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; break;
default:
break; break;
} /* hwsettings_rv_auxport */ } /* hwsettings_rv_auxport */
} }

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

@ -52,6 +52,8 @@ int32_t $(NAME)Initialize();
UAVObjHandle $(NAME)Handle(); UAVObjHandle $(NAME)Handle();
void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId); void $(NAME)SetDefaults(UAVObjHandle obj, uint16_t instId);
$(DATAFIELDINFO)
$(DATASTRUCTURES) $(DATASTRUCTURES)
/* /*
* Packed Object data (unaligned). * 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 int32_t $(NAME)SetMetadata(const UAVObjMetadata *dataIn) { return UAVObjSetMetadata($(NAME)Handle(), dataIn); }
static inline int8_t $(NAME)ReadOnly() { return UAVObjReadOnly($(NAME)Handle()); } static inline int8_t $(NAME)ReadOnly() { return UAVObjReadOnly($(NAME)Handle()); }
$(DATAFIELDINFO)
/* Set/Get functions */ /* Set/Get functions */
$(SETGETFIELDSEXTERN) $(SETGETFIELDSEXTERN)

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

@ -158,6 +158,9 @@ void InputChannelForm::groupUpdated()
case ManualControlSettings::CHANNELGROUPS_SBUS: case ManualControlSettings::CHANNELGROUPS_SBUS:
count = 18; count = 18;
break; break;
case ManualControlSettings::CHANNELGROUPS_SRXL:
count = 16;
break;
case ManualControlSettings::CHANNELGROUPS_GCS: case ManualControlSettings::CHANNELGROUPS_GCS:
count = GCSReceiver::CHANNEL_NUMELEM; count = GCSReceiver::CHANNEL_NUMELEM;
break; 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 // Write the flight object inialization files
flightInitTemplate.replace(QString("$(OBJINC)"), objInc); flightInitTemplate.replace(QString("$(OBJINC)"), objInc);
flightInitTemplate.replace(QString("$(OBJINIT)"), flightObjInit); flightInitTemplate.replace(QString("$(OBJINIT)"), flightObjInit);
bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.c", bool res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/uavobjectsinit.c",
flightInitTemplate); flightInitTemplate);
if (!res) { if (!res) {
cout << "Error: Could not write flight object init file" << endl; cout << "Error: Could not write flight object init file" << endl;
return false; return false;
@ -77,8 +77,8 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templat
// Write the flight object initialization header // Write the flight object initialization header
flightInitIncludeTemplate.replace(QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc)); flightInitIncludeTemplate.replace(QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc));
res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/uavobjectsinit.h", res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/uavobjectsinit.h",
flightInitIncludeTemplate); flightInitIncludeTemplate);
if (!res) { if (!res) {
cout << "Error: Could not write flight object init header file" << endl; cout << "Error: Could not write flight object init header file" << endl;
return false; return false;
@ -87,8 +87,8 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser *parser, QString templat
// Write the flight object Makefile // Write the flight object Makefile
flightMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames); flightMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames);
flightMakeTemplate.replace(QString("$(UAVOBJNAMES)"), objNames); flightMakeTemplate.replace(QString("$(UAVOBJNAMES)"), objNames);
res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/Makefile.inc", res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/Makefile.inc",
flightMakeTemplate); flightMakeTemplate);
if (!res) { if (!res) {
cout << "Error: Could not write flight Makefile" << endl; cout << "Error: Could not write flight Makefile" << endl;
return false; return false;
@ -115,13 +115,25 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
replaceCommonTags(outInclude, info); replaceCommonTags(outInclude, info);
replaceCommonTags(outCode, 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 // Replace the $(DATAFIELDS) tag
QString type; QString type;
QString fields; QString fields;
QString dataStructures; QString dataStructures;
for (int n = 0; n < info->fields.length(); ++n) { for (int n = 0; n < info->fields.length(); ++n) {
// Determine type // Determine type
type = fieldTypeStrC[info->fields[n]->type]; type = typeList[n];
// Append field // Append field
// Check if it a named set and creates structures accordingly // Check if it a named set and creates structures accordingly
if (info->fields[n]->numElements > 1) { if (info->fields[n]->numElements > 1) {
@ -158,7 +170,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
// Only for enum types // Only for enum types
if (info->fields[n]->type == FIELDTYPE_ENUM) { if (info->fields[n]->type == FIELDTYPE_ENUM) {
enums.append(QString("\n// Enumeration options for field %1\n").arg(info->fields[n]->name)); 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 // Go through each option
QStringList options = info->fields[n]->options; QStringList options = info->fields[n]->options;
for (int m = 0; m < options.length(); ++m) { for (int m = 0; m < options.length(); ++m) {
@ -262,26 +274,26 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
if (info->fields[n]->numElements == 1) { if (info->fields[n]->numElements == 1) {
/* Set */ /* Set */
setgetfields.append(QString("void %2%3Set(%1 *New%3)\n") setgetfields.append(QString("void %2%3Set(%1 *New%3)\n")
.arg(fieldTypeStrC[info->fields[n]->type]) .arg(typeList[n])
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name)); .arg(info->fields[n]->name));
setgetfields.append(QString("{\n")); setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") setgetfields.append(QString(" UAVObjSetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n")
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(fieldTypeStrC[info->fields[n]->type])); .arg(typeList[n]));
setgetfields.append(QString("}\n")); setgetfields.append(QString("}\n"));
/* GET */ /* GET */
setgetfields.append(QString("void %2%3Get(%1 *New%3)\n") setgetfields.append(QString("void %2%3Get(%1 *New%3)\n")
.arg(fieldTypeStrC[info->fields[n]->type]) .arg(typeList[n])
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name)); .arg(info->fields[n]->name));
setgetfields.append(QString("{\n")); setgetfields.append(QString("{\n"));
setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n") setgetfields.append(QString(" UAVObjGetDataField(%1Handle(), (void *)New%2, offsetof(%1Data, %2), sizeof(%3));\n")
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(fieldTypeStrC[info->fields[n]->type])); .arg(typeList[n]));
setgetfields.append(QString("}\n")); setgetfields.append(QString("}\n"));
} else { } else {
// When no struct accessor is available for a field array accessor is the default. // 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->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(info->fields[n]->numElements) .arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type])); .arg(typeList[n]));
setgetfields.append(QString("}\n")); setgetfields.append(QString("}\n"));
/* GET */ /* GET */
@ -313,7 +325,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(info->fields[n]->numElements) .arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type])); .arg(typeList[n]));
setgetfields.append(QString("}\n")); setgetfields.append(QString("}\n"));
// Append array suffix to array accessors // Append array suffix to array accessors
@ -323,7 +335,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
// array based field accessor // array based field accessor
/* SET */ /* SET */
setgetfields.append(QString("void %2%3%4Set( %1 *New%3 )\n") 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->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(suffix)); .arg(suffix));
@ -332,12 +344,12 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(info->fields[n]->numElements) .arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type])); .arg(typeList[n]));
setgetfields.append(QString("}\n")); setgetfields.append(QString("}\n"));
/* GET */ /* GET */
setgetfields.append(QString("void %2%3%4Get( %1 *New%3 )\n") 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->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(suffix)); .arg(suffix));
@ -346,7 +358,7 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
.arg(info->name) .arg(info->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(info->fields[n]->numElements) .arg(info->fields[n]->numElements)
.arg(fieldTypeStrC[info->fields[n]->type])); .arg(typeList[n]));
setgetfields.append(QString("}\n")); setgetfields.append(QString("}\n"));
} }
} }
@ -378,14 +390,14 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
} }
/* SET */ /* SET */
setgetfieldsextern.append(QString("extern void %2%3%4Set(%1 *New%3);\n") 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->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(suffix)); .arg(suffix));
/* GET */ /* GET */
setgetfieldsextern.append(QString("extern void %2%3%4Get(%1 *New%3);\n") 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->name)
.arg(info->fields[n]->name) .arg(info->fields[n]->name)
.arg(suffix)); .arg(suffix));
@ -394,13 +406,13 @@ bool UAVObjectGeneratorFlight::process_object(ObjectInfo *info)
outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern); outInclude.replace(QString("$(SETGETFIELDSEXTERN)"), setgetfieldsextern);
// Write the flight code // Write the flight code
bool res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode); bool res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/" + info->namelc + ".c", outCode);
if (!res) { if (!res) {
cout << "Error: Could not write flight code files" << endl; cout << "Error: Could not write flight code files" << endl;
return false; return false;
} }
res = writeFileIfDiffrent(flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude); res = writeFileIfDifferent(flightOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude);
if (!res) { if (!res) {
cout << "Error: Could not write flight include files" << endl; cout << "Error: Could not write flight include files" << endl;
return false; 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 // Write the gcs object inialization files
gcsInitTemplate.replace(QString("$(OBJINC)"), objInc); gcsInitTemplate.replace(QString("$(OBJINC)"), objInc);
gcsInitTemplate.replace(QString("$(OBJINIT)"), gcsObjInit); gcsInitTemplate.replace(QString("$(OBJINIT)"), gcsObjInit);
bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate); bool res = writeFileIfDifferent(gcsOutputPath.absolutePath() + "/uavobjectsinit.cpp", gcsInitTemplate);
if (!res) { if (!res) {
cout << "Error: Could not write output files" << endl; cout << "Error: Could not write output files" << endl;
return false; return false;
@ -371,12 +371,12 @@ bool UAVObjectGeneratorGCS::process_object(ObjectInfo *info)
outCode.replace(QString("$(INITFIELDS)"), initfields); outCode.replace(QString("$(INITFIELDS)"), initfields);
// Write the GCS code // Write the GCS code
bool res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode); bool res = writeFileIfDifferent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".cpp", outCode);
if (!res) { if (!res) {
cout << "Error: Could not write gcs output files" << endl; cout << "Error: Could not write gcs output files" << endl;
return false; return false;
} }
res = writeFileIfDiffrent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude); res = writeFileIfDifferent(gcsOutputPath.absolutePath() + "/" + info->namelc + ".h", outInclude);
if (!res) { if (!res) {
cout << "Error: Could not write gcs output files" << endl; cout << "Error: Could not write gcs output files" << endl;
return false; 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 * 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)) { if (str == readFile(name, false)) {
return true; return true;

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

@ -37,6 +37,6 @@
QString readFile(QString name); QString readFile(QString name);
QString readFile(QString name); QString readFile(QString name);
bool writeFile(QString name, QString & str); bool writeFile(QString name, QString & str);
bool writeFileIfDiffrent(QString name, QString & str); bool writeFileIfDifferent(QString name, QString & str);
#endif #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 // Write the gcs object inialization files
javaInitTemplate.replace(QString("$(OBJINC)"), objInc); javaInitTemplate.replace(QString("$(OBJINC)"), objInc);
javaInitTemplate.replace(QString("$(OBJINIT)"), javaObjInit); javaInitTemplate.replace(QString("$(OBJINIT)"), javaObjInit);
bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate); bool res = writeFileIfDifferent(javaOutputPath.absolutePath() + "/UAVObjectsInitialize.java", javaInitTemplate);
if (!res) { if (!res) {
cout << "Error: Could not write output files" << endl; cout << "Error: Could not write output files" << endl;
return false; return false;
@ -238,7 +238,7 @@ bool UAVObjectGeneratorJava::process_object(ObjectInfo *info)
outCode.replace(QString("$(INITFIELDS)"), initfields); outCode.replace(QString("$(INITFIELDS)"), initfields);
// Write the java code // Write the java code
bool res = writeFileIfDiffrent(javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode); bool res = writeFileIfDifferent(javaOutputPath.absolutePath() + "/" + info->name + ".java", outCode);
if (!res) { if (!res) {
cout << "Error: Could not write gcs output files" << endl; cout << "Error: Could not write gcs output files" << endl;
return false; 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); outCode.replace(QString("$(DATAFIELDINIT)"), fields);
// Write the Python code // Write the Python code
bool res = writeFileIfDiffrent(pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode); bool res = writeFileIfDifferent(pythonOutputPath.absolutePath() + "/" + info->namelc + ".py", outCode);
if (!res) { if (!res) {
cout << "Error: Could not write Python output files" << endl; cout << "Error: Could not write Python output files" << endl;
return false; 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 */ /* Write the uavobject dissector's Makefile.common */
wiresharkMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames); wiresharkMakeTemplate.replace(QString("$(UAVOBJFILENAMES)"), objFileNames);
bool res = writeFileIfDiffrent(uavobjectsOutputPath.absolutePath() + "/Makefile.common", bool res = writeFileIfDifferent(uavobjectsOutputPath.absolutePath() + "/Makefile.common",
wiresharkMakeTemplate); wiresharkMakeTemplate);
if (!res) { if (!res) {
cout << "Error: Could not write wireshark Makefile" << endl; cout << "Error: Could not write wireshark Makefile" << endl;
return false; return false;
@ -279,7 +279,7 @@ bool UAVObjectGeneratorWireshark::process_object(ObjectInfo *info, QDir outputpa
outCode.replace(QString("$(HEADERFIELDS)"), headerfields); outCode.replace(QString("$(HEADERFIELDS)"), headerfields);
// Write the flight code // 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) { if (!res) {
cout << "Error: Could not write wireshark code files" << endl; cout << "Error: Could not write wireshark code files" << endl;
return false; return false;

5
ground/uavobjgenerator/uavobjectparser.cpp
View File

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

2
ground/uavobjgenerator/uavobjectparser.h
View File

@ -30,8 +30,6 @@
#include <QString> #include <QString>
#include <QStringList> #include <QStringList>
#include <QList> #include <QList>
#include <QDomDocument>
#include <QDomElement>
#include <QDomNode> #include <QDomNode>
#include <QByteArray> #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_rfm22b_com.c
SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_rcvr.c
SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_sbus.c
SRC += $(PIOSCOMMON)/pios_srxl.c
SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_sdcard.c
SRC += $(PIOSCOMMON)/pios_sensors.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_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_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="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"/> <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> <description>Settings to indicate how to decode receiver input by @ref ManualControlModule.</description>
<field name="ChannelGroups" units="Channel Group" type="enum" <field name="ChannelGroups" units="Channel Group" type="enum"
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2" 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" <field name="ChannelNumber" units="channel" type="uint8" defaultvalue="0"
elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/> elementnames="Throttle,Roll,Pitch,Yaw,FlightMode,Collective,Accessory0,Accessory1,Accessory2"/>
<field name="ChannelMin" units="us" type="int16" defaultvalue="1000" <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"> <object name="ReceiverActivity" singleinstance="true" settings="false" category="System">
<description>Monitors which receiver channels have been active within the last second.</description> <description>Monitors which receiver channels have been active within the last second.</description>
<field name="ActiveGroup" units="Channel Group" type="enum" elements="1" <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"/> defaultvalue="None"/>
<field name="ActiveChannel" units="channel" type="uint8" elements="1" <field name="ActiveChannel" units="channel" type="uint8" elements="1"
defaultvalue="255"/> defaultvalue="255"/>