flight/libraries/notification.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       notification.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
   6  * @brief      notification library.
   7  *             --
   8  * @see        The GNU Public License (GPL) Version 3
   9  *
  10  *****************************************************************************/
  11 /*
  12  * This program is free software; you can redistribute it and/or modify
  13  * it under the terms of the GNU General Public License as published by
  14  * the Free Software Foundation; either version 3 of the License, or
  15  * (at your option) any later version.
  16  *
  17  * This program is distributed in the hope that it will be useful, but
  18  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  19  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  20  * for more details.
  21  *
  22  * You should have received a copy of the GNU General Public License along
  23  * with this program; if not, write to the Free Software Foundation, Inc.,
  24  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  25  */
  26 #include "inc/notification.h"
  27 #include <openpilot.h>
  28 #include <systemalarms.h>
  29 #include <flightstatus.h>
  30 #include <pios_notify.h>
  31
  32 #ifdef PIOS_LED_ALARM
  33 #define ALARM_LED_ON()  PIOS_LED_On(PIOS_LED_ALARM)
  34 #define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM)
  35 #else
  36 #define ALARM_LED_ON()
  37 #define ALARM_LED_OFF()
  38 #endif
  39
  40 #ifdef PIOS_LED_HEARTBEAT
  41 #define HEARTBEAT_LED_ON()  PIOS_LED_On(PIOS_LED_HEARTBEAT)
  42 #define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT)
  43 #else
  44 #define HEARTBEAT_LED_ON()
  45 #define HEARTBEAT_LED_OFF()
  46 #endif
  47
  48 #define BLINK_R_ALARM_PATTERN(x) \
  49     (x == SYSTEMALARMS_ALARM_OK ? 0 : \
  50      x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \
  51      x == SYSTEMALARMS_ALARM_ERROR ? 0b0000001000100000 : \
  52      x == SYSTEMALARMS_ALARM_CRITICAL ? 0b0111111111111110 : 0)
  53 #define BLINK_B_ALARM_PATTERN(x) \
  54     (x == SYSTEMALARMS_ALARM_OK ? 0 : \
  55      x == SYSTEMALARMS_ALARM_WARNING ? 0 : \
  56      x == SYSTEMALARMS_ALARM_ERROR ? 0 : \
  57      x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0)
  58
  59
  60 #define BLINK_B_FM_ARMED_PATTERN(x) \
  61     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \
  62      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \
  63      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \
  64      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
  65      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000100001 : \
  66      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000010000100001 : \
  67      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \
  68      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \
  69      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \
  70      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \
  71      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001)
  72
  73 #define BLINK_R_FM_ARMED_PATTERN(x) \
  74     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
  75      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
  76      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
  77      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
  78      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000001 : \
  79      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000001 : \
  80      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \
  81      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \
  82      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \
  83      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \
  84      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000000000 : 0b0000010000000000)
  85
  86 #define BLINK_B_FM_DISARMED_PATTERN(x) \
  87     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000011 : \
  88      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000001100011 : \
  89      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000110001100011 : \
  90      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
  91      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000001100011 : \
  92      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000110001100011 : \
  93      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110001100011 : \
  94      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100110011 : \
  95      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100110011 : \
  96      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110001100011 : \
  97      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110001100011 : 0b0000000000000011)
  98
  99 #define BLINK_R_FM_DISARMED_PATTERN(x) \
 100     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
 101      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
 102      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
 103      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
 104      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000011 : \
 105      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000011 : \
 106      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110000000000 : \
 107      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100000000 : \
 108      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100000000 : \
 109      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110000000000 : \
 110      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110000000000 : 0b0000110000000000)
 111
 112 #define BLINK_B_HEARTBEAT_PATTERN 0b0001111111111111
 113 #define BLINK_R_HEARTBEAT_PATTERN 0
 114
 115 #define BLINK_B_NOTIFY_PATTERN(x) \
 116     (x == NOTIFY_NONE ? 0 : \
 117      x == NOTIFY_OK ? 0b0000100100111111 : \
 118      x == NOTIFY_NOK ? 0b0000000000111111 : \
 119      x == NOTIFY_DRAW_ATTENTION ? 0b0101010101010101 : 0b0101010101010101)
 120
 121 #define BLINK_R_NOTIFY_PATTERN(x) \
 122     (x == NOTIFY_NONE ? 0 : \
 123      x == NOTIFY_OK ? 0b0000000000001111 : \
 124      x == NOTIFY_NOK ? 0b0011000011001111 : \
 125      x == NOTIFY_DRAW_ATTENTION ? 0b1010101010101010 : 0b1010101010101010)
 126
 127 // led notification handling
 128 static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK;
 129 static volatile FlightStatusData currentFlightStatus;
 130 static volatile bool started = false;
 131 static volatile pios_notify_notification nextNotification = NOTIFY_NONE;
 132
 133 #ifdef PIOS_LED_ALARM
 134 static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
 135 #endif // PIOS_LED_ALARM
 136 static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
 137 static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern);
 138 static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern);
 139 void NotificationUpdateStatus()
 140 {
 141     started = true;
 142     // get values to be used for led handling
 143     FlightStatusGet((FlightStatusData *)&currentFlightStatus);
 144     currentAlarmLevel = AlarmsGetHighestSeverity();
 145     if (nextNotification == NOTIFY_NONE) {
 146         nextNotification = PIOS_NOTIFY_GetActiveNotification(true);
 147     }
 148 }
 149
 150 void NotificationOnboardLedsRun()
 151 {
 152     static portTickType lastRunTimestamp;
 153     static uint16_t r_pattern;
 154     static uint16_t b_pattern;
 155     static uint8_t cycleCount; // count the number of cycles
 156     static uint8_t lastFlightMode   = -1;
 157     static bool forceShowFlightMode = false;
 158     static pios_notify_notification runningNotification = NOTIFY_NONE;
 159     static enum {
 160         STATUS_NOTIFY,
 161         STATUS_ALARM,
 162         STATUS_FLIGHTMODE, // flightMode/HeartBeat
 163         STATUS_LENGHT
 164     } status;
 165
 166     if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
 167         return;
 168     }
 169
 170     lastRunTimestamp = xTaskGetTickCount();
 171     // the led will show various status information, subdivided in three phases
 172     // - Notification
 173     // - Alarm
 174     // - Flight status
 175     // they are shown using the above priority
 176     // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end
 177
 178     cycleCount++;
 179     // Notifications are "modal" to other statuses so they takes precedence
 180     if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) {
 181         // read next notification to show
 182         runningNotification = nextNotification;
 183         nextNotification    = NOTIFY_NONE;
 184         // Force a notification status
 185         status     = STATUS_NOTIFY;
 186         cycleCount = 0; // instantly start a notify cycle
 187     } else {
 188         if (lastFlightMode != currentFlightStatus.FlightMode) {
 189             status         = STATUS_FLIGHTMODE;
 190             lastFlightMode = currentFlightStatus.FlightMode;
 191             cycleCount     = 0; // instantly start a flightMode cycle
 192             forceShowFlightMode = true;
 193         }
 194     }
 195
 196     // check if a phase has just finished
 197     if (cycleCount & 0x10) {
 198         HEARTBEAT_LED_OFF();
 199         ALARM_LED_OFF();
 200         cycleCount = 0x0;
 201         forceShowFlightMode = false;
 202         // Notification has been just shown, cleanup
 203         if (status == STATUS_NOTIFY) {
 204             runningNotification = NOTIFY_NONE;
 205         }
 206         status = (status + 1) % STATUS_LENGHT;
 207     }
 208
 209     if (status == STATUS_NOTIFY) {
 210         if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) {
 211             // no notifications, advance
 212             status++;
 213         }
 214     }
 215
 216     // Handles Alarm display
 217     if (status == STATUS_ALARM) {
 218 #ifdef PIOS_LED_ALARM
 219         if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) {
 220             // no alarms, advance
 221             status++;
 222         }
 223 #else
 224         // no alarms leds, advance
 225         status++;
 226 #endif // PIOS_LED_ALARM
 227     }
 228
 229     // **** Handles flightmode display
 230     if (status == STATUS_FLIGHTMODE && !cycleCount) {
 231         if (forceShowFlightMode || currentFlightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
 232             handleFlightMode(&r_pattern, &b_pattern);
 233         } else {
 234             handleHeartbeat(&r_pattern, &b_pattern);
 235         }
 236     }
 237
 238     // led output
 239     if (b_pattern & 0x1) {
 240         HEARTBEAT_LED_ON();
 241     } else {
 242         HEARTBEAT_LED_OFF();
 243     }
 244     if (r_pattern & 0x1) {
 245         ALARM_LED_ON();
 246     } else {
 247         ALARM_LED_OFF();
 248     }
 249     r_pattern >>= 1;
 250     b_pattern >>= 1;
 251 }
 252
 253 #if defined(PIOS_LED_ALARM)
 254 static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern)
 255 {
 256     if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) {
 257         return false;
 258     }
 259     *b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel);
 260     *r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel);
 261     return true;
 262 }
 263 #endif /* PIOS_LED_ALARM */
 264
 265
 266 static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern)
 267 {
 268     if (runningNotification == NOTIFY_NONE) {
 269         return false;
 270     }
 271     *b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification);
 272     *r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification);
 273     return true;
 274 }
 275
 276 static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern)
 277 {
 278     // Flash the heartbeat LED
 279     uint8_t flightmode = currentFlightStatus.FlightMode;
 280
 281     if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
 282         *b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode);
 283         *r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode);
 284     } else {
 285         *b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode);
 286         *r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode);
 287     }
 288 }
 289
 290 static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern)
 291 {
 292     // Flash the heartbeat LED
 293     *b_pattern = BLINK_B_HEARTBEAT_PATTERN;
 294     *r_pattern = BLINK_R_HEARTBEAT_PATTERN;
 295 }