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