/**
* Checks the stabiliation settings for a paritcular mode and makes
* sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_CONFIGERROR_NONE or SYSTEMALARMS_CONFIGERROR_MULTIROTOR
*/
static int32_t check_stabilization_settings(int index, bool multirotor)
{
// Make sure the modes have identical sizes
if (MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NUMELEM ||
MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM != MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NUMELEM)
return SYSTEMALARMS_CONFIGERROR_MULTIROTOR;
uint8_t modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch(index) {
case 1:
ManualControlSettingsStabilization1SettingsGet(modes);
break;
case 2:
ManualControlSettingsStabilization2SettingsGet(modes);
break;
case 3:
ManualControlSettingsStabilization3SettingsGet(modes);
break;
default:
return SYSTEMALARMS_CONFIGERROR_NONE;
}
// For multirotors verify that nothing is set to "none"
if (multirotor) {
for(uint32_t i = 0; i < NELEMENTS(modes); i++) {
if (modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE)
return SYSTEMALARMS_CONFIGERROR_MULTIROTOR;
// If this axis allows enabling an autotune behavior without the module
// running then set an alarm now that aututune module initializes the
// appropriate objects
if ((modes[i] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_SYSTEMIDENT) &&
(!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE)))
return SYSTEMALARMS_CONFIGERROR_AUTOTUNE;
}
}
// POI mode is only valid for YAW in the case it is enabled and camera stab is running
if (modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ROLL] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_POI ||
modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_PITCH] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_POI)
return SYSTEMALARMS_CONFIGERROR_STABILIZATION;
if (modes[MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_YAW] == MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_POI) {
#if !defined(CAMERASTAB_POI_MODE)
return SYSTEMALARMS_CONFIGERROR_STABILIZATION;
#endif
// TODO: Need to check camera stab is actually running
}
// Warning: This assumes that certain conditions in the XML file are met. That
// MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_NONE
return SYSTEMALARMS_CONFIGERROR_NONE;
}
/**
* Run a preflight check over the hardware configuration
* and currently active modules
*/
int32_t configuration_check()
{
SystemAlarmsConfigErrorOptions error_code = SYSTEMALARMS_CONFIGERROR_NONE;
// Get board type
const struct pios_board_info * bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// For when modules are not running we should explicitly check the objects are
// valid
if (ManualControlSettingsHandle() == NULL ||
SystemSettingsHandle() == NULL) {
AlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_CRITICAL);
return 0;
}
// Classify airframe type
bool multirotor = true;
uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch(airframe_type) {
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
multirotor = true;
break;
default:
multirotor = false;
}
// For each available flight mode position sanity check the available
// modes
uint8_t num_modes;
uint8_t modes[MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
ManualControlSettingsFlightModePositionGet(modes);
for(uint32_t i = 0; i < num_modes; i++) {
switch(modes[i]) {
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
error_code = SYSTEMALARMS_CONFIGERROR_STABILIZATION;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ACRO:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ACROPLUS:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LEVELING:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MWRATE:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_HORIZON:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AXISLOCK:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VIRTUALBAR:
// always ok
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(1, multirotor) : error_code;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(2, multirotor) : error_code;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_settings(3, multirotor) : error_code;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_AUTOTUNE))
error_code = SYSTEMALARMS_CONFIGERROR_AUTOTUNE;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
if (coptercontrol)
error_code = SYSTEMALARMS_CONFIGERROR_ALTITUDEHOLD;
else {
if ( !TaskMonitorQueryRunning(TASKINFO_RUNNING_ALTITUDEHOLD) )
error_code = SYSTEMALARMS_CONFIGERROR_ALTITUDEHOLD;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOHOME:
if (coptercontrol) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
}
else {
if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
} else {
error_code = check_safe_autonomous();
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_TABLETCONTROL:
if (coptercontrol) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
}
else {
if (!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER) ||
!TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHPLANNER)) {
error_code = SYSTEMALARMS_CONFIGERROR_PATHPLANNER;
} else {
error_code = check_safe_autonomous();
}
}
break;
default:
// Uncovered modes are automatically an error
error_code = SYSTEMALARMS_CONFIGERROR_UNDEFINED;
}
}
// Check the stabilization rates are within what the sensors can track
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_stabilization_rates() : error_code;
// Only check safe to arm if no other errors exist
error_code = (error_code == SYSTEMALARMS_CONFIGERROR_NONE) ? check_safe_to_arm() : error_code;
set_config_error(error_code);
return 0;
}
/**
* Module task
*/
static void pathPlannerTask(void *parameters)
{
// If the PathStatus isn't available no follower is running and we should abort
while (PathStatusHandle() == NULL || !TaskMonitorQueryRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
AlarmsSet(SYSTEMALARMS_ALARM_PATHPLANNER, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_Thread_Sleep(1000);
}
AlarmsClear(SYSTEMALARMS_ALARM_PATHPLANNER);
PathPlannerSettingsConnectCallback(settingsUpdated);
settingsUpdated(PathPlannerSettingsHandle());
WaypointConnectCallback(waypointsUpdated);
WaypointActiveConnectCallback(waypointsUpdated);
PathStatusConnectCallback(pathStatusUpdated);
FlightStatusData flightStatus;
// Main thread loop
bool pathplanner_active = false;
path_status_updated = false;
while (1)
{
PIOS_Thread_Sleep(UPDATE_RATE_MS);
// When not running the path planner short circuit and wait
FlightStatusGet(&flightStatus);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
continue;
}
if(pathplanner_active == false) {
// Reset the state. Active waypoint should be set to an invalid
// value to force waypoint 0 to become activated when starting
// Note: this needs to be done before the callback is triggered!
active_waypoint = -1;
previous_waypoint = -1;
// This triggers callback to update variable
WaypointActiveGet(&waypointActive);
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
pathplanner_active = true;
continue;
}
/* This method determines if we have achieved the goal of the active */
/* waypoint */
if (path_status_updated)
checkTerminationCondition();
/* If advance waypoint takes a long time to calculate then it should */
/* be called from here when the active_waypoints does not equal the */
/* WaypointActive.Index */
/* if (active_waypoint != WaypointActive.Index) */
/* advanceWaypoint(WaypointActive.Index) */
}
}
