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 <systemalarms.h>
  40 #include <revosettings.h>
  41 #include <positionstate.h>
  42 #include <taskinfo.h>
  43
  44 // a number of useful macros
  45 #define ADDSEVERITY(check) severity = (severity != SYSTEMALARMS_ALARM_OK ? severity : ((check) ? SYSTEMALARMS_ALARM_OK : SYSTEMALARMS_ALARM_CRITICAL))
  46
  47
  48 /****************************
  49 * Current checks:
  50 * 1. If a flight mode switch allows autotune and autotune module not running
  51 * 2. If airframe is a multirotor and either manual is available or a stabilization mode uses "none"
  52 ****************************/
  53
  54 // ! Check a stabilization mode switch position for safety
  55 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol);
  56
  57 /**
  58  * Run a preflight check over the hardware configuration
  59  * and currently active modules
  60  */
  61 int32_t configuration_check()
  62 {
  63     int32_t severity = SYSTEMALARMS_ALARM_OK;
  64     SystemAlarmsExtendedAlarmStatusOptions alarmstatus = SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE;
  65     uint8_t alarmsubstatus = 0;
  66     // Get board type
  67     const struct pios_board_info *bdinfo = &pios_board_info_blob;
  68     bool coptercontrol     = bdinfo->board_type == 0x04;
  69
  70     // Classify navigation capability
  71 #ifdef REVOLUTION
  72     RevoSettingsInitialize();
  73     uint8_t revoFusion;
  74     RevoSettingsFusionAlgorithmGet(&revoFusion);
  75     bool navCapableFusion;
  76     switch (revoFusion) {
  77     case REVOSETTINGS_FUSIONALGORITHM_COMPLEMENTARYMAGGPSOUTDOOR:
  78     case REVOSETTINGS_FUSIONALGORITHM_INS13GPSOUTDOOR:
  79         navCapableFusion = true;
  80         break;
  81     default:
  82         navCapableFusion = false;
  83         // check for hitl.  hitl allows to feed position and velocity state via
  84         // telemetry, this makes nav possible even with an unsuited algorithm
  85         if (PositionStateHandle()) {
  86             if (PositionStateReadOnly()) {
  87                 navCapableFusion = true;
  88             }
  89         }
  90     }
  91 #else
  92     const bool navCapableFusion = false;
  93 #endif /* ifdef REVOLUTION */
  94
  95
  96     // Classify airframe type
  97     bool multirotor = (GetCurrentFrameType() == FRAME_TYPE_MULTIROTOR);
  98
  99
 100     // For each available flight mode position sanity check the available
 101     // modes
 102     uint8_t num_modes;
 103     uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
 104     ManualControlSettingsFlightModeNumberGet(&num_modes);
 105     FlightModeSettingsFlightModePositionGet(modes);
 106
 107     for (uint32_t i = 0; i < num_modes; i++) {
 108         switch (modes[i]) {
 109         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL:
 110             ADDSEVERITY(!multirotor);
 111             break;
 112         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1:
 113             ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol));
 114             break;
 115         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2:
 116             ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol));
 117             break;
 118         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3:
 119             ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol));
 120             break;
 121         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4:
 122             ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol));
 123             break;
 124         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5:
 125             ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol));
 126             break;
 127         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6:
 128             ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol));
 129             break;
 130         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
 131             ADDSEVERITY(PIOS_TASK_MONITOR_IsRunning(TASKINFO_RUNNING_AUTOTUNE));
 132             break;
 133         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER:
 134         {
 135             // Revo supports PathPlanner and that must be OK or we are not sane
 136             // PathPlan alarm is uninitialized if not running
 137             // PathPlan alarm is warning or error if the flightplan is invalid
 138             SystemAlarmsAlarmData alarms;
 139             SystemAlarmsAlarmGet(&alarms);
 140             ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK);
 141         }
 142         // intentionally no break as this also needs pathfollower
 143         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD:
 144         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOFPV:
 145         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIOLOS:
 146         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONVARIONSEW:
 147         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND:
 148         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI:
 149         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE:
 150         case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE:
 151             ADDSEVERITY(!coptercontrol);
 152             ADDSEVERITY(navCapableFusion);
 153             break;
 154         default:
 155             // Uncovered modes are automatically an error
 156             ADDSEVERITY(false);
 157         }
 158         // mark the first encountered erroneous setting in status and substatus
 159         if ((severity != SYSTEMALARMS_ALARM_OK) && (alarmstatus == SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE)) {
 160             alarmstatus    = SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE;
 161             alarmsubstatus = i;
 162         }
 163     }
 164
 165     uint8_t checks_disabled;
 166     FlightModeSettingsDisableSanityChecksGet(&checks_disabled);
 167     if (checks_disabled == FLIGHTMODESETTINGS_DISABLESANITYCHECKS_TRUE) {
 168         severity = SYSTEMALARMS_ALARM_WARNING;
 169     }
 170
 171     if (severity != SYSTEMALARMS_ALARM_OK) {
 172         ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, severity, alarmstatus, alarmsubstatus);
 173     } else {
 174         AlarmsClear(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION);
 175     }
 176
 177     return 0;
 178 }
 179
 180 /**
 181  * Checks the stabiliation settings for a paritcular mode and makes
 182  * sure it is appropriate for the airframe
 183  * @param[in] index Which stabilization mode to check
 184  * @returns true or false
 185  */
 186 static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol)
 187 {
 188     uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM];
 189
 190     // Get the different axis modes for this switch position
 191     switch (index) {
 192     case 1:
 193         FlightModeSettingsStabilization1SettingsArrayGet(modes);
 194         break;
 195     case 2:
 196         FlightModeSettingsStabilization2SettingsArrayGet(modes);
 197         break;
 198     case 3:
 199         FlightModeSettingsStabilization3SettingsArrayGet(modes);
 200         break;
 201     case 4:
 202         FlightModeSettingsStabilization4SettingsArrayGet(modes);
 203         break;
 204     case 5:
 205         FlightModeSettingsStabilization5SettingsArrayGet(modes);
 206         break;
 207     case 6:
 208         FlightModeSettingsStabilization6SettingsArrayGet(modes);
 209         break;
 210     default:
 211         return false;
 212     }
 213
 214     // For multirotors verify that roll/pitch/yaw are not set to "none"
 215     // (why not? might be fun to test ones reactions ;) if you dare, set your frame to "custom"!
 216     if (multirotor) {
 217         for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 218             if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL) {
 219                 return false;
 220             }
 221         }
 222     }
 223
 224     // coptercontrol cannot do altitude holding
 225     if (coptercontrol) {
 226         if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 227             || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 228             ) {
 229             return false;
 230         }
 231     }
 232
 233     // check that thrust modes are only set to thrust axis
 234     for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST; i++) {
 235         if (modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 236             || modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 237             ) {
 238             return false;
 239         }
 240     }
 241     if (!(modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL
 242           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD
 243           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO
 244           || modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL
 245           )) {
 246         return false;
 247     }
 248
 249     // Warning: This assumes that certain conditions in the XML file are met.  That
 250     // FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL has the same numeric value for each channel
 251     // and is the same for STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL
 252     // (this is checked at compile time by static constraint manualcontrol.h)
 253
 254     return true;
 255 }
 256
 257 FrameType_t GetCurrentFrameType()
 258 {
 259     uint8_t airframe_type;
 260
 261     SystemSettingsAirframeTypeGet(&airframe_type);
 262     switch ((SystemSettingsAirframeTypeOptions)airframe_type) {
 263     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADX:
 264     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADP:
 265     case SYSTEMSETTINGS_AIRFRAMETYPE_QUADH:
 266     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXA:
 267     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTO:
 268     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOX:
 269     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAX:
 270     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXAH:
 271     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOV:
 272     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXP:
 273     case SYSTEMSETTINGS_AIRFRAMETYPE_HEXACOAX:
 274     case SYSTEMSETTINGS_AIRFRAMETYPE_TRI:
 275     case SYSTEMSETTINGS_AIRFRAMETYPE_OCTOCOAXX:
 276         return FRAME_TYPE_MULTIROTOR;
 277
 278     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWING:
 279     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGELEVON:
 280     case SYSTEMSETTINGS_AIRFRAMETYPE_FIXEDWINGVTAIL:
 281         return FRAME_TYPE_FIXED_WING;
 282
 283     case SYSTEMSETTINGS_AIRFRAMETYPE_HELICP:
 284         return FRAME_TYPE_HELI;
 285
 286     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLECAR:
 287     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEDIFFERENTIAL:
 288     case SYSTEMSETTINGS_AIRFRAMETYPE_GROUNDVEHICLEMOTORCYCLE:
 289         return FRAME_TYPE_GROUND;
 290
 291     case SYSTEMSETTINGS_AIRFRAMETYPE_VTOL:
 292     case SYSTEMSETTINGS_AIRFRAMETYPE_CUSTOM:
 293         return FRAME_TYPE_CUSTOM;
 294     }
 295     // anyway it should not reach here
 296     return FRAME_TYPE_CUSTOM;
 297 }