rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-30 08:24:11 +01:00
Code Issues Releases Activity
25c8d43db3
LibrePilot/flight/libraries/sanitycheck.c

375 lines
14 KiB
C
Raw Blame History

/**
******************************************************************************
* @addtogroup OpenPilot System OpenPilot System
* @{
* @addtogroup OpenPilot Libraries OpenPilot System Libraries
* @{
* @file sanitycheck.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
* @brief Utilities to validate a flight configuration
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
* for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include <openpilot.h>
#include <pios_board_info.h>
// Private includes
#include "inc/sanitycheck.h"
// UAVOs
#include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <systemsettings.h>
#include <stabilizationsettings.h>
#include <systemalarms.h>
#include <revosettings.h>
#include <positionstate.h>
#include <taskinfo.h>
// a number of useful macros
#define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
// private types
typedef struct SANITYCHECK_CustomHookInstance {
SANITYCHECK_CustomHook_function *hook;
struct SANITYCHECK_CustomHookInstance *next;
bool enabled;
} SANITYCHECK_CustomHookInstance;
// ! Check a stabilization mode switch position for safety
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
SANITYCHECK_CustomHookInstance *hooks = 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 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];
uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
ManualControlSettingsFlightModeNumberGet(&num_modes);
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
FlightModeSettingsFlightModePositionGet(modes);
for (uint32_t i = 0; i < num_modes; i++) {
uint8_t gps_assisted = FlightModeAssistMap[i];
if (gps_assisted) {
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(multirotor);
ADDSEVERITY(navCapableFusion);
}
switch (modes[i]) {
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
ADDSEVERITY(!gps_assisted);
ADDSEVERITY(!multirotor);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol, gps_assisted));
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol, gps_assisted));
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);
ADDSEVERITY(!gps_assisted);
}
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
ADDSEVERITY(!coptercontrol);
ADDSEVERITY(navCapableFusion);
break;
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
ADDSEVERITY(!gps_assisted);
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;
}
}
// query sanity check hooks
if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
SANITYCHECK_CustomHookInstance *instance = NULL;
LL_FOREACH(hooks, instance) {
if (instance->enabled) {
alarmstatus = instance->hook();
if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
severity = SYSTEMALARMS_ALARM_CRITICAL;
break;
}
}
}
}
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;
}
/**
* 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 true or false
*/
static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted)
{
uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
// Get the different axis modes for this switch position
switch (index) {
case 1:
FlightModeSettingsStabilization1SettingsArrayGet(modes);
break;
case 2:
FlightModeSettingsStabilization2SettingsArrayGet(modes);
break;
case 3:
FlightModeSettingsStabilization3SettingsArrayGet(modes);
break;
case 4:
FlightModeSettingsStabilization4SettingsArrayGet(modes);
break;
case 5:
FlightModeSettingsStabilization5SettingsArrayGet(modes);
break;
case 6:
FlightModeSettingsStabilization6SettingsArrayGet(modes);
break;
default:
return false;
}
// For multirotors verify that roll/pitch/yaw are not set to "none"
// (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
if (multirotor) {
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
return false;
}
}
}
if (gpsassisted) {
// For multirotors verify that roll/pitch are either attitude or rattitude
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
return false;
}
}
}
// coptercontrol cannot do altitude holding
if (coptercontrol) {
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
) {
return false;
}
}
// check that thrust modes are only set to thrust axis
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
) {
return false;
}
}
if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
|| modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
)) {
return false;
}
// Warning: This assumes that certain conditions in the XML file are met. That
// FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
// and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
// (this is checked at compile time by static constraint manualcontrol.h)
return true;
}
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_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;
}
void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
{
PIOS_Assert(hook);
SANITYCHECK_CustomHookInstance *instance = NULL;
// Check whether there is an existing instance and enable it
LL_FOREACH(hooks, instance) {
if (instance->hook == hook) {
instance->enabled = true;
return;
}
}
// No existing instance found, attach this new one
instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
PIOS_Assert(instance);
instance->hook = hook;
instance->next = NULL;
instance->enabled = true;
LL_APPEND(hooks, instance);
}
void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
{
if (!hooks) {
return;
}
SANITYCHECK_CustomHookInstance *instance = NULL;
LL_FOREACH(hooks, instance) {
if (instance->hook == hook) {
instance->enabled = false;
return;
}
}
}
Reference in New Issue View Git Blame Copy Permalink