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 #include <stdbool.h>
  32
  33
  34 #define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS)
  35 // Private data types definition
  36
  37 #ifdef PIOS_LED_ALARM
  38 #define ALARM_LED_ON()  PIOS_LED_On(PIOS_LED_ALARM)
  39 #define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM)
  40 #else
  41 #define ALARM_LED_ON()
  42 #define ALARM_LED_OFF()
  43 #endif
  44
  45 #ifdef PIOS_LED_HEARTBEAT
  46 #define HEARTBEAT_LED_ON()  PIOS_LED_On(PIOS_LED_HEARTBEAT)
  47 #define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT)
  48 #else
  49 #define HEARTBEAT_LED_ON()
  50 #define HEARTBEAT_LED_OFF()
  51 #endif
  52
  53 #define BLINK_R_ALARM_PATTERN(x) \
  54     (x == SYSTEMALARMS_ALARM_OK ? 0 : \
  55      x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \
  56      x == SYSTEMALARMS_ALARM_ERROR ? 0b0000001000100000 : \
  57      x == SYSTEMALARMS_ALARM_CRITICAL ? 0b0111111111111110 : 0)
  58 #define BLINK_B_ALARM_PATTERN(x) \
  59     (x == SYSTEMALARMS_ALARM_OK ? 0 : \
  60      x == SYSTEMALARMS_ALARM_WARNING ? 0 : \
  61      x == SYSTEMALARMS_ALARM_ERROR ? 0 : \
  62      x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0)
  63
  64
  65 #define BLINK_B_FM_ARMED_PATTERN(x) \
  66     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \
  67      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \
  68      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \
  69      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
  70      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000100001 : \
  71      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000010000100001 : \
  72      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \
  73      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \
  74      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \
  75      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \
  76      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001)
  77
  78 #define BLINK_R_FM_ARMED_PATTERN(x) \
  79     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
  80      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
  81      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
  82      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \
  83      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000001 : \
  84      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000001 : \
  85      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \
  86      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \
  87      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \
  88      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \
  89      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000000000 : 0b0000010000000000)
  90
  91 #define BLINK_B_FM_DISARMED_PATTERN(x) \
  92     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000011 : \
  93      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000001100011 : \
  94      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000110001100011 : \
  95      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
  96      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000001100011 : \
  97      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000110001100011 : \
  98      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110001100011 : \
  99      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100110011 : \
 100      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100110011 : \
 101      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110001100011 : \
 102      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110001100011 : 0b0000000000000011)
 103
 104 #define BLINK_R_FM_DISARMED_PATTERN(x) \
 105     (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
 106      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
 107      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
 108      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \
 109      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000011 : \
 110      x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000011 : \
 111      x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110000000000 : \
 112      x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100000000 : \
 113      x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100000000 : \
 114      x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110000000000 : \
 115      x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110000000000 : 0b0000110000000000)
 116
 117 #define BLINK_B_HEARTBEAT_PATTERN 0b0001111111111111
 118 #define BLINK_R_HEARTBEAT_PATTERN 0
 119
 120 #define BLINK_B_NOTIFY_PATTERN(x) \
 121     (x == NOTIFY_NONE ? 0 : \
 122      x == NOTIFY_OK ? 0b0000100100111111 : \
 123      x == NOTIFY_NOK ? 0b0000000000111111 : \
 124      x == NOTIFY_DRAW_ATTENTION ? 0b0101010101010101 : 0b0101010101010101)
 125
 126 #define BLINK_R_NOTIFY_PATTERN(x) \
 127     (x == NOTIFY_NONE ? 0 : \
 128      x == NOTIFY_OK ? 0b0000000000001111 : \
 129      x == NOTIFY_NOK ? 0b0011000011001111 : \
 130      x == NOTIFY_DRAW_ATTENTION ? 0b1010101010101010 : 0b1010101010101010)
 131
 132 // led notification handling
 133 static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK;
 134 static volatile FlightStatusData currentFlightStatus;
 135 static volatile bool started = false;
 136 static volatile pios_notify_notification nextNotification = NOTIFY_NONE;
 137
 138 #ifdef PIOS_LED_ALARM
 139 static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
 140 #endif // PIOS_LED_ALARM
 141 static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
 142 static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern);
 143 static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern);
 144
 145
 146 void NotificationUpdateStatus()
 147 {
 148     started = true;
 149     // get values to be used for led handling
 150     FlightStatusGet((FlightStatusData *)&currentFlightStatus);
 151     currentAlarmLevel = AlarmsGetHighestSeverity();
 152     if (nextNotification == NOTIFY_NONE) {
 153         nextNotification = PIOS_NOTIFY_GetActiveNotification(true);
 154     }
 155 }
 156
 157 void NotificationOnboardLedsRun()
 158 {
 159     static uint32_t lastRunTimestamp;
 160     static uint16_t r_pattern;
 161     static uint16_t b_pattern;
 162     static uint8_t cycleCount; // count the number of cycles
 163     static uint8_t lastFlightMode   = -1;
 164     static bool forceShowFlightMode = false;
 165     static pios_notify_notification runningNotification = NOTIFY_NONE;
 166     static enum {
 167         STATUS_NOTIFY,
 168         STATUS_ALARM,
 169         STATUS_FLIGHTMODE, // flightMode/HeartBeat
 170         STATUS_LENGHT
 171     } status;
 172
 173     const uint32_t current_timestamp = GET_CURRENT_MILLIS;
 174
 175     if (!started || (current_timestamp - lastRunTimestamp) < LED_BLINK_PERIOD_MS) {
 176         return;
 177     }
 178
 179     lastRunTimestamp = current_timestamp;
 180     // the led will show various status information, subdivided in three phases
 181     // - Notification
 182     // - Alarm
 183     // - Flight status
 184     // they are shown using the above priority
 185     // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end
 186
 187     cycleCount++;
 188     // Notifications are "modal" to other statuses so they takes precedence
 189     if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) {
 190         // read next notification to show
 191         runningNotification = nextNotification;
 192         nextNotification    = NOTIFY_NONE;
 193         // Force a notification status
 194         status     = STATUS_NOTIFY;
 195         cycleCount = 0; // instantly start a notify cycle
 196     } else {
 197         if (lastFlightMode != currentFlightStatus.FlightMode) {
 198             status         = STATUS_FLIGHTMODE;
 199             lastFlightMode = currentFlightStatus.FlightMode;
 200             cycleCount     = 0; // instantly start a flightMode cycle
 201             forceShowFlightMode = true;
 202         }
 203     }
 204
 205     // check if a phase has just finished
 206     if (cycleCount & 0x10) {
 207         HEARTBEAT_LED_OFF();
 208         ALARM_LED_OFF();
 209         cycleCount = 0x0;
 210         forceShowFlightMode = false;
 211         // Notification has been just shown, cleanup
 212         if (status == STATUS_NOTIFY) {
 213             runningNotification = NOTIFY_NONE;
 214         }
 215         status = (status + 1) % STATUS_LENGHT;
 216     }
 217
 218     if (status == STATUS_NOTIFY) {
 219         if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) {
 220             // no notifications, advance
 221             status++;
 222         }
 223     }
 224
 225     // Handles Alarm display
 226     if (status == STATUS_ALARM) {
 227 #ifdef PIOS_LED_ALARM
 228         if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) {
 229             // no alarms, advance
 230             status++;
 231         }
 232 #else
 233         // no alarms leds, advance
 234         status++;
 235 #endif // PIOS_LED_ALARM
 236     }
 237
 238     // **** Handles flightmode display
 239     if (status == STATUS_FLIGHTMODE && !cycleCount) {
 240         if (forceShowFlightMode || currentFlightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) {
 241             handleFlightMode(&r_pattern, &b_pattern);
 242         } else {
 243             handleHeartbeat(&r_pattern, &b_pattern);
 244         }
 245     }
 246
 247     // led output
 248     if (b_pattern & 0x1) {
 249         HEARTBEAT_LED_ON();
 250     } else {
 251         HEARTBEAT_LED_OFF();
 252     }
 253     if (r_pattern & 0x1) {
 254         ALARM_LED_ON();
 255     } else {
 256         ALARM_LED_OFF();
 257     }
 258     r_pattern >>= 1;
 259     b_pattern >>= 1;
 260 }
 261
 262 #if defined(PIOS_LED_ALARM)
 263 static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern)
 264 {
 265     if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) {
 266         return false;
 267     }
 268     *b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel);
 269     *r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel);
 270     return true;
 271 }
 272 #endif /* PIOS_LED_ALARM */
 273
 274
 275 static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern)
 276 {
 277     if (runningNotification == NOTIFY_NONE) {
 278         return false;
 279     }
 280     *b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification);
 281     *r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification);
 282     return true;
 283 }
 284
 285 static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern)
 286 {
 287     // Flash the heartbeat LED
 288     uint8_t flightmode = currentFlightStatus.FlightMode;
 289
 290     if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
 291         *b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode);
 292         *r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode);
 293     } else {
 294         *b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode);
 295         *r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode);
 296     }
 297 }
 298
 299 static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern)
 300 {
 301     // Flash the heartbeat LED
 302     *b_pattern = BLINK_B_HEARTBEAT_PATTERN;
 303     *r_pattern = BLINK_R_HEARTBEAT_PATTERN;
 304 }