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 OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
   9  * @brief      Utilities to validate a flight configuration
  10  * @see        The GNU Public License (GPL) Version 3
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 #include <openpilot.h>
  30 #include <pios_board_info.h>
  31
  32 // Private includes
  33 #include "inc/sanitycheck.h"
  34
  35 // UAVOs
  36 #include <manualcontrolsettings.h>
  37 #include <flightmodesettings.h>
  38 #include <systemsettings.h>
  39 #include <stabilizationsettings.h>
  40 #include <systemalarms.h>
  41 #include <revosettings.h>
  42 #include <positionstate.h>
  43 #include <taskinfo.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         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
  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         default:
 167             // Uncovered modes are automatically an error
 168             ADDSEVERITY(false);
 169         }
 170         // mark the first encountered erroneous setting in status and substatus
 171         if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
 172             alarmstatus    = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
 173             alarmsubstatus = i;
 174         }
 175     }
 176
 177
 178     // Check throttle/collective channel range for valid configuration of input for critical control
 179     SystemSettingsThrustControlOptions thrustType;
 180     SystemSettingsThrustControlGet(&thrustType);
 181     ManualControlSettingsChannelMinData channelMin;
 182     ManualControlSettingsChannelMaxData channelMax;
 183     ManualControlSettingsChannelMinGet(&channelMin);
 184     ManualControlSettingsChannelMaxGet(&channelMax);
 185     switch (thrustType) {
 186     case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE:
 187         ADDSEVERITY(fabsf(channelMax.Throttle - channelMin.Throttle) > 300.0f);
 188         ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
 189         break;
 190     case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE:
 191         ADDSEVERITY(fabsf(channelMax.Collective - channelMin.Collective) > 300.0f);
 192         ADDEXTENDEDALARMSTATUS(SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE, 0);
 193         break;
 194     default:
 195         break;
 196     }
 197
 198     // query sanity check hooks
 199     if (severity < SYSTEMALARMS_ALARM_CRITICAL) {
 200         SANITYCHECK_CustomHookInstance *instance = NULL;
 201         LL_FOREACH(hooks, instance) {
 202             if (instance->enabled) {
 203                 alarmstatus = instance->hook();
 204                 if (alarmstatus != SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE) {
 205                     severity = SYSTEMALARMS_ALARM_CRITICAL;
 206                     break;
 207                 }
 208             }
 209         }
 210     }
 211
 212     FlightModeSettingsDisableSanityChecksOptions checks_disabled;
 213     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
 214     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
 215         severity = SYSTEMALARMS_ALARM_WARNING;
 216     }
 217
 218     if (severity != SYSTEMALARMS_ALARM_OK) {
 219         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
 220     } else {
 221         AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
 222     }
 223
 224     return 0;
 225 }
 226
 227 /**
 228  * Checks the stabiliation settings for a paritcular mode and makes
 229  * sure it is appropriate for the airframe
 230  * @param[in] index Which stabilization mode to check
 231  * @returns true or false
 232  */
 233 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted)
 234 {
 235     uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 236
 237     // Get the different axis modes for this switch position
 238     switch (index) {
 239     case 1:
 240         FlightModeSettingsStabilization1SettingsArrayGet((FlightModeSettingsStabilization1SettingsOptions *)modes);
 241         break;
 242     case 2:
 243         FlightModeSettingsStabilization2SettingsArrayGet((FlightModeSettingsStabilization2SettingsOptions *)modes);
 244         break;
 245     case 3:
 246         FlightModeSettingsStabilization3SettingsArrayGet((FlightModeSettingsStabilization3SettingsOptions *)modes);
 247         break;
 248     case 4:
 249         FlightModeSettingsStabilization4SettingsArrayGet((FlightModeSettingsStabilization4SettingsOptions *)modes);
 250         break;
 251     case 5:
 252         FlightModeSettingsStabilization5SettingsArrayGet((FlightModeSettingsStabilization5SettingsOptions *)modes);
 253         break;
 254     case 6:
 255         FlightModeSettingsStabilization6SettingsArrayGet((FlightModeSettingsStabilization6SettingsOptions *)modes);
 256         break;
 257     default:
 258         return false;
 259     }
 260
 261     // For multirotors verify that roll/pitch/yaw are not set to "none"
 262     // (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
 263     if (multirotor) {
 264         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 265             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
 266                 return false;
 267             }
 268         }
 269     }
 270
 271     if (gpsassisted) {
 272         // For multirotors verify that roll/pitch are either attitude or rattitude
 273         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
 274             if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE ||
 275                   modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) {
 276                 return false;
 277             }
 278         }
 279     }
 280
 281
 282     // coptercontrol cannot do altitude holding
 283     if (coptercontrol) {
 284         if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 285             || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 286             ) {
 287             return false;
 288         }
 289     }
 290
 291     // check that thrust modes are only set to thrust axis
 292     for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 293         if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 294             || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 295             ) {
 296             return false;
 297         }
 298     }
 299     if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
 300           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 301           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 302           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
 303           )) {
 304         return false;
 305     }
 306
 307
 308     // if cruise control, ensure rate or acro are not set
 309     if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL) {
 310         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) {
 311             if ((modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATE ||
 312                  modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ACRO)) {
 313                 return false;
 314             }
 315         }
 316     }
 317
 318     // Warning: This assumes that certain conditions in the XML file are met.  That
 319     // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
 320     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
 321     // (this is checked at compile time by static constraint manualcontrol.h)
 322
 323
 324     return true;
 325 }
 326
 327 FrameType_t GetCurrentFrameType()
 328 {
 329     SystemSettingsAirframeTypeOptions airframe_type;
 330
 331     SystemSettingsAirframeTypeGet(&airframe_type);
 332     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
 333     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
 334     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
 335     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
 336     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
 337     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
 338     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
 339     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH:
 340     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
 341     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
 342     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
 343     case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
 344     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
 345         return FRAME_TYPE_MULTIROTOR;
 346
 347     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
 348     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
 349     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
 350         return FRAME_TYPE_FIXED_WING;
 351
 352     case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
 353         return FRAME_TYPE_HELI;
 354
 355     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
 356     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
 357     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
 358         return FRAME_TYPE_GROUND;
 359
 360     case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
 361     case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
 362         return FRAME_TYPE_CUSTOM;
 363     }
 364     // anyway it should not reach here
 365     return FRAME_TYPE_CUSTOM;
 366 }
 367
 368 void SANITYCHECK_AttachHook(SANITYCHECK_CustomHook_function *hook)
 369 {
 370     PIOS_Assert(hook);
 371     SANITYCHECK_CustomHookInstance *instance = NULL;
 372
 373     // Check whether there is an existing instance and enable it
 374     LL_FOREACH(hooks, instance) {
 375         if (instance->hook == hook) {
 376             instance->enabled = true;
 377             return;
 378         }
 379     }
 380
 381     // No existing instance found, attach this new one
 382     instance = (SANITYCHECK_CustomHookInstance *)pios_malloc(sizeof(SANITYCHECK_CustomHookInstance));
 383     PIOS_Assert(instance);
 384     instance->hook    = hook;
 385     instance->next    = NULL;
 386     instance->enabled = true;
 387     LL_APPEND(hooks, instance);
 388 }
 389
 390 void SANITYCHECK_DetachHook(SANITYCHECK_CustomHook_function *hook)
 391 {
 392     if (!hooks) {
 393         return;
 394     }
 395     SANITYCHECK_CustomHookInstance *instance = NULL;
 396     LL_FOREACH(hooks, instance) {
 397         if (instance->hook == hook) {
 398             instance->enabled = false;
 399             return;
 400         }
 401     }
 402 }