rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-21 11:54:15 +01:00
Code Issues Releases Activity

OP-1302 simplify the code adding some pattern "tables"

Browse Source
This commit is contained in:
Alessio Morale 2014-04-16 07:57:15 +02:00
parent 20508314f2
commit ee91207ede
2 changed files with 117 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -27,9 +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 200
// Define the pause in half blink periods to be added between phases
#define LED_PAUSE_BETWEEN_PHASES 3
// update the status snapshot used by notifcations // update the status snapshot used by notifcations
void NotificationUpdateStatus(); void NotificationUpdateStatus();

219
flight/libraries/notification.c
View File

@ -46,38 +46,82 @@
#define HEARTBEAT_LED_OFF() #define HEARTBEAT_LED_OFF()
#endif #endif
#define ALARM_BLINK_COUNT(x) \ #define BLINK_R_ALARM_PATTERN(x) \
(x == SYSTEMALARMS_ALARM_OK ? 0 : \ (x == SYSTEMALARMS_ALARM_OK ? 0 : \
x == SYSTEMALARMS_ALARM_WARNING ? 1 : \ x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \
x == SYSTEMALARMS_ALARM_ERROR ? 2 : \ x == SYSTEMALARMS_ALARM_ERROR ? 0b0000001000100000 : \
x == SYSTEMALARMS_ALARM_CRITICAL ? 0b0111111111111110 : 0)
#define BLINK_B_ALARM_PATTERN(x) \
(x == SYSTEMALARMS_ALARM_OK ? 0 : \
x == SYSTEMALARMS_ALARM_WARNING ? 0 : \
x == SYSTEMALARMS_ALARM_ERROR ? 0 : \
x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0) x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0)
#define BLINK_COUNT(x) \ #define BLINK_B_FM_ARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 1 : \ (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 2 : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 3 : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 2 : \ x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 2 : \ x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 2 : \ x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0000000000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 3 : \ x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 4 : \ x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 4 : \ x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 3 : \ x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 3 : 1) x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001)
#define BLINK_RED(x) \ #define BLINK_R_FM_ARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? false : \ (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? false : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? false : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0000000000100000 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0000000000100000 : \
x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0000000000100000 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? true : \ x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? true : false) x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000000000 : 0b0000010000000000)
#define BLINK_B_FM_DISARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0001111111111111 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0001111111111111 : 0b0001111111111111)
#define BLINK_R_FM_DISARMED_PATTERN(x) \
(x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0 : \
x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0 : 0)
#define BLINK_B_NOTIFY_PATTERN(x) \
(x == NOTIFY_NONE ? 0 : \
x == NOTIFY_OK ? 0b0000100100111111 : \
x == NOTIFY_NOK ? 0b0000000000111111 : \
x == NOTIFY_DRAW_ATTENTION ? 0b0101010101010101 : 0b0101010101010101)
#define BLINK_R_NOTIFY_PATTERN(x) \
(x == NOTIFY_NONE ? 0 : \
x == NOTIFY_OK ? 0b0000000000001111 : \
x == NOTIFY_NOK ? 0b0011000011001111 : \
x == NOTIFY_DRAW_ATTENTION ? 0b1010101010101010 : 0b1010101010101010)
// led notification handling // led notification handling
static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK; static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK;
@ -86,10 +130,10 @@ static volatile bool started = false;
static volatile pios_notify_notification nextNotification = NOTIFY_NONE; static volatile pios_notify_notification nextNotification = NOTIFY_NONE;
#ifdef PIOS_LED_ALARM #ifdef PIOS_LED_ALARM
static bool handleAlarms(uint8_t cycleCount, uint8_t blinkCount); static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
#endif // PIOS_LED_ALARM #endif // PIOS_LED_ALARM
static bool handleNotifications(uint8_t cycleCount, pios_notify_notification runningNotification); static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern);
static void handleStatus(uint8_t cycleCount, uint8_t blinkCount); static void handleStatus(uint16_t *r_pattern, uint16_t *b_pattern);
void NotificationUpdateStatus() void NotificationUpdateStatus()
{ {
@ -105,8 +149,9 @@ void NotificationUpdateStatus()
void NotificationOnboardLedsRun() void NotificationOnboardLedsRun()
{ {
static portTickType lastRunTimestamp; static portTickType lastRunTimestamp;
static uint8_t blinkCount; // number of blinks since phase start static uint16_t r_pattern;
static uint8_t cycleCount; // cound the number of cycles (half of a blink) static uint16_t b_pattern;
static uint8_t cycleCount; // count the number of cycles
static pios_notify_notification runningNotification = NOTIFY_NONE; static pios_notify_notification runningNotification = NOTIFY_NONE;
static enum { static enum {
@ -116,7 +161,7 @@ void NotificationOnboardLedsRun()
STATUS_LENGHT STATUS_LENGHT
} status; } status;
if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
return; return;
} }
@ -140,29 +185,19 @@ void NotificationOnboardLedsRun()
} }
// check if a phase has just finished // check if a phase has just finished
if (cycleCount & 0x08) { if (cycleCount & 0x10) {
// add a pause between each phase HEARTBEAT_LED_OFF();
if (cycleCount > 0x8 + LED_PAUSE_BETWEEN_PHASES) { ALARM_LED_OFF();
// ready to start a new phase cycleCount = 0x0;
cycleCount = 0x0; // Notification has been just shown, cleanup
if (status == STATUS_NOTIFY) {
// Notification has been just shown, cleanup runningNotification = NOTIFY_NONE;
if (status == STATUS_NOTIFY) {
runningNotification = NOTIFY_NONE;
}
status = (status + 1) % STATUS_LENGHT;
} else {
HEARTBEAT_LED_OFF();
ALARM_LED_OFF();
return;
} }
status = (status + 1) % STATUS_LENGHT;
} }
// a blink last 2 cycles(on and off cycle).
blinkCount = (cycleCount & 0xF) >> 1;
if (status == STATUS_NOTIFY) { if (status == STATUS_NOTIFY) {
if (!handleNotifications(cycleCount, runningNotification)) { if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) {
// no notifications, advance // no notifications, advance
status++; status++;
} }
@ -171,7 +206,7 @@ void NotificationOnboardLedsRun()
// Handles Alarm display // Handles Alarm display
if (status == STATUS_ALARM) { if (status == STATUS_ALARM) {
#ifdef PIOS_LED_ALARM #ifdef PIOS_LED_ALARM
if (!handleAlarms(cycleCount, blinkCount)) { if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) {
// no alarms, advance // no alarms, advance
status++; status++;
} }
@ -182,80 +217,58 @@ void NotificationOnboardLedsRun()
} }
// **** Handles flightmode display // **** Handles flightmode display
if (status == STATUS_FLIGHTMODE) { if (status == STATUS_FLIGHTMODE && !cycleCount) {
handleStatus(cycleCount, blinkCount); handleStatus(&r_pattern, &b_pattern);
} }
// led output
if (b_pattern & 0x1) {
HEARTBEAT_LED_ON();
} else {
HEARTBEAT_LED_OFF();
}
if (r_pattern & 0x1) {
ALARM_LED_ON();
} else {
ALARM_LED_OFF();
}
r_pattern >>= 1;
b_pattern >>= 1;
} }
#if defined(PIOS_LED_ALARM) #if defined(PIOS_LED_ALARM)
static bool handleAlarms(uint8_t cycleCount, uint8_t blinkCount) static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern)
{ {
if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) { if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) {
return false; return false;
} }
*b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel);
if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { *r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel);
// Slow blink
ALARM_LED_OFF();
if (cycleCount & 0x4) {
ALARM_LED_OFF();
} else {
ALARM_LED_ON();
}
} else {
if ((blinkCount < (ALARM_BLINK_COUNT(currentAlarmLevel))) &&
(cycleCount & 0x1)) {
ALARM_LED_ON();
} else {
ALARM_LED_OFF();
}
}
return true; return true;
} }
#endif /* PIOS_LED_ALARM */ #endif /* PIOS_LED_ALARM */
static bool handleNotifications(uint8_t cycleCount, pios_notify_notification runningNotification) static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern)
{ {
if (runningNotification == NOTIFY_NONE) { if (runningNotification == NOTIFY_NONE) {
return false; return false;
} }
if (cycleCount & 0x1) { *b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification);
ALARM_LED_OFF(); *r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification);
HEARTBEAT_LED_ON();
} else {
ALARM_LED_ON();
HEARTBEAT_LED_OFF();
}
return true; return true;
} }
static void handleStatus(uint8_t cycleCount, uint8_t blinkCount) static void handleStatus(uint16_t *r_pattern, uint16_t *b_pattern)
{ {
// Flash the heartbeat LED // Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
uint8_t flightmode = currentFlightStatus.FlightMode; uint8_t flightmode = currentFlightStatus.FlightMode;
if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) {
// Slow blink *b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode);
if (blinkCount < 3) { *r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode);
HEARTBEAT_LED_ON();
} else {
HEARTBEAT_LED_OFF();
}
} else { } else {
if ((blinkCount < BLINK_COUNT(flightmode)) && *b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode);
(cycleCount & 0x1)) { *r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode);
// red led will be active active on last (1-3 blinks) or last two (4 blinks case) blinks
if (BLINK_RED(flightmode) &&
((blinkCount == BLINK_COUNT(flightmode) - 1) ||
blinkCount > 1)) {
ALARM_LED_ON();
}
HEARTBEAT_LED_ON();
} else {
HEARTBEAT_LED_OFF();
ALARM_LED_OFF();
}
} }
#endif /* PIOS_LED_HEARTBEAT */
} }