FrameType_t GetCurrentFrameType()
{
uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type);
switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
case SYSTEMSETTINGS_AIRFRAMETYPE_QUADH:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
return FRAME_TYPE_MULTIROTOR;
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
return FRAME_TYPE_FIXED_WING;
case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
return FRAME_TYPE_HELI;
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
return FRAME_TYPE_GROUND;
case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
return FRAME_TYPE_CUSTOM;
}
// anyway it should not reach here
return FRAME_TYPE_CUSTOM;
}
/**
* Run a preflight check over the hardware configuration
* and currently active modules
*/
int32_t configuration_check()
{
int32_t severity = SYSTEMALARMS_ALARM_OK;
SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
uint8_t alarmsubstatus = 0;
// Get board type
const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04;
// 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:
case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
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) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor) : severity;
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEHOLD:
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_ALTITUDEVARIO:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
// TODO: put check equivalent to TASK_MONITOR_IsRunning
// here as soon as available for delayed callbacks
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { // Revo supports altitude hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports Position Hold
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_LAND:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports AutoLand Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POI:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports POI Mode
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
// Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
}
break;
case MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
default:
// Uncovered modes are automatically an error
severity = SYSTEMALARMS_ALARM_ERROR;
}
// mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
alarmsubstatus = i;
}
}
// TODO: Check on a multirotor no axis supports "None"
if (severity != SYSTEMALARMS_ALARM_OK) {
ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
} else {
AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
}
return 0;
}
/**
* 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;
}
