flight/libraries/alarms.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotSystem OpenPilot System
   4  * @{
   5  * @addtogroup OpenPilotLibraries OpenPilot System Libraries
   6  * @brief OpenPilot System libraries are available to all OP modules.
   7  * @{
   8  * @file       alarms.c
   9  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  10  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      Library for setting and clearing system alarms
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include <openpilot.h>
  32 #include "inc/alarms.h"
  33
  34 // Private constants
  35 #ifndef PIOS_ALARM_GRACETIME
  36 // alarm cannot be turned off for at least 1000 milliseconds
  37 // to prevent event system overload through flapping alarms
  38         #define PIOS_ALARM_GRACETIME 1000
  39 #endif // PIOS_ALARM_GRACETIME
  40
  41 // Private types
  42
  43 // Private variables
  44 static xSemaphoreHandle lock;
  45 static volatile uint16_t lastAlarmChange[SYSTEMALARMS_ALARM_NUMELEM] = { 0 }; // this deliberately overflows every 2^16 milliseconds to save memory
  46
  47 // Private functions
  48 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
  49
  50 /**
  51  * Initialize the alarms library
  52  */
  53 int32_t AlarmsInitialize(void)
  54 {
  55     SystemAlarmsInitialize();
  56
  57     lock = xSemaphoreCreateRecursiveMutex();
  58     // do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
  59     // AlarmsClearAll();
  60     // AlarmsDefaultAll();
  61     return 0;
  62 }
  63
  64 /**
  65  * Set an alarm
  66  * @param alarm The system alarm to be modified
  67  * @param severity The alarm severity
  68  * @return 0 if success, -1 if an error
  69  */
  70 int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
  71 {
  72     SystemAlarmsAlarmData alarms;
  73
  74     // Check that this is a valid alarm
  75     if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
  76         return -1;
  77     }
  78
  79     // Lock
  80     xSemaphoreTakeRecursive(lock, portMAX_DELAY);
  81
  82     // Read alarm and update its severity only if it was changed
  83     SystemAlarmsAlarmGet(&alarms);
  84     uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
  85     if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
  86          SystemAlarmsAlarmToArray(alarms)[alarm] != severity)
  87         || SystemAlarmsAlarmToArray(alarms)[alarm] < severity) {
  88         SystemAlarmsAlarmToArray(alarms)[alarm] = severity;
  89         lastAlarmChange[alarm] = flightTime;
  90         SystemAlarmsAlarmSet(&alarms);
  91     }
  92
  93     // Release lock
  94     xSemaphoreGiveRecursive(lock);
  95     return 0;
  96 }
  97
  98 /**
  99  * Set an Extended Alarm
 100  * @param alarm The system alarm to be modified
 101  * @param severity The alarm severity
 102  * @param status The Extended alarm status field
 103  * @param subStatus The Extended alarm substatus field
 104  * @return 0 if success, -1 if an error
 105  */
 106 int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
 107                           SystemAlarmsAlarmOptions severity,
 108                           SystemAlarmsExtendedAlarmStatusOptions status,
 109                           uint8_t subStatus)
 110 {
 111     SystemAlarmsData alarms;
 112
 113     // Check that this is a valid alarm
 114     if (alarm >= SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM) {
 115         return -1;
 116     }
 117
 118     // Lock
 119     xSemaphoreTakeRecursive(lock, portMAX_DELAY);
 120
 121     // Read alarm and update its severity only if it was changed
 122     SystemAlarmsGet(&alarms);
 123     uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
 124     if (((uint16_t)(flightTime - lastAlarmChange[alarm]) > PIOS_ALARM_GRACETIME &&
 125          SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] != severity)
 126         || SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] < severity) {
 127         SystemAlarmsExtendedAlarmStatusToArray(alarms.ExtendedAlarmStatus)[alarm] = status;
 128         SystemAlarmsExtendedAlarmSubStatusToArray(alarms.ExtendedAlarmSubStatus)[alarm] = subStatus;
 129         SystemAlarmsAlarmToArray(alarms.Alarm)[alarm] = severity;
 130         lastAlarmChange[alarm] = flightTime;
 131         SystemAlarmsSet(&alarms);
 132     }
 133
 134     // Release lock
 135     xSemaphoreGiveRecursive(lock);
 136     return 0;
 137 }
 138
 139 /**
 140  * Get an alarm
 141  * @param alarm The system alarm to be read
 142  * @return Alarm severity
 143  */
 144 SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
 145 {
 146     SystemAlarmsAlarmData alarms;
 147
 148     // Check that this is a valid alarm
 149     if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
 150         return 0;
 151     }
 152
 153     // Read alarm
 154     SystemAlarmsAlarmGet(&alarms);
 155     return SystemAlarmsAlarmToArray(alarms)[alarm];
 156 }
 157
 158 /**
 159  * Set an alarm to it's default value
 160  * @param alarm The system alarm to be modified
 161  * @return 0 if success, -1 if an error
 162  */
 163 int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm)
 164 {
 165     return AlarmsSet(alarm, SYSTEMALARMS_ALARM_DEFAULT);
 166 }
 167
 168 /**
 169  * Default all alarms
 170  */
 171 void AlarmsDefaultAll()
 172 {
 173     for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
 174         AlarmsDefault(n);
 175     }
 176 }
 177
 178 /**
 179  * Clear an alarm
 180  * @param alarm The system alarm to be modified
 181  * @return 0 if success, -1 if an error
 182  */
 183 int32_t AlarmsClear(SystemAlarmsAlarmElem alarm)
 184 {
 185     if (alarm < SYSTEMALARMS_EXTENDEDALARMSTATUS_NUMELEM) {
 186         return ExtendedAlarmsSet(alarm, SYSTEMALARMS_ALARM_OK, SYSTEMALARMS_EXTENDEDALARMSTATUS_NONE, 0);
 187     } else {
 188         return AlarmsSet(alarm, SYSTEMALARMS_ALARM_OK);
 189     }
 190 }
 191
 192 /**
 193  * Clear all alarms
 194  */
 195 void AlarmsClearAll()
 196 {
 197     for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
 198         AlarmsClear(n);
 199     }
 200 }
 201
 202 /**
 203  * Check if there are any alarms with the given or higher severity
 204  * @return 0 if no alarms are found, 1 if at least one alarm is found
 205  */
 206 int32_t AlarmsHasWarnings()
 207 {
 208     return hasSeverity(SYSTEMALARMS_ALARM_WARNING);
 209 }
 210
 211 /**
 212  * Check if there are any alarms with error or higher severity
 213  * @return 0 if no alarms are found, 1 if at least one alarm is found
 214  */
 215 int32_t AlarmsHasErrors()
 216 {
 217     return hasSeverity(SYSTEMALARMS_ALARM_ERROR);
 218 }
 219
 220
 221 /**
 222  * Check if there are any alarms with critical or higher severity
 223  * @return 0 if no alarms are found, 1 if at least one alarm is found
 224  */
 225 int32_t AlarmsHasCritical()
 226 {
 227     return hasSeverity(SYSTEMALARMS_ALARM_CRITICAL);
 228 }
 229
 230
 231 /**
 232  * Check if there are any alarms with the given or higher severity
 233  * @return 0 if no alarms are found, 1 if at least one alarm is found
 234  */
 235 static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
 236 {
 237     SystemAlarmsAlarmData alarms;
 238
 239     // Lock
 240     xSemaphoreTakeRecursive(lock, portMAX_DELAY);
 241
 242     // Read alarms
 243     SystemAlarmsAlarmGet(&alarms);
 244
 245     // Go through alarms and check if any are of the given severity or higher
 246     for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
 247         if (SystemAlarmsAlarmToArray(alarms)[n] >= severity) {
 248             xSemaphoreGiveRecursive(lock);
 249             return 1;
 250         }
 251     }
 252
 253     // If this point is reached then no alarms found
 254     xSemaphoreGiveRecursive(lock);
 255     return 0;
 256 }
 257 /**
 258  * Get the highest alarm severity
 259  * @return
 260  */
 261 SystemAlarmsAlarmOptions AlarmsGetHighestSeverity()
 262 {
 263     SystemAlarmsAlarmData alarmsData;
 264     SystemAlarmsAlarmOptions highest = SYSTEMALARMS_ALARM_UNINITIALISED;
 265
 266     // Lock
 267     xSemaphoreTakeRecursive(lock, portMAX_DELAY);
 268
 269     // Read alarms
 270     SystemAlarmsAlarmGet(&alarmsData);
 271
 272     // Go through alarms and find the highest severity
 273     uint32_t n = 0;
 274     while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) {
 275         if (SystemAlarmsAlarmToArray(alarmsData)[n] > highest) {
 276             highest = SystemAlarmsAlarmToArray(alarmsData)[n];
 277         }
 278         n++;
 279     }
 280
 281     xSemaphoreGiveRecursive(lock);
 282     return highest;
 283 }
 284
 285
 286 static const char *const systemalarms_severity_names[] = {
 287     [SYSTEMALARMS_ALARM_UNINITIALISED] = "UNINITIALISED",
 288     [SYSTEMALARMS_ALARM_OK]       = "OK",
 289     [SYSTEMALARMS_ALARM_WARNING]  = "WARNING",
 290     [SYSTEMALARMS_ALARM_CRITICAL] = "CRITICAL",
 291     [SYSTEMALARMS_ALARM_ERROR]    = "ERROR"
 292 };
 293
 294 static const char *const systemalarms_alarm_names[] = {
 295     [SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION] = "CONFIG",
 296     [SYSTEMALARMS_ALARM_BOOTFAULT]     = "BOOT",
 297     [SYSTEMALARMS_ALARM_OUTOFMEMORY]   = "MEM",
 298     [SYSTEMALARMS_ALARM_STACKOVERFLOW] = "STACK",
 299     [SYSTEMALARMS_ALARM_CPUOVERLOAD]   = "CPU",
 300     [SYSTEMALARMS_ALARM_EVENTSYSTEM]   = "EVENT",
 301     [SYSTEMALARMS_ALARM_TELEMETRY]     = "TELEMETRY",
 302     [SYSTEMALARMS_ALARM_RECEIVER] = "INPUT",
 303     [SYSTEMALARMS_ALARM_MANUALCONTROL] = "MANUAL",
 304     [SYSTEMALARMS_ALARM_ACTUATOR]      = "ACTUATOR",
 305     [SYSTEMALARMS_ALARM_ATTITUDE]      = "ATTI",
 306     [SYSTEMALARMS_ALARM_SENSORS]       = "SENSOR",
 307     [SYSTEMALARMS_ALARM_MAGNETOMETER]  = "MAG",
 308     [SYSTEMALARMS_ALARM_AIRSPEED]      = "AIRSPD",
 309     [SYSTEMALARMS_ALARM_STABILIZATION] = "STAB",
 310     [SYSTEMALARMS_ALARM_NAV] = "NAV",
 311     [SYSTEMALARMS_ALARM_GUIDANCE]      = "GUIDANCE",
 312     [SYSTEMALARMS_ALARM_PATHPLAN]      = "PLAN",
 313     [SYSTEMALARMS_ALARM_BATTERY]       = "BATT",
 314     [SYSTEMALARMS_ALARM_FLIGHTTIME]    = "TIME",
 315     [SYSTEMALARMS_ALARM_I2C] = "I2C",
 316     [SYSTEMALARMS_ALARM_GPS] = "GPS",
 317 };
 318
 319 static const char *const systemalarms_extendedalarmstatus_names[] = {
 320     [SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED] = "CFG:REBOOT",
 321     [SYSTEMALARMS_EXTENDEDALARMSTATUS_FLIGHTMODE]     = "CFG:FLIGHTMODE",
 322     [SYSTEMALARMS_EXTENDEDALARMSTATUS_UNSUPPORTEDCONFIG_ONESHOT] = "CFG:ONESHOT",
 323     [SYSTEMALARMS_EXTENDEDALARMSTATUS_BADTHROTTLEORCOLLECTIVEINPUTRANGE] = "CFG:THR-COL",
 324 };
 325
 326 size_t AlarmString(SystemAlarmsData *alarm, char *buffer, size_t buffer_size, SystemAlarmsAlarmOptions level, SystemAlarmsAlarmOptions *highestSeverity)
 327 {
 328     size_t pos = 0;
 329
 330     PIOS_STATIC_ASSERT(NELEMENTS(systemalarms_alarm_names) == SYSTEMALARMS_ALARM_NUMELEM);
 331
 332     for (unsigned severity = SYSTEMALARMS_ALARM_ERROR; severity >= level; --severity) {
 333         // should we prepend severity level here? No, not for now.
 334
 335         for (unsigned i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; ++i) {
 336             if ((SystemAlarmsAlarmToArray(alarm->Alarm)[i] == severity)
 337                 && (systemalarms_alarm_names[i])) {
 338                 if (highestSeverity) { // they are already sorted by severity as we are processing in specific order
 339                     *highestSeverity = severity;
 340                     highestSeverity  = 0;
 341                 }
 342
 343                 // in which case should we dig into extended alarm status?
 344                 // looks like SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION sets most of the extended alarms
 345                 // except SYSTEMALARMS_ALARM_BOOTFAULT which also sets SYSTEMALARMS_EXTENDEDALARMSTATUS_REBOOTREQUIRED
 346
 347                 const char *current_msg = systemalarms_alarm_names[i];
 348
 349                 switch (i) {
 350                 case SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION:
 351                     if (alarm->ExtendedAlarmStatus.SystemConfiguration < NELEMENTS(systemalarms_extendedalarmstatus_names)) {
 352                         current_msg = systemalarms_extendedalarmstatus_names[alarm->ExtendedAlarmStatus.SystemConfiguration];
 353                     }
 354                     break;
 355
 356                 case SYSTEMALARMS_ALARM_BOOTFAULT:
 357                     if (alarm->ExtendedAlarmStatus.BootFault < NELEMENTS(systemalarms_extendedalarmstatus_names)) {
 358                         current_msg = systemalarms_extendedalarmstatus_names[alarm->ExtendedAlarmStatus.BootFault];
 359                     }
 360                     break;
 361                 }
 362
 363                 int current_len = strlen(current_msg);
 364
 365                 if ((pos + current_len + 1) > buffer_size) {
 366                     break;
 367                 }
 368
 369                 memcpy(buffer + pos, current_msg, current_len);
 370
 371                 pos += current_len;
 372
 373                 buffer[pos++] = ',';
 374             }
 375         }
 376     }
 377
 378     if (pos > 0) {
 379         --pos; // get rid of that trailing separator.
 380     }
 381
 382     buffer[pos] = 0;
 383
 384     return pos; // return length of the string in buffer. Actual bytes written is +1
 385 }
 386
 387 /**
 388  * @}
 389  * @}
 390  */