flight/libraries/sanitycheck.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilot System OpenPilot System
   4  * @{
   5  * @addtogroup OpenPilot Libraries OpenPilot System Libraries
   6  * @{
   7  * @file       sanitycheck.c
   8  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
   9  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
  10  * @brief      Utilities to validate a flight configuration
  11  * @see        The GNU Public License (GPL) Version 3
  12  *
  13  *****************************************************************************/
  14 /*
  15  * This program is free software; you can redistribute it and/or modify
  16  * it under the terms of the GNU General Public License as published by
  17  * the Free Software Foundation; either version 3 of the License, or
  18  * (at your option) any later version.
  19  *
  20  * This program is distributed in the hope that it will be useful, but
  21  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  22  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  23  * for more details.
  24  *
  25  * You should have received a copy of the GNU General Public License along
  26  * with this program; if not, write to the Free Software Foundation, Inc.,
  27  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  28  */
  29
  30 #include <openpilot.h>
  31 #include <pios_board_info.h>
  32
  33 // Private includes
  34 #include "inc/sanitycheck.h"
  35
  36 // UAVOs
  37 #include <manualcontrolsettings.h>
  38 #include <flightmodesettings.h>
  39 #include <systemsettings.h>
  40 #include <stabilizationsettings.h>
  41 #include <systemalarms.h>
  42 #include <revosettings.h>
  43 #include <positionstate.h>
  44
  45 // a number of useful macros
  46 #define ADDSEVERITY(check)                                  severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
  47 #define ADDEXTENDEDALARMSTATUS(error_code, error_substatus) if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) { alarmstatus = (error_code); alarmsubstatus = (error_substatus); }
  48
  49 // private types
  50 typedef struct SANITYCHECK_CustomHookInstance {
  51     SANITYCHECK_CustomHook_function *hook;
  52     struct SANITYCHECK_CustomHookInstance *next;
  53     bool enabled;
  54 } SANITYCHECK_CustomHookInstance;
  55
  56 // ! Check a stabilization mode switch position for safety
  57 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted);
  58
  59 SANITYCHECK_CustomHookInstance *hooks = 0;
  60
  61 /**
  62  * Run a preflight check over the hardware configuration
  63  * and currently active modules
  64  */
  65 int32_t configuration_check()
  66 {
  67     int32_t severity = SYSTEMALARMS_ALARM_OK;
  68     SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
  69     uint8_t alarmsubstatus = 0;
  70     // Get board type
  71     const struct pios_board_info *bdinfo = &pios_board_info_blob;
  72     bool coptercontrol     = bdinfo->board_type == 0x04;
  73
  74     // Classify navigation capability
  75 #ifdef REVOLUTION
  76     RevoSettingsInitialize();
  77     RevoSettingsFusionAlgorithmOptions revoFusion;
  78     RevoSettingsFusionAlgorithmGet(&revoFusion);
  79     bool navCapableFusion;
  80     switch (revoFusion) {
  81     case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
  82     case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
  83     case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:
  84         navCapableFusion = true;
  85         break;
  86     default:
  87         navCapableFusion = false;
  88         // check for hitl.  hitl allows to feed position and velocity state via
  89         // telemetry, this makes nav possible even with an unsuited algorithm
  90         if (PositionStateHandle()) {
  91             if (PositionStateReadOnly()) {
  92                 navCapableFusion = true;
  93             }
  94         }
  95     }
  96 #else /* ifdef REVOLUTION */
  97     const bool navCapableFusion = false;
  98 #endif /* ifdef REVOLUTION */
  99
 100
 101     // Classify airframe type
 102     bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
 103
 104
 105     // For each available flight mode position sanity check the available
 106     // modes
 107     uint8_t num_modes;
 108     FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
 109     StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 110     ManualControlSettingsFlightModeNumberGet(&num_modes);
 111     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
 112     FlightModeSettingsFlightModePositionGet(modes);
 113
 114     for (uint32_t i = 0; i < num_modes; i++) {
 115         uint8_t gps_assisted = FlightModeAssistMap[i];
 116         if (gps_assisted) {
 117             ADDSEVERITY(!coptercontrol);
 118             ADDSEVERITY(multirotor);
 119             ADDSEVERITY(navCapableFusion);
 120         }
 121
 122         switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
 123         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
 124             ADDSEVERITY(!gps_assisted);
 125             ADDSEVERITY(!multirotor);
 126             break;
 127         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
 128             ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol, gps_assisted));
 129             break;
 130         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
 131             ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol, gps_assisted));
 132             break;
 133         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
 134             ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol, gps_assisted));
 135             break;
 136         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
 137             ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol, gps_assisted));
 138             break;
 139         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
 140             ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol, gps_assisted));
 141             break;
 142         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
 143             ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol, gps_assisted));
 144             break;
 145         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
 146         {
 147             ADDSEVERITY(!gps_assisted);
 148         }
 149         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
 150         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYROAM:
 151         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
 152         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
 153             ADDSEVERITY(!coptercontrol);
 154             ADDSEVERITY(navCapableFusion);
 155             break;
 156
 157         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
 158         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
 159         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
 160         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
 161         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
 162         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
 163             ADDSEVERITY(!gps_assisted);
 164             ADDSEVERITY(!coptercontrol);
 165             ADDSEVERITY(navCapableFusion);
 166             break;
 167 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 168         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
 169             ADDSEVERITY(!gps_assisted);
 170             // it would be fun to try autotune on a fixed wing
 171             // but that should only be attempted by devs at first
 172             ADDSEVERITY(multirotor);
 173             break;
 174 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 175         default:
 176             // Uncovered modes are automatically an error
 177             ADDSEVERITY(false);
 178         }
 179         // mark the first encountered erroneous setting in status and substatus
 180         if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
 181             alarmstatus    = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
 182             alarmsubstatus = i;
 183         }
 184     }
 185
 186
 187     // Check throttle/collective channel range for valid configuration of input for critical control
 188     SystemSettingsThrustControlOptions thrustType;
 189     SystemSettingsThrustControlGet(&thrustType);
 190     ManualControlSettingsChannelMinData channelMin;
 191     ManualControlSettingsChannelMaxData channelMax;
 192     ManualControlSettingsChannelMinGet(&channelMin);
 193     ManualControlSettingsChannelMaxGet(&channelMax);
 194     switch (thrustType) {
 195     case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
 196         ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
 197         ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
 198         break;
 199     case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
 200         ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
 201         ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
 202         break;
 203     default:
 204         break;
 205     }
 206
 207     // query sanity check hooks
 208     if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
 209         SANITYCHECK_CustomHookInstance *instance = NULL;
 210         LL_FOREACH(hooks, instance) {
 211             if (instance->enabled) {
 212                 alarmstatus = instance->hook();
 213                 if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
 214                     severity = SYSTEMALARMS_ALARM_CRITICAL;
 215                     break;
 216                 }
 217             }
 218         }
 219     }
 220
 221     FlightModeSettingsDisableSanityChecksOptions checks_disabled;
 222     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
 223     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
 224         severity = SYSTEMALARMS_ALARM_WARNING;
 225     }
 226
 227     if (severity != SYSTEMALARMS_ALARM_OK) {
 228         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
 229     } else {
 230         AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
 231     }
 232
 233     return 0;
 234 }
 235
 236 /**
 237  * Checks the stabilization settings for a particular mode and makes
 238  * sure it is appropriate for the airframe
 239  * @param[in] index Which stabilization mode to check
 240  * @returns true or false
 241  */
 242 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted)
 243 {
 244     uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 245
 246     // Get the different axis modes for this switch position
 247     switch (index) {
 248     case 1:
 249         FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
 250         break;
 251     case 2:
 252         FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
 253         break;
 254     case 3:
 255         FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
 256         break;
 257     case 4:
 258         FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
 259         break;
 260     case 5:
 261         FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
 262         break;
 263     case 6:
 264         FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
 265         break;
 266     default:
 267         return false;
 268     }
 269
 270     // For multirotors verify that roll/pitch/yaw are not set to "none"
 271     // (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
 272     if (multirotor) {
 273         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 274             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
 275                 return false;
 276             }
 277         }
 278     }
 279
 280     if (gpsassisted) {
 281         // For multirotors verify that roll/pitch are either attitude or rattitude
 282         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
 283             if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
 284                   modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
 285                 return false;
 286             }
 287         }
 288     }
 289
 290
 291     // coptercontrol cannot do altitude holding
 292     if (coptercontrol) {
 293         if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 294             || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 295             ) {
 296             return false;
 297         }
 298     }
 299
 300     // check that thrust modes are only set to thrust axis
 301     for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 302         if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 303             || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 304             ) {
 305             return false;
 306         }
 307     }
 308     if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
 309           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 310           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 311           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
 312           )) {
 313         return false;
 314     }
 315
 316     // if cruise control, ensure Acro+ is not set
 317     if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
 318         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
 319             // Do not allow Acro+, attitude estimation is not safe.
 320             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO) {
 321                 return false;
 322             }
 323         }
 324     }
 325
 326     // Warning: This assumes that certain conditions in the XML file are met.  That
 327     // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
 328     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
 329     // (this is checked at compile time by static constraint manualcontrol.h)
 330
 331
 332     return true;
 333 }
 334
 335 FrameType_t GetCurrentFrameType()
 336 {
 337     SystemSettingsAirframeTypeOptions airframe_type;
 338
 339     SystemSettingsAirframeTypeGet(&airframe_type);
 340     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
 341     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
 342     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
 343     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
 344     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
 345     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
 346     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
 347     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH:
 348     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
 349     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
 350     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
 351     case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
 352     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
 353         return FRAME_TYPE_MULTIROTOR;
 354
 355     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
 356     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
 357     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
 358         return FRAME_TYPE_FIXED_WING;
 359
 360     case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
 361         return FRAME_TYPE_HELI;
 362
 363     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
 364     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
 365     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
 366     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEBOAT:
 367     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIALBOAT:
 368         return FRAME_TYPE_GROUND;
 369
 370     case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
 371     case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
 372         return FRAME_TYPE_CUSTOM;
 373     }
 374     // anyway it should not reach here
 375     return FRAME_TYPE_CUSTOM;
 376 }
 377
 378 void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
 379 {
 380     PIOS_Assert(hook);
 381     SANITYCHECK_CustomHookInstance *instance = NULL;
 382
 383     // Check whether there is an existing instance and enable it
 384     LL_FOREACH(hooks, instance) {
 385         if (instance->hook == hook) {
 386             instance->enabled = true;
 387             return;
 388         }
 389     }
 390
 391     // No existing instance found, attach this new one
 392     instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
 393     PIOS_Assert(instance);
 394     instance->hook    = hook;
 395     instance->next    = NULL;
 396     instance->enabled = true;
 397     LL_APPEND(hooks, instance);
 398 }
 399
 400 void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
 401 {
 402     if (!hooks) {
 403         return;
 404     }
 405     SANITYCHECK_CustomHookInstance *instance = NULL;
 406     LL_FOREACH(hooks, instance) {
 407         if (instance->hook == hook) {
 408             instance->enabled = false;
 409             return;
 410         }
 411     }
 412 }