mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
c9188fc5a2
Theothercliff/lp 76_port_autotune_from_dronin
411 lines
16 KiB
C
411 lines
16 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilot System OpenPilot System
|
|
* @{
|
|
* @addtogroup OpenPilot Libraries OpenPilot System Libraries
|
|
* @{
|
|
* @file sanitycheck.c
|
|
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
|
|
* 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))
|
|
#define ADDEXTENDEDALARMSTATUS(error_code, error_substatus) if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = (error_code); alarmsubstatus = (error_substatus); }
|
|
|
|
// 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();
|
|
RevoSettingsFusionAlgorithmOptions 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;
|
|
FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
|
|
StabilizationSettingsFlightModeAssistMapOptions 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 ((FlightModeSettingsFlightModePositionOptions)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:
|
|
{
|
|
ADDSEVERITY(!gps_assisted);
|
|
}
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYROAM:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
|
|
ADDSEVERITY(!coptercontrol);
|
|
ADDSEVERITY(navCapableFusion);
|
|
break;
|
|
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
|
|
ADDSEVERITY(!gps_assisted);
|
|
ADDSEVERITY(!coptercontrol);
|
|
ADDSEVERITY(navCapableFusion);
|
|
break;
|
|
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
|
|
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
|
|
ADDSEVERITY(!gps_assisted);
|
|
// it would be fun to try autotune on a fixed wing
|
|
// but that should only be attempted by devs at first
|
|
ADDSEVERITY(multirotor);
|
|
break;
|
|
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
|
|
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;
|
|
}
|
|
}
|
|
|
|
|
|
// Check throttle/collective channel range for valid configuration of input for critical control
|
|
SystemSettingsThrustControlOptions thrustType;
|
|
SystemSettingsThrustControlGet(&thrustType);
|
|
ManualControlSettingsChannelMinData channelMin;
|
|
ManualControlSettingsChannelMaxData channelMax;
|
|
ManualControlSettingsChannelMinGet(&channelMin);
|
|
ManualControlSettingsChannelMaxGet(&channelMax);
|
|
switch (thrustType) {
|
|
case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
|
|
ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
|
|
ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
|
|
break;
|
|
case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
|
|
ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
|
|
ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
FlightModeSettingsDisableSanityChecksOptions 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 stabilization settings for a particular 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((FlightModeSettingsStabilization1SettingsOptions *)modes);
|
|
break;
|
|
case 2:
|
|
FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
|
|
break;
|
|
case 3:
|
|
FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
|
|
break;
|
|
case 4:
|
|
FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
|
|
break;
|
|
case 5:
|
|
FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
|
|
break;
|
|
case 6:
|
|
FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)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;
|
|
}
|
|
|
|
// if cruise control, ensure Acro+ is not set
|
|
if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
|
|
for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
|
|
// Do not allow Acro+, attitude estimation is not safe.
|
|
if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO) {
|
|
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()
|
|
{
|
|
SystemSettingsAirframeTypeOptions 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;
|
|
}
|
|
}
|
|
}
|