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     RevoSettingsFusionAlgorithmOptions revoFusion;
  77     RevoSettingsFusionAlgorithmGet(&revoFusion);
  78     bool navCapableFusion;
  79     switch (revoFusion) {
  80     case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
  81     case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13:
  82     case REVOSETTINGS_FUSIONALGORITHM_GPSNAVIGATIONINS13CF:
  83         navCapableFusion = true;
  84         break;
  85     default:
  86         navCapableFusion = false;
  87         // check for hitl.  hitl allows to feed position and velocity state via
  88         // telemetry, this makes nav possible even with an unsuited algorithm
  89         if (PositionStateHandle()) {
  90             if (PositionStateReadOnly()) {
  91                 navCapableFusion = true;
  92             }
  93         }
  94     }
  95 #else /* ifdef REVOLUTION */
  96     const bool navCapableFusion = false;
  97 #endif /* ifdef REVOLUTION */
  98
  99
 100     // Classify airframe type
 101     bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
 102
 103
 104     // For each available flight mode position sanity check the available
 105     // modes
 106     uint8_t num_modes;
 107     FlightModeSettingsFlightModePositionOptions modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
 108     StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
 109     ManualControlSettingsFlightModeNumberGet(&num_modes);
 110     StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
 111     FlightModeSettingsFlightModePositionGet(modes);
 112
 113     for (uint32_t i = 0; i < num_modes; i++) {
 114         uint8_t gps_assisted = FlightModeAssistMap[i];
 115         if (gps_assisted) {
 116             ADDSEVERITY(!coptercontrol);
 117             ADDSEVERITY(multirotor);
 118             ADDSEVERITY(navCapableFusion);
 119         }
 120
 121         switch ((FlightModeSettingsFlightModePositionOptions)modes[i]) {
 122         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
 123             ADDSEVERITY(!gps_assisted);
 124             ADDSEVERITY(!multirotor);
 125             break;
 126         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
 127             ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol, gps_assisted));
 128             break;
 129         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
 130             ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol, gps_assisted));
 131             break;
 132         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
 133             ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol, gps_assisted));
 134             break;
 135         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
 136             ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol, gps_assisted));
 137             break;
 138         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
 139             ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol, gps_assisted));
 140             break;
 141         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
 142             ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol, gps_assisted));
 143             break;
 144         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
 145         {
 146             ADDSEVERITY(!gps_assisted);
 147         }
 148         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
 149         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYROAM:
 150         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
 151         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF:
 152             ADDSEVERITY(!coptercontrol);
 153             ADDSEVERITY(navCapableFusion);
 154             break;
 155
 156         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK:
 157         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH:
 158         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_ABSOLUTEPOSITION:
 159         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
 160         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
 161         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
 162             ADDSEVERITY(!gps_assisted);
 163             ADDSEVERITY(!coptercontrol);
 164             ADDSEVERITY(navCapableFusion);
 165             break;
 166 #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
 167         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
 168             ADDSEVERITY(!gps_assisted);
 169             // it would be fun to try autotune on a fixed wing
 170             // but that should only be attempted by devs at first
 171             ADDSEVERITY(multirotor);
 172             break;
 173 #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
 174         default:
 175             // Uncovered modes are automatically an error
 176             ADDSEVERITY(false);
 177         }
 178         // mark the first encountered erroneous setting in status and substatus
 179         if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
 180             alarmstatus    = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
 181             alarmsubstatus = i;
 182         }
 183     }
 184
 185
 186     // Check throttle/collective channel range for valid configuration of input for critical control
 187     SystemSettingsThrustControlOptions thrustType;
 188     SystemSettingsThrustControlGet(&thrustType);
 189     ManualControlSettingsChannelMinData channelMin;
 190     ManualControlSettingsChannelMaxData channelMax;
 191     ManualControlSettingsChannelMinGet(&channelMin);
 192     ManualControlSettingsChannelMaxGet(&channelMax);
 193     switch (thrustType) {
 194     case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
 195         ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
 196         ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
 197         break;
 198     case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
 199         ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
 200         ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
 201         break;
 202     default:
 203         break;
 204     }
 205
 206     // query sanity check hooks
 207     if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
 208         SANITYCHECK_CustomHookInstance *instance = NULL;
 209         LL_FOREACH(hooks, instance) {
 210             if (instance->enabled) {
 211                 alarmstatus = instance->hook();
 212                 if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
 213                     severity = SYSTEMALARMS_ALARM_CRITICAL;
 214                     break;
 215                 }
 216             }
 217         }
 218     }
 219
 220     FlightModeSettingsDisableSanityChecksOptions checks_disabled;
 221     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
 222     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
 223         severity = SYSTEMALARMS_ALARM_WARNING;
 224     }
 225
 226     if (severity != SYSTEMALARMS_ALARM_OK) {
 227         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
 228     } else {
 229         AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
 230     }
 231
 232     return 0;
 233 }
 234
 235 /**
 236  * Checks the stabilization settings for a particular mode and makes
 237  * sure it is appropriate for the airframe
 238  * @param[in] index Which stabilization mode to check
 239  * @returns true or false
 240  */
 241 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted)
 242 {
 243     uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 244
 245     // Get the different axis modes for this switch position
 246     switch (index) {
 247     case 1:
 248         FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
 249         break;
 250     case 2:
 251         FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
 252         break;
 253     case 3:
 254         FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
 255         break;
 256     case 4:
 257         FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
 258         break;
 259     case 5:
 260         FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
 261         break;
 262     case 6:
 263         FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
 264         break;
 265     default:
 266         return false;
 267     }
 268
 269     // For multirotors verify that roll/pitch/yaw are not set to "none"
 270     // (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
 271     if (multirotor) {
 272         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 273             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
 274                 return false;
 275             }
 276         }
 277     }
 278
 279     if (gpsassisted) {
 280         // For multirotors verify that roll/pitch are either attitude or rattitude
 281         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
 282             if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
 283                   modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
 284                 return false;
 285             }
 286         }
 287     }
 288
 289
 290     // coptercontrol cannot do altitude holding
 291     if (coptercontrol) {
 292         if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 293             || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 294             ) {
 295             return false;
 296         }
 297     }
 298
 299     // check that thrust modes are only set to thrust axis
 300     for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 301         if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 302             || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 303             ) {
 304             return false;
 305         }
 306     }
 307     if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
 308           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 309           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 310           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
 311           )) {
 312         return false;
 313     }
 314
 315     // if cruise control, ensure Acro+ is not set
 316     if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
 317         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
 318             // Do not allow Acro+, attitude estimation is not safe.
 319             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO) {
 320                 return false;
 321             }
 322         }
 323     }
 324
 325     // Warning: This assumes that certain conditions in the XML file are met.  That
 326     // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
 327     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
 328     // (this is checked at compile time by static constraint manualcontrol.h)
 329
 330
 331     return true;
 332 }
 333
 334 FrameType_t GetCurrentFrameType()
 335 {
 336     SystemSettingsAirframeTypeOptions airframe_type;
 337
 338     SystemSettingsAirframeTypeGet(&airframe_type);
 339     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
 340     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
 341     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
 342     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
 343     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
 344     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
 345     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
 346     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH:
 347     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
 348     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
 349     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
 350     case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
 351     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
 352         return FRAME_TYPE_MULTIROTOR;
 353
 354     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
 355     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
 356     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
 357         return FRAME_TYPE_FIXED_WING;
 358
 359     case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
 360         return FRAME_TYPE_HELI;
 361
 362     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
 363     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
 364     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
 365     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEBOAT:
 366     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIALBOAT:
 367         return FRAME_TYPE_GROUND;
 368
 369     case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
 370     case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
 371         return FRAME_TYPE_CUSTOM;
 372     }
 373     // anyway it should not reach here
 374     return FRAME_TYPE_CUSTOM;
 375 }
 376
 377 void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
 378 {
 379     PIOS_Assert(hook);
 380     SANITYCHECK_CustomHookInstance *instance = NULL;
 381
 382     // Check whether there is an existing instance and enable it
 383     LL_FOREACH(hooks, instance) {
 384         if (instance->hook == hook) {
 385             instance->enabled = true;
 386             return;
 387         }
 388     }
 389
 390     // No existing instance found, attach this new one
 391     instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
 392     PIOS_Assert(instance);
 393     instance->hook    = hook;
 394     instance->next    = NULL;
 395     instance->enabled = true;
 396     LL_APPEND(hooks, instance);
 397 }
 398
 399 void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
 400 {
 401     if (!hooks) {
 402         return;
 403     }
 404     SANITYCHECK_CustomHookInstance *instance = NULL;
 405     LL_FOREACH(hooks, instance) {
 406         if (instance->hook == hook) {
 407             instance->enabled = false;
 408             return;
 409         }
 410     }
 411 }