rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-21 13:28:58 +01:00
Code Issues Releases Activity

OP-1363 cleanups to sanitychecks

Browse Source
This commit is contained in:
Corvus Corax 2014-06-01 20:28:38 +02:00
parent 63b08ec129
commit aa6c5f9d15
1 changed files with 37 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
flight/libraries/sanitycheck.c
View File

@ -40,6 +40,10 @@
#include <revosettings.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:
* 1. If a flight mode switch allows autotune and autotune module not running
@ -47,7 +51,7 @@
****************************/
// ! 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
@ -113,100 +117,50 @@ int32_t configuration_check()
for (uint32_t i = 0; i < num_modes; i++) {
switch (modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
if (multirotor) {
severity = SYSTEMALARMS_ALARM_ERROR;
}
ADDSEVERITY(!multirotor);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(1, multirotor, coptercontrol) : severity;
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(2, multirotor, coptercontrol) : severity;
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(3, multirotor, coptercontrol) : severity;
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(4, multirotor, coptercontrol) : severity;
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(5, multirotor, coptercontrol) : severity;
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
severity = (severity == SYSTEMALARMS_ALARM_OK) ? check_stabilization_settings(6, multirotor, coptercontrol) : severity;
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
if (!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;
} else if (!navCapableFusion) {
// we need nav capable fusion algorithm
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;
} else if (!navCapableFusion) {
// we need nav capable fusion algorithm
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;
} else if (!navCapableFusion) {
// we need nav capable fusion algorithm
severity = SYSTEMALARMS_ALARM_ERROR;
}
ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports PathPlanner
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!navCapableFusion) {
// we need nav capable fusion algorithm
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;
{
// 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_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
if (coptercontrol) {
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER)) {
// Revo supports ReturnToBase
severity = SYSTEMALARMS_ALARM_ERROR;
} else if (!navCapableFusion) {
// we need nav capable fusion algorithm
severity = SYSTEMALARMS_ALARM_ERROR;
}
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_PATHFOLLOWER));
ADDSEVERITY(navCapableFusion);
break;
default:
// Uncovered modes are automatically an error
severity = SYSTEMALARMS_ALARM_ERROR;
ADDSEVERITY(false);
}
// mark the first encountered erroneous setting in status and substatus
if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
@ -229,9 +183,9 @@ int32_t configuration_check()
* 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_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];
@ -256,7 +210,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
FlightModeSettingsStabilization6SettingsArrayGet(modes);
break;
default:
return SYSTEMALARMS_ALARM_ERROR;
return false;
}
// For multirotors verify that roll/pitch/yaw are not set to "none"
@ -264,7 +218,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (multirotor) {
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
return SYSTEMALARMS_ALARM_ERROR;
return false;
}
}
}
@ -274,7 +228,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) {
return SYSTEMALARMS_ALARM_ERROR;
return false;
}
}
@ -283,7 +237,7 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_VERTICALVELOCITY
) {
return SYSTEMALARMS_ALARM_ERROR;
return false;
}
}
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
@ -291,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_CRUISECONTROL
)) {
return SYSTEMALARMS_ALARM_ERROR;
return false;
}
// Warning: This assumes that certain conditions in the XML file are met. That
@ -299,5 +253,5 @@ static int32_t check_stabilization_settings(int index, bool multirotor, bool cop
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
// (this is checked at compile time by static constraint manualcontrol.h)
return SYSTEMALARMS_ALARM_OK;
return true;
}