rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

OP-1379 - Various fixes and additions

Browse Source 
- Support blocks of led for notification (that is a certain number of leds between a min and a max index);
- Fix an issue with bogus colours on fast updates at LED N°0.
- Add NOTIFY_SEQUENCE_NULL to skip handling a specific alarm status;
- Adjust sequences and add Config and Receiver Alarms;
- fix issue with Alarm trigger/repetition;
- Enables Notify module only if a WS281x output is enabled.
- Add some documentation for sequence/alarm definitions (sequences.h)
- Make sequences more coherent. All GPS related info are now shown in green.
- Revert to original blinking rate for onboard led
- add Notify on DiscoveryF4Bare target
This commit is contained in:
Alessio Morale 2014-08-18 16:35:02 +02:00
parent e549c71da6
commit 233dec6d8d
8 changed files with 219 additions and 378 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/libraries/inc/notification.h
View File

@ -27,7 +27,7 @@
#define NOTIFICATION_H #define NOTIFICATION_H
// period of each blink phase // period of each blink phase
#define LED_BLINK_PERIOD_MS 200 #define LED_BLINK_PERIOD_MS 50
// update the status snapshot used by notifcations // update the status snapshot used by notifcations
void NotificationUpdateStatus(); void NotificationUpdateStatus();

31
flight/libraries/lednotification.c
View File

