/** * 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 navigation capability #ifdef REVOLUTION RevoSettingsInitialize(); uint8_t revoFusion; RevoSettingsFusionAlgorithmGet(&revoFusion); bool navCapableFusion; switch (revoFusion) { case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR: case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13: navCapableFusion = true; break; default: navCapableFusion = false; // check for hitl. hitl allows to feed position and velocity state via // telemetry, this makes nav possible even with an unsuited algorithm if (PositionStateHandle()) { if (PositionStateReadOnly()) { navCapableFusion = true; } } } #else const bool navCapableFusion = false; #endif /* ifdef REVOLUTION */ // Classify airframe type bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR); // For each available flight mode position sanity check the available // modes uint8_t num_modes; uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; ManualControlSettingsFlightModeNumberGet(&num_modes); FlightModeSettingsFlightModePositionGet(modes); for (uint32_t i = 0; i < num_modes; i++) { switch (modes[i]) { case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: ADDSEVERITY(!multirotor); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2: ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4: ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5: ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6: ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: { // 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); ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK); } // intentionally no break as this also needs pathfollower case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOFPV: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOLOS: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIONSEW: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: ADDSEVERITY(!coptercontrol); ADDSEVERITY(navCapableFusion); break; default: // Uncovered modes are automatically an error ADDSEVERITY(false); } // 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; } } uint8_t checks_disabled; FlightModeSettingsDisableSanityChecksGet(&checks_disabled); if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) { severity = SYSTEMALARMS_ALARM_WARNING; } 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() { 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; }