rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-28 17:54:15 +01:00
Code Issues Releases Activity

Merge branch 'corvuscorax/OP-1363_sanitychecks-check-for-outdoor-mode-if-required' into next

Browse Source
This commit is contained in:
Corvus Corax 2014-06-01 21:52:41 +02:00
commit bd12bbea91
1 changed files with 58 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
flight/libraries/sanitycheck.c
View File

@ -37,8 +37,13 @@
#include <flightmodesettings.h> #include <flightmodesettings.h>
#include <systemsettings.h> #include <systemsettings.h>
#include <systemalarms.h> #include <systemalarms.h>
#include <revosettings.h>
#include <taskinfo.h> #include <taskinfo.h>
// a number of useful macros
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_ERROR))
/**************************** /****************************
* Current checks: * Current checks:
* 1. If a flight mode switch allows autotune and autotune module not running * 1. If a flight mode switch allows autotune and autotune module not running
@ -46,7 +51,7 @@
****************************/ ****************************/
// ! Check a stabilization mode switch position for safety // ! Check a stabilization mode switch position for safety
static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol); static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
/** /**
* Run a preflight check over the hardware configuration * Run a preflight check over the hardware configuration
@ -61,8 +66,27 @@ int32_t configuration_check()
const struct pios_board_info *bdinfo = &pios_board_info_blob; const struct pios_board_info *bdinfo = &pios_board_info_blob;
bool coptercontrol = bdinfo->board_type == 0x04; 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_INS13GPSOUTDOOR:
navCapableFusion = true;
break;
default:
navCapableFusion = false;
}
#else
const bool navCapableFusion = false;
#endif
// Classify airframe type // Classify airframe type
bool multirotor = true; bool multirotor;
uint8_t airframe_type; uint8_t airframe_type;
SystemSettingsAirframeTypeGet(&airframe_type); SystemSettingsAirframeTypeGet(&airframe_type);
@ -93,82 +117,50 @@ int32_t configuration_check()
for (uint32_t i = 0; i < num_modes; i++) { for (uint32_t i = 0; i < num_modes; i++) {
switch (modes[i]) { switch (modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) { ADDSEVERITY(!multirotor);
severity = SYSTEMALARMS_ALARM_ERROR;
}
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor, coptercontrol) : severity; ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor, coptercontrol) : severity; ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor, coptercontrol) : severity; ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(4, multirotor, coptercontrol) : severity; ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(5, multirotor, coptercontrol) : severity; ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(6, multirotor, coptercontrol) : severity; ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
break; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE)) { ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE));
severity = SYSTEMALARMS_ALARM_ERROR;
}
break;
case FLIGHTMODESETTINGS_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 FLIGHTMODESETTINGS_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 FLIGHTMODESETTINGS_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; break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) { {
severity = SYSTEMALARMS_ALARM_ERROR;
} else {
// Revo supports PathPlanner and that must be OK or we are not sane // Revo supports PathPlanner and that must be OK or we are not sane
// PathPlan alarm is uninitialized if not running // PathPlan alarm is uninitialized if not running
// PathPlan alarm is warning or error if the flightplan is invalid // PathPlan alarm is warning or error if the flightplan is invalid
SystemAlarmsAlarmData alarms; SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms); SystemAlarmsAlarmGet(&alarms);
if (alarms.PathPlan != SYSTEMALARMS_ALARM_OK) { ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
severity = SYSTEMALARMS_ALARM_ERROR;
} }
} // intentionally no break as this also needs pathfollower
break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) { ADDSEVERITY(!coptercontrol);
severity = SYSTEMALARMS_ALARM_ERROR; ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER));
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) { ADDSEVERITY(navCapableFusion);
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
}
break; break;
default: default:
// Uncovered modes are automatically an error // Uncovered modes are automatically an error
severity = SYSTEMALARMS_ALARM_ERROR; ADDSEVERITY(false);
} }
// mark the first encountered erroneous setting in status and substatus // mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
@ -191,9 +183,9 @@ int32_t configuration_check()
* Checks the stabiliation settings for a paritcular mode and makes * Checks the stabiliation settings for a paritcular mode and makes
* sure it is appropriate for the airframe * sure it is appropriate for the airframe
* @param[in] index Which stabilization mode to check * @param[in] index Which stabilization mode to check
* @returns SYSTEMALARMS_ALARM_OK or SYSTEMALARMS_ALARM_ERROR * @returns true or false
*/ */
static int32_t check_stabilization_settings(int index, bool multirotor, bool coptercontrol) static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
{ {
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM]; uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
@ -218,7 +210,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
FlightModeSettingsStabilization6SettingsArrayGet(modes); FlightModeSettingsStabilization6SettingsArrayGet(modes);
break; break;
default: default:
return SYSTEMALARMS_ALARM_ERROR; return false;
} }
// For multirotors verify that roll/pitch/yaw are not set to "none" // For multirotors verify that roll/pitch/yaw are not set to "none"
@ -226,7 +218,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (multirotor) { if (multirotor) {
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) { for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) { if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
return SYSTEMALARMS_ALARM_ERROR; return false;
} }
} }
} }
@ -236,7 +228,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) { ) {
return SYSTEMALARMS_ALARM_ERROR; return false;
} }
} }
@ -245,7 +237,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) { ) {
return SYSTEMALARMS_ALARM_ERROR; return false;
} }
} }
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
@ -253,7 +245,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) { )) {
return SYSTEMALARMS_ALARM_ERROR; return false;
} }
// Warning: This assumes that certain conditions in the XML file are met. That // Warning: This assumes that certain conditions in the XML file are met. That
@ -261,5 +253,5 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
// (this is checked at compile time by static constraint manualcontrol.h) // (this is checked at compile time by static constraint manualcontrol.h)
return SYSTEMALARMS_ALARM_OK; return true;
} }