@ -27,8 +27,8 @@
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <freeRTOS.h> #include <FreeRTOS.h>
#include <task.h> #include <pios.h>
#include <pios_notify.h> #include <pios_notify.h>
#include <pios_ws2811.h> #include <pios_ws2811.h>
@ -57,7 +57,8 @@ typedef struct {
uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase) uint8_t next_sequence_step; // (step number to be executed) << 1 || (0x00 = on phase, 0x01 = off phase)
uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1) uint8_t next_step_rep; // next repetition number for next step (valid if step.repeats >1)
uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1) uint8_t next_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1)
uint8_t led_set; // target led set uint8_t led_set_start; // first target led for this set
uint8_t led_set_end; // last target led for this set
} NotifierLedStatus_t; } NotifierLedStatus_t;
static bool led_status_initialized = false; static bool led_status_initialized = false;
@ -69,7 +70,8 @@ static void InitExtLed()
memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED); memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED);
const uint32_t now = GET_CURRENT_MILLIS; const uint32_t now = GET_CURRENT_MILLIS;
for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) { for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) {
led_status[l].led_set = 0; led_status[l].led_set_start = 0;
led_status[l].led_set_end = PIOS_WS2811_NUMLEDS - 1;
led_status[l].next_run_time = now + 500; // start within half a second led_status[l].next_run_time = now + 500; // start within half a second
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND; led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND;
@ -105,9 +107,9 @@ static void push_queued_sequence(ExtLedNotification_t *new_notification, Notifie
} else { } else {
// a notification with priority higher than background. // a notification with priority higher than background.
// try to enqueue it // try to enqueue it
int8_t insert_point = -1; int8_t insert_point = MAX_BACKGROUND_NOTIFICATIONS - 1;
int8_t first_free = -1; int8_t first_free = -1;
for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS; i > -1; i--) { for (int8_t i = MAX_BACKGROUND_NOTIFICATIONS - 1; i >= 0; i--) {
const int8_t priority_i = status->queued_priorities[i]; const int8_t priority_i = status->queued_priorities[i];
if (priority_i == NOTIFY_PRIORITY_BACKGROUND) { if (priority_i == NOTIFY_PRIORITY_BACKGROUND) {
first_free = i; first_free = i;
@ -115,10 +117,14 @@ static void push_queued_sequence(ExtLedNotification_t *new_notification, Notifie
continue; continue;
} }
if (priority_i > new_notification->priority) { if (priority_i > new_notification->priority) {
insert_point = i; insert_point = ((i > 0) || (first_free > -1)) ? i : -1; // check whether priority is no bigger than lowest queued priority
} }
} }
// no space left on queue for this new notification, ignore.
if (insert_point < 0) {
return;
}
if (insert_point != first_free) { if (insert_point != first_free) {
// there is a free slot, move everything up one place // there is a free slot, move everything up one place
if (first_free != -1) { if (first_free != -1) {
@ -230,7 +236,7 @@ static void advance_sequence(NotifierLedStatus_t *status)
*/ */
static void run_led(NotifierLedStatus_t *status) static void run_led(NotifierLedStatus_t *status)
{ {
uint32_t currentTime = GET_CURRENT_MILLIS; const uint32_t currentTime = GET_CURRENT_MILLIS;
if (!status->running || currentTime < status->next_run_time) { if (!status->running || currentTime < status->next_run_time) {
return; return;
@ -240,11 +246,12 @@ static void run_led(NotifierLedStatus_t *status)
LedSequence_t *activeSequence = status->active_sequence_num == BACKGROUND_SEQUENCE ? LedSequence_t *activeSequence = status->active_sequence_num == BACKGROUND_SEQUENCE ?
&status->background_sequence : &status->queued_sequences[status->active_sequence_num]; &status->background_sequence : &status->queued_sequences[status->active_sequence_num];
if (status->step_phase_on) { const Color_t color = status->step_phase_on ? activeSequence->steps[step].color : Color_Off;
PIOS_WS2811_setColorRGB(activeSequence->steps[step].color, status->led_set, true);
} else { for (uint8_t i = status->led_set_start; i <= status->led_set_end; i++) {
PIOS_WS2811_setColorRGB(Color_Off, status->led_set, true); PIOS_WS2811_setColorRGB(color, i, false);
} }
PIOS_WS2811_Update();
advance_sequence(status); advance_sequence(status);
} }

433
flight/modules/Notify/inc/sequences.h
View File

@ -31,6 +31,7 @@
#include <systemalarms.h> #include <systemalarms.h>
#include <pios_helpers.h> #include <pios_helpers.h>
// This represent the list of basic light sequences, defined later
typedef enum { typedef enum {
NOTIFY_SEQUENCE_ARMED_FM_MANUAL = 0, NOTIFY_SEQUENCE_ARMED_FM_MANUAL = 0,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1 = 1, NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1 = 1,
@ -50,328 +51,118 @@ typedef enum {
NOTIFY_SEQUENCE_ALM_ERROR_BATTERY = 15, NOTIFY_SEQUENCE_ALM_ERROR_BATTERY = 15,
NOTIFY_SEQUENCE_ALM_MAG = 16, NOTIFY_SEQUENCE_ALM_MAG = 16,
NOTIFY_SEQUENCE_ALM_CONFIG = 17, NOTIFY_SEQUENCE_ALM_CONFIG = 17,
NOTIFY_SEQUENCE_DISARMED = 18, NOTIFY_SEQUENCE_ALM_RECEIVER = 18,
NOTIFY_SEQUENCE_DISARMED = 19,
NOTIFY_SEQUENCE_NULL = 255, // skips any signalling for this condition
} NotifySequences; } NotifySequences;
// This structure determine sequences attached to an alarm
typedef struct { typedef struct {
uint32_t timeBetweenNotifications; uint32_t timeBetweenNotifications; // time in milliseconds to wait between each notification iteration
uint8_t alarmIndex; uint8_t alarmIndex; // Index of the alarm, use one of the SYSTEMALARMS_ALARM_XXXXX defines
uint8_t warnNotification; uint8_t warnNotification; // index of the sequence to be used when alarm is in warning status(pick one from NotifySequences enum)
uint8_t errorNotification; uint8_t errorNotification; // index of the sequence to be used when alarm is in error status(pick one from NotifySequences enum)
} AlarmDefinition_t; } AlarmDefinition_t;
// consts
// This is the list of defined light sequences
/* how each sequence is defined
* [NOTIFY_SEQUENCE_DISARMED] = { // Sequence ID
.repeats = -1, // Number of repetitions or -1 for infinite
.steps = { // List of steps (until NOTIFY_SEQUENCE_MAX_STEPS steps, default to 5)
{
.time_off = 500, // Off time for the step
.time_on = 500, // On time for the step
.color = COLOR_TEAL, // color
.repeats = 1, // repetitions for this step
},
},
},
*
* There are two kind of sequences:
* - "Background" sequences, executed if no higher priority sequence is played;
* - "Alarm" sequences, that are "modal", they temporarily suspends background sequences and plays.
* Cannot have "-1" repetitions
* At the end background sequence are resumed;
*
*/
const LedSequence_t notifications[] = { const LedSequence_t notifications[] = {
[NOTIFY_SEQUENCE_DISARMED] = { [NOTIFY_SEQUENCE_DISARMED] = { .repeats = -1, .steps = {
.repeats = -1, { .time_off = 500, .time_on = 500, .color = COLOR_TEAL, .repeats = 1, },
.steps = { }, },
{ [NOTIFY_SEQUENCE_ARMED_FM_MANUAL] = { .repeats = -1, .steps = {
.time_off = 1000, { .time_off = 900, .time_on = 100, .color = COLOR_YELLOW, .repeats = 1, },
.time_on = 1000, }, },
.color = COLOR_BLUE, [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1] = { .repeats = -1, .steps = {
.repeats = 1, { .time_off = 900, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
}, }, },
{ [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2] = { .repeats = -1, .steps = {
.time_off = 1000, { .time_off = 100, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
.time_on = 1000, { .time_off = 700, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
.color = COLOR_LIME, }, },
.repeats = 1, [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3] = { .repeats = -1, .steps = {
}, { .time_off = 100, .time_on = 100, .color = COLOR_BLUE, .repeats = 2, },
{ { .time_off = 500, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
.time_off = 1000, }, },
.time_on = 1000, [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4] = { .repeats = -1, .steps = {
.color = COLOR_RED, { .time_off = 900, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
.repeats = 1, }, },
}, [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5] = { .repeats = -1, .steps = {
}, { .time_off = 100, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
}, { .time_off = 700, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
[NOTIFY_SEQUENCE_ARMED_FM_MANUAL] = { }, },
.repeats = -1, [NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6] = { .repeats = -1, .steps = {
.steps = { { .time_off = 100, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
{ { .time_off = 100, .time_on = 100, .color = COLOR_PURPLE, .repeats = 1, },
.time_off = 900, { .time_off = 500, .time_on = 100, .color = COLOR_BLUE, .repeats = 1, },
.time_on = 100, }, },
.color = COLOR_YELLOW, [NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE] = { .repeats = -1, .steps = {
.repeats = 1, { .time_off = 800, .time_on = 200, .color = COLOR_BLUE, .repeats = 1, },
}, }, },
}, [NOTIFY_SEQUENCE_ARMED_FM_GPS] = { .repeats = -1, .steps = {
}, { .time_off = 800, .time_on = 200, .color = COLOR_GREEN, .repeats = 1, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1] = { }, },
.repeats = -1, [NOTIFY_SEQUENCE_ARMED_FM_RTH] = { .repeats = -1, .steps = {
.steps = { { .time_off = 100, .time_on = 100, .color = COLOR_GREEN, .repeats = 1, },
{ { .time_off = 100, .time_on = 100, .color = COLOR_YELLOW, .repeats = 1, },
.time_off = 900, }, },
.time_on = 100, [NOTIFY_SEQUENCE_ARMED_FM_LAND] = { .repeats = -1, .steps = {
.color = COLOR_BLUE, { .time_off = 100, .time_on = 100, .color = COLOR_GREEN, .repeats = 1, },
.repeats = 1, }, },
}, [NOTIFY_SEQUENCE_ARMED_FM_AUTO] = { .repeats = -1, .steps = {
}, { .time_off = 100, .time_on = 200, .color = COLOR_GREEN, .repeats = 2, },
}, { .time_off = 500, .time_on = 200, .color = COLOR_GREEN, .repeats = 1, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2] = { }, },
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_BLUE,
.repeats = 1,
},
{
.time_off = 700,
.time_on = 100,
.color = COLOR_BLUE,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_BLUE,
.repeats = 2,
},
{
.time_off = 500,
.time_on = 100,
.color = COLOR_BLUE,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4] = {
.repeats = -1,
.steps = {
{
.time_off = 900,
.time_on = 100,
.color = COLOR_PURPLE,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_PURPLE,
.repeats = 1,
},
{
.time_off = 700,
.time_on = 100,
.color = COLOR_BLUE,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_PURPLE,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 100,
.color = COLOR_PURPLE,
.repeats = 1,
},
{
.time_off = 500,
.time_on = 100,
.color = COLOR_BLUE,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE] = {
.repeats = -1,
.steps = {
{
.time_off = 800,
.time_on = 200,
.color = COLOR_BLUE,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_GPS] = { [NOTIFY_SEQUENCE_ALM_WARN_GPS] = { .repeats = 2, .steps = {
.repeats = -1, { .time_off = 300, .time_on = 300, .color = COLOR_ORANGE, .repeats = 2, },
.steps = { { .time_off = 300, .time_on = 300, .color = COLOR_GREEN, .repeats = 1, },
{ }, },
.time_off = 800, [NOTIFY_SEQUENCE_ALM_ERROR_GPS] = { .repeats = 2, .steps = {
.time_on = 200, { .time_off = 300, .time_on = 300, .color = COLOR_RED, .repeats = 2, },
.color = COLOR_GREEN, { .time_off = 300, .time_on = 300, .color = COLOR_GREEN, .repeats = 1, },
.repeats = 1, }, },
}, [NOTIFY_SEQUENCE_ALM_WARN_BATTERY] = { .repeats = 1, .steps = {
}, { .time_off = 100, .time_on = 100, .color = COLOR_ORANGE, .repeats = 10, },
}, }, },
[NOTIFY_SEQUENCE_ALM_ERROR_BATTERY] = { .repeats = 1, .steps = {
[NOTIFY_SEQUENCE_ARMED_FM_RTH] = { { .time_off = 100, .time_on = 100, .color = COLOR_RED, .repeats = 10, },
.repeats = -1, }, },
.steps = { [NOTIFY_SEQUENCE_ALM_MAG] = { .repeats = 1, .steps = {
{ { .time_off = 300, .time_on = 300, .color = COLOR_RED, .repeats = 2, },
.time_off = 100, { .time_off = 300, .time_on = 300, .color = COLOR_PURPLE, .repeats = 1, },
.time_on = 200, }, },
.color = COLOR_GREEN, [NOTIFY_SEQUENCE_ALM_CONFIG] = { .repeats = 1, .steps = {
.repeats = 1, { .time_off = 50, .time_on = 50, .color = COLOR_RED, .repeats = 9, },
}, { .time_off = 500, .time_on = 50, .color = COLOR_RED, .repeats = 1, },
{ }, },
.time_off = 500, [NOTIFY_SEQUENCE_ALM_RECEIVER] = { .repeats = 1, .steps = {
.time_on = 200, { .time_off = 50, .time_on = 50, .color = COLOR_ORANGE, .repeats = 9, },
.color = COLOR_GREEN, { .time_off = 500, .time_on = 50, .color = COLOR_ORANGE, .repeats = 1, },
.repeats = 1, }, },
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_LAND] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_BLUE,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_BLUE,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_AUTO] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 2,
},
{
.time_off = 500,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_WARN_GPS] = {
.repeats = 2,
.steps = {
{
.time_off = 300,
.time_on = 300,
.color = COLOR_ORANGE,
.repeats = 2,
},
{
.time_off = 300,
.time_on = 300,
.color = COLOR_YELLOW,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_ERROR_GPS] = {
.repeats = 2,
.steps = {
{
.time_off = 300,
.time_on = 300,
.color = COLOR_RED,
.repeats = 2,
},
{
.time_off = 300,
.time_on = 300,
.color = COLOR_YELLOW,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_WARN_BATTERY] = {
.repeats = 1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_ORANGE,
.repeats = 10,
},
},
},
[NOTIFY_SEQUENCE_ALM_ERROR_BATTERY] = {
.repeats = 1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_RED,
.repeats = 10,
},
},
},
[NOTIFY_SEQUENCE_ALM_MAG] = {
.repeats = 1,
.steps = {
{
.time_off = 300,
.time_on = 300,
.color = COLOR_RED,
.repeats = 2,
},
{
.time_off = 300,
.time_on = 300,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_CONFIG] = {
.repeats = -1,
.steps = {
{
.time_off = 500,
.time_on = 100,
.color = COLOR_RED,
.repeats = 1,
},
},
},
}; };
// List of background sequences attached to each flight mode
const LedSequence_t *flightModeMap[] = { const LedSequence_t *flightModeMap[] = {
[FLIGHTSTATUS_FLIGHTMODE_MANUAL] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL], [FLIGHTSTATUS_FLIGHTMODE_MANUAL] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED1] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1], [FLIGHTSTATUS_FLIGHTMODE_STABILIZED1] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1],
@ -392,6 +183,7 @@ const LedSequence_t *flightModeMap[] = {
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
}; };
// List of alarms to show with attached sequences for each status
const AlarmDefinition_t alarmsMap[] = { const AlarmDefinition_t alarmsMap[] = {
{ {
.timeBetweenNotifications = 10000, .timeBetweenNotifications = 10000,
@ -402,7 +194,7 @@ const AlarmDefinition_t alarmsMap[] = {
{ {
.timeBetweenNotifications = 15000, .timeBetweenNotifications = 15000,
.alarmIndex = SYSTEMALARMS_ALARM_MAGNETOMETER, .alarmIndex = SYSTEMALARMS_ALARM_MAGNETOMETER,
.warnNotification = NOTIFY_SEQUENCE_ALM_MAG, .warnNotification = NOTIFY_SEQUENCE_NULL,
.errorNotification = NOTIFY_SEQUENCE_ALM_MAG, .errorNotification = NOTIFY_SEQUENCE_ALM_MAG,
}, },
{ {
@ -411,7 +203,20 @@ const AlarmDefinition_t alarmsMap[] = {
.warnNotification = NOTIFY_SEQUENCE_ALM_WARN_BATTERY, .warnNotification = NOTIFY_SEQUENCE_ALM_WARN_BATTERY,
.errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_BATTERY, .errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_BATTERY,
}, },
{
.timeBetweenNotifications = 5000,
.alarmIndex = SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION,
.warnNotification = NOTIFY_SEQUENCE_NULL,
.errorNotification = NOTIFY_SEQUENCE_ALM_CONFIG,
},
{
.timeBetweenNotifications = 5000,
.alarmIndex = SYSTEMALARMS_ALARM_RECEIVER,
.warnNotification = NOTIFY_SEQUENCE_ALM_RECEIVER,
.errorNotification = NOTIFY_SEQUENCE_ALM_RECEIVER,
},
}; };
const uint8_t alarmsMapSize = NELEMENTS(alarmsMap); const uint8_t alarmsMapSize = NELEMENTS(alarmsMap);
#endif /* SEQUENCES_H_ */ #endif /* SEQUENCES_H_ */

18
flight/modules/Notify/notify.c
View File

@ -49,12 +49,13 @@
#include <lednotification.h> #include <lednotification.h>
#include <optypes.h> #include <optypes.h>
#include <pios_notify.h> #include <pios_notify.h>
#include <freeRTOS.h> #include <FreeRTOS.h>
#include <task.h> #include <task.h>
#include <eventdispatcher.h> #include <eventdispatcher.h>
#include "inc/notify.h" #include "inc/notify.h"
#include "inc/sequences.h" #include "inc/sequences.h"
#include <pios_mem.h> #include <pios_mem.h>
#include <hwsettings.h>
#define SAMPLE_PERIOD_MS 250 #define SAMPLE_PERIOD_MS 250
// private types // private types
@ -71,6 +72,13 @@ static void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_ti
static AlarmStatus_t *alarmStatus; static AlarmStatus_t *alarmStatus;
int32_t NotifyInitialize(void) int32_t NotifyInitialize(void)
{ {
uint8_t ws281xOutStatus;
HwSettingsWS2811LED_OutGet(&ws281xOutStatus);
// Todo: Until further applications exists for WS2811 notify enabled status is tied to ws281x output configuration
bool enabled = ws281xOutStatus != HWSETTINGS_WS2811LED_OUT_DISABLED;
if (enabled) {
alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize); alarmStatus = (AlarmStatus_t *)pios_malloc(sizeof(AlarmStatus_t) * alarmsMapSize);
for (uint8_t i = 0; i < alarmsMapSize; i++) { for (uint8_t i = 0; i < alarmsMapSize; i++) {
alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK; alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK;
@ -78,10 +86,12 @@ int32_t NotifyInitialize(void)
} }
FlightStatusConnectCallback(&updatedCb); FlightStatusConnectCallback(&updatedCb);
updatedCb(0);
static UAVObjEvent ev; static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent)); memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS); EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
updatedCb(0);
}
return 0; return 0;
} }
MODULE_INITCALL(NotifyInitialize, 0); MODULE_INITCALL(NotifyInitialize, 0);
@ -128,11 +138,13 @@ void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time, uin
{ {
if (alarm > SYSTEMALARMS_ALARM_OK) { if (alarm > SYSTEMALARMS_ALARM_OK) {
uint32_t current_time = PIOS_DELAY_GetuS(); uint32_t current_time = PIOS_DELAY_GetuS();
if (*last_alarm < alarm || *last_alm_time + timeBetweenNotifications < current_time) { if (*last_alarm < alarm || *last_alm_time + timeBetweenNotifications * 1000 < current_time) {
uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence : error_sequence; uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence : error_sequence;
if (sequence != NOTIFY_SEQUENCE_NULL) {
PIOS_NOTIFICATION_Default_Ext_Led_Play( PIOS_NOTIFICATION_Default_Ext_Led_Play(
&notifications[sequence], &notifications[sequence],
alarm == SYSTEMALARMS_ALARM_WARNING ? NOTIFY_PRIORITY_REGULAR : NOTIFY_PRIORITY_CRITICAL); alarm == SYSTEMALARMS_ALARM_WARNING ? NOTIFY_PRIORITY_REGULAR : NOTIFY_PRIORITY_CRITICAL);
}
*last_alarm = alarm; *last_alarm = alarm;
*last_alm_time = current_time; *last_alm_time = current_time;
} }

1
flight/pios/inc/pios_ws2811_cfg.h
View File

@ -38,7 +38,6 @@
#include <optypes.h> #include <optypes.h>
#include <pios_ws2811.h> #include <pios_ws2811.h>
#define sign(x) ((x > 0) - (x < 0))
#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24)) #define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24))
#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord #define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord
#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord #define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord

8
flight/pios/stm32f4xx/pios_ws2811.c
View File

@ -310,6 +310,7 @@ void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update)
setColor(c.G, fb + (led * 24)); setColor(c.G, fb + (led * 24));
setColor(c.R, fb + 8 + (led * 24)); setColor(c.R, fb + 8 + (led * 24));
setColor(c.B, fb + 16 + (led * 24)); setColor(c.B, fb + 16 + (led * 24));
if (update) { if (update) {
PIOS_WS2811_Update(); PIOS_WS2811_Update();
} }
@ -327,6 +328,10 @@ void PIOS_WS2811_Update()
// reset counters for synchronization // reset counters for synchronization
pios_ws2811_cfg->timer->CNT = PIOS_WS2811_TIM_PERIOD - 1; pios_ws2811_cfg->timer->CNT = PIOS_WS2811_TIM_PERIOD - 1;
DMA_Cmd(pios_ws2811_cfg->streamCh2, ENABLE);
DMA_Cmd(pios_ws2811_cfg->streamCh1, ENABLE);
DMA_Cmd(pios_ws2811_cfg->streamUpdate, ENABLE);
// Start a new cycle // Start a new cycle
TIM_Cmd(pios_ws2811_cfg->timer, ENABLE); TIM_Cmd(pios_ws2811_cfg->timer, ENABLE);
} }
@ -339,6 +344,9 @@ void PIOS_WS2811_DMA_irq_handler()
{ {
pios_ws2811_cfg->timer->CR1 &= (uint16_t) ~TIM_CR1_CEN; pios_ws2811_cfg->timer->CR1 &= (uint16_t) ~TIM_CR1_CEN;
DMA_ClearFlag(pios_ws2811_cfg->streamCh1, pios_ws2811_cfg->irq.flags); DMA_ClearFlag(pios_ws2811_cfg->streamCh1, pios_ws2811_cfg->irq.flags);
DMA_Cmd(pios_ws2811_cfg->streamCh2, DISABLE);
DMA_Cmd(pios_ws2811_cfg->streamCh1, DISABLE);
DMA_Cmd(pios_ws2811_cfg->streamUpdate, DISABLE);
} }
#endif // PIOS_INCLUDE_WS2811 #endif // PIOS_INCLUDE_WS2811

1
flight/targets/boards/discoveryf4bare/firmware/Makefile
View File

@ -35,6 +35,7 @@ USE_DSP_LIB ?= NO
#MODULES += Airspeed #MODULES += Airspeed
#MODULES += AltitudeHold #MODULES += AltitudeHold
#MODULES += Stabilization #MODULES += Stabilization
MODULES += Notify
MODULES += VtolPathFollower MODULES += VtolPathFollower
MODULES += ManualControl MODULES += ManualControl
MODULES += Actuator MODULES += Actuator

77
flight/tests/lednotification/unittest.cpp
View File

@ -10,43 +10,43 @@ extern "C" {
#include "lednotification.c" #include "lednotification.c"
void PIOS_WS2811_setColorRGB(__attribute__((unused)) Color_t c, __attribute__((unused)) uint8_t led, __attribute__((unused)) bool update){ void PIOS_WS2811_setColorRGB(__attribute__((unused)) Color_t c, __attribute__((unused)) uint8_t led, __attribute__((unused)) bool update) {}
void PIOS_WS2811_Update() {}
}
void PIOS_WS2811_Update(){
}
} }
class LedNotificationTest : public testing::Test {}; class LedNotificationTest : public testing::Test {};
void insert(NotifierLedStatus_t *status, pios_notify_priority priority)
{
ExtLedNotification_t notification;
notification.priority = priority;
push_queued_sequence(&notification, status);
}
void init(NotifierLedStatus_t *status, pios_notify_priority priority)
{
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
status->queued_priorities[i] = priority;
}
}
TEST_F(LedNotificationTest, TestQueueOrder1) { TEST_F(LedNotificationTest, TestQueueOrder1) {
NotifierLedStatus_t status; NotifierLedStatus_t status;
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { init(&status, NOTIFY_PRIORITY_BACKGROUND);
status.queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND;
}
insert(&status, NOTIFY_PRIORITY_LOW);
insert(&status, NOTIFY_PRIORITY_CRITICAL);
insert(&status, NOTIFY_PRIORITY_LOW);
insert(&status, NOTIFY_PRIORITY_CRITICAL);
ExtLedNotification_t notification0;
notification0.priority = NOTIFY_PRIORITY_LOW;
push_queued_sequence(&notification0, &status);
ExtLedNotification_t notification1;
notification1.priority = NOTIFY_PRIORITY_CRITICAL;
push_queued_sequence(&notification1, &status);
ExtLedNotification_t notification2;
notification2.priority = NOTIFY_PRIORITY_LOW;
push_queued_sequence(&notification2, &status);
ExtLedNotification_t notification3;
notification3.priority = NOTIFY_PRIORITY_CRITICAL;
push_queued_sequence(&notification3, &status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]); EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]); EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[1]);
EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[2]); EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[2]);
EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[3]); EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, status.queued_priorities[3]);
EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]); EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]);
} }
TEST_F(LedNotificationTest, TestQueueOrder2) { TEST_F(LedNotificationTest, TestQueueOrder2) {
@ -57,13 +57,9 @@ TEST_F(LedNotificationTest, TestQueueOrder2) {
// 147 status->queued_sequences[insert_point] = new_notification->sequence; // 147 status->queued_sequences[insert_point] = new_notification->sequence;
// 148 updated_sequence = insert_point; // 148 updated_sequence = insert_point;
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { init(&status, NOTIFY_PRIORITY_LOW);
status.queued_priorities[i] = NOTIFY_PRIORITY_LOW;
}
ExtLedNotification_t notification; insert(&status, NOTIFY_PRIORITY_REGULAR);
notification.priority = NOTIFY_PRIORITY_REGULAR;
push_queued_sequence(&notification, &status);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[4]); EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[4]);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[3]); EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[3]);
@ -76,15 +72,28 @@ TEST_F(LedNotificationTest, TestQueueOrder3) {
NotifierLedStatus_t status; NotifierLedStatus_t status;
// Fails because queued_priorities[0] _LOW and not _REGULAR. I _think_ this is a bug. // Fails because queued_priorities[0] _LOW and not _REGULAR. I _think_ this is a bug.
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { init(&status, NOTIFY_PRIORITY_REGULAR);
status.queued_priorities[i] = NOTIFY_PRIORITY_REGULAR;
}
ExtLedNotification_t notification; insert(&status, NOTIFY_PRIORITY_LOW);
notification.priority = NOTIFY_PRIORITY_LOW;
push_queued_sequence(&notification, &status);
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) { for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[i]); EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[i]);
} }
} }
TEST_F(LedNotificationTest, TestQueueOrder4) {
NotifierLedStatus_t status;
init(&status, NOTIFY_PRIORITY_BACKGROUND);
insert(&status, NOTIFY_PRIORITY_REGULAR);
insert(&status, NOTIFY_PRIORITY_REGULAR);
insert(&status, NOTIFY_PRIORITY_REGULAR);
insert(&status, NOTIFY_PRIORITY_LOW);
EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, status.queued_priorities[4]);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[3]);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[2]);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, status.queued_priorities[1]);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, status.queued_priorities[0]);
}