flight/modules/Notify/notify.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       notify.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      Notify module, show events and status on external led.
   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
  27 #include "openpilot.h"
  28 #include <flightstatus.h>
  29 #include <systemalarms.h>
  30 #include <flightbatterystate.h>
  31 #include <lednotification.h>
  32 #include <optypes.h>
  33 #include <pios_notify.h>
  34 #include <FreeRTOS.h>
  35 #include <task.h>
  36 #include <eventdispatcher.h>
  37 #include "inc/notify.h"
  38 #include "inc/sequences.h"
  39 #include <pios_mem.h>
  40 #include <hwsettings.h>
  41
  42 #define SAMPLE_PERIOD_MS 250
  43 // private types
  44 typedef struct {
  45     uint32_t lastAlarmTime;
  46     uint8_t  lastAlarm;
  47 } AlarmStatus_t;
  48 // function declarations
  49 static void updatedCb(UAVObjEvent *ev);
  50 static void onTimerCb(UAVObjEvent *ev);
  51 static void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time,
  52                        uint8_t warn_sequence, uint8_t critical_sequence, uint8_t error_sequence,
  53                        uint32_t timeBetweenNotifications);
  54 static AlarmStatus_t *alarmStatus;
  55 int32_t NotifyInitialize(void)
  56 {
  57     uint8_t ws281xOutStatus;
  58
  59     HwSettingsWS2811LED_OutGet(&ws281xOutStatus);
  60     // Todo: Until further applications exists for WS2811 notify enabled status is tied to ws281x output configuration
  61     bool enabled = ws281xOutStatus != HWSETTINGS_WS2811LED_OUT_DISABLED;
  62
  63     if (enabled) {
  64         alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize);
  65         for (uint8_t i = 0; i < alarmsMapSize; i++) {
  66             alarmStatus[i].lastAlarm     = SYSTEMALARMS_ALARM_OK;
  67             alarmStatus[i].lastAlarmTime = 0;
  68         }
  69
  70         FlightStatusConnectCallback(&updatedCb);
  71         static UAVObjEvent ev;
  72         memset(&ev, 0, sizeof(UAVObjEvent));
  73         EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
  74
  75         updatedCb(0);
  76     }
  77     return 0;
  78 }
  79 MODULE_INITCALL(NotifyInitialize, 0);
  80
  81
  82 void updatedCb(UAVObjEvent *ev)
  83 {
  84     if (!ev || ev->obj == FlightStatusHandle()) {
  85         static uint8_t last_armed = 0xff;
  86         static uint8_t last_flightmode = 0xff;
  87         uint8_t armed;
  88         uint8_t flightmode;
  89         FlightStatusArmedGet(&armed);
  90         FlightStatusFlightModeGet(&flightmode);
  91         if (last_armed != armed || (armed && flightmode != last_flightmode)) {
  92             if (armed) {
  93                 PIOS_NOTIFICATION_Default_Ext_Led_Play(flightModeMap[flightmode], NOTIFY_PRIORITY_BACKGROUND);
  94             } else {
  95                 PIOS_NOTIFICATION_Default_Ext_Led_Play(&notifications[NOTIFY_SEQUENCE_DISARMED], NOTIFY_PRIORITY_BACKGROUND);
  96             }
  97         }
  98         last_armed = armed;
  99         last_flightmode = flightmode;
 100     }
 101 }
 102
 103 void onTimerCb(__attribute__((unused)) UAVObjEvent *ev)
 104 {
 105     static SystemAlarmsAlarmData alarms;
 106
 107     SystemAlarmsAlarmGet(&alarms);
 108     for (uint8_t i = 0; i < alarmsMapSize; i++) {
 109         uint8_t alarm = SystemAlarmsAlarmToArray(alarms)[alarmsMap[i].alarmIndex];
 110         checkAlarm(alarm,
 111                    &alarmStatus[i].lastAlarm,
 112                    &alarmStatus[i].lastAlarmTime,
 113                    alarmsMap[i].warnNotification,
 114                    alarmsMap[i].criticalNotification,
 115                    alarmsMap[i].errorNotification,
 116                    alarmsMap[i].timeBetweenNotifications);
 117     }
 118 }
 119
 120 void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time, uint8_t warn_sequence, uint8_t critical_sequence, uint8_t error_sequence, uint32_t timeBetweenNotifications)
 121 {
 122     if (alarm > SYSTEMALARMS_ALARM_OK) {
 123         uint32_t current_time = PIOS_DELAY_GetuS();
 124         if (*last_alarm < alarm || (*last_alm_time + timeBetweenNotifications * 1000) < current_time) {
 125             uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence :
 126                                ((alarm == SYSTEMALARMS_ALARM_CRITICAL) ? critical_sequence : error_sequence);
 127             bool updated     = true;
 128             if (sequence != NOTIFY_SEQUENCE_NULL) {
 129                 updated = PIOS_NOTIFICATION_Default_Ext_Led_Play(
 130                     &notifications[sequence],
 131                     alarm == SYSTEMALARMS_ALARM_WARNING ? NOTIFY_PRIORITY_REGULAR : NOTIFY_PRIORITY_CRITICAL);
 132             }
 133             if (updated) {
 134                 *last_alarm    = alarm;
 135                 *last_alm_time = current_time;
 136             }
 137         }
 138         // workaround timer overflow
 139         if (*last_alm_time > current_time) {
 140             *last_alm_time = 0;
 141         }
 142     } else {
 143         *last_alarm = SYSTEMALARMS_ALARM_OK;
 144     }
 145 }