From 818a42ec059f7c6a11b84db938be7dc9ced86885 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 11 Apr 2014 21:02:29 +0200 Subject: [PATCH 01/10] Fix revolution model filename casing --- .../Revolution/{Revolution.3DS => revolution.3ds} | Bin 1 file changed, 0 insertions(+), 0 deletions(-) rename artwork/3D Model/boards/Revolution/{Revolution.3DS => revolution.3ds} (100%) diff --git a/artwork/3D Model/boards/Revolution/Revolution.3DS b/artwork/3D Model/boards/Revolution/revolution.3ds similarity index 100% rename from artwork/3D Model/boards/Revolution/Revolution.3DS rename to artwork/3D Model/boards/Revolution/revolution.3ds From 82c81fef6e784adab20de0860879cba6be205461 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 12 Apr 2014 22:00:02 +0200 Subject: [PATCH 02/10] OP-1302 make leds more "readable" Show alarm and arming/flightmode in sequence Alarm: Warning = rP Error = rrP Critical = R Heartbeat/flightmode: disarmed = B armed/Stabilization1 = bbP armed/Stabilization2 = bbbP armed/Stabilization3 = bbbbP armed/AH/AVario/Velocitycontrol = bvP armed/PH/Pathplanner/POI = bbvP armed/Land/RTB = bbvvP note: r/b/v = short red/blue/both blink R/B/V = long red/blue/both blink p/P = short/long pause --- flight/libraries/alarms.c | 27 ++++ flight/libraries/inc/alarms.h | 2 + flight/modules/System/systemmod.c | 256 ++++++++++++++++++++++-------- 3 files changed, 218 insertions(+), 67 deletions(-) diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 0ea46c5e5..5dc3cda2e 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -254,6 +254,33 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) xSemaphoreGiveRecursive(lock); return 0; } +/** + * Get the highest alarm severity + * @return + */ +SystemAlarmsAlarmOptions AlarmsGetHighestSeverity() +{ + SystemAlarmsAlarmData alarmsData; + SystemAlarmsAlarmOptions highest = SYSTEMALARMS_ALARM_UNINITIALISED; + + // Lock + xSemaphoreTakeRecursive(lock, portMAX_DELAY); + + // Read alarms + SystemAlarmsAlarmGet(&alarmsData); + + // Go through alarms and find the highest severity + uint32_t n = 0; + while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) { + if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) { + highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n]; + } + n++; + } + + xSemaphoreGiveRecursive(lock); + return highest; +} /** * @} diff --git a/flight/libraries/inc/alarms.h b/flight/libraries/inc/alarms.h index 1d7a4ed3b..304916dbc 100644 --- a/flight/libraries/inc/alarms.h +++ b/flight/libraries/inc/alarms.h @@ -42,9 +42,11 @@ int32_t AlarmsDefault(SystemAlarmsAlarmElem alarm); void AlarmsDefaultAll(); int32_t AlarmsClear(SystemAlarmsAlarmElem alarm); void AlarmsClearAll(); + int32_t AlarmsHasWarnings(); int32_t AlarmsHasErrors(); int32_t AlarmsHasCritical(); +SystemAlarmsAlarmOptions AlarmsGetHighestSeverity(); #endif // ALARMS_H diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 648e2d3a0..d785615c8 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -72,22 +72,65 @@ #endif // Private constants -#define SYSTEM_UPDATE_PERIOD_MS 1000 -#define LED_BLINK_RATE_HZ 5 - -#ifndef IDLE_COUNTS_PER_SEC_AT_NO_LOAD -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 995998 // calibrated by running tests/test_cpuload.c -// must be updated if the FreeRTOS or compiler -// optimisation options are changed. -#endif +#define SYSTEM_UPDATE_PERIOD_MS 500 +#define LED_BLINK_PERIOD_MS 200 #if defined(PIOS_SYSTEM_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE #else -#define STACK_SIZE_BYTES 1024 +#define STACK_SIZE_BYTES 1024 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#ifdef PIOS_LED_ALARM +#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) +#define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM) +#else +#define ALARM_LED_ON() +#define ALARM_LED_OFF() +#endif + +#ifdef PIOS_LED_HEARTBEAT +#define HEARTBEAT_LED_ON() PIOS_LED_On(PIOS_LED_HEARTBEAT) +#define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT) +#else +#define HEARTBEAT_LED_ON() +#define HEARTBEAT_LED_OFF() +#endif + +#define ALARM_BLINK_COUNT(x) \ + (x == SYSTEMALARMS_ALARM_OK ? 0 : \ + x == SYSTEMALARMS_ALARM_WARNING ? 1 : \ + x == SYSTEMALARMS_ALARM_ERROR ? 2 : \ + x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0) + + +#define BLINK_COUNT(x) \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 3 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 4 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 3 : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 4 : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 4 : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 3 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? 3 : 1) + +#define BLINK_RED(x) \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? false : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? false : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? false : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? true : false) + +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -98,6 +141,12 @@ static enum { STACKOVERFLOW_NONE = 0, STACKOVERFLOW_WARNING = 1, STACKOVERFLOW_C static bool mallocFailed; static HwSettingsData bootHwSettings; static struct PIOS_FLASHFS_Stats fsStats; + +// led notification handling +static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK; +static volatile FlightStatusData currentFlightStatus; +static volatile bool started = false; + // Private functions static void objectUpdatedCb(UAVObjEvent *ev); static void hwSettingsUpdatedCb(UAVObjEvent *ev); @@ -170,8 +219,6 @@ MODULE_INITCALL(SystemModInitialize, 0); */ static void systemTask(__attribute__((unused)) void *parameters) { - uint8_t cycleCount = 0; - /* create all modules thread */ MODULE_TASKCREATE_ALL; @@ -189,9 +236,6 @@ static void systemTask(__attribute__((unused)) void *parameters) /* Record a successful boot */ PIOS_IAP_WriteBootCount(0); #endif - - // Initialize vars - // Listen for SettingPersistance object updates, connect a callback function ObjectPersistenceConnectQueue(objectPersistenceQueue); @@ -204,13 +248,13 @@ static void systemTask(__attribute__((unused)) void *parameters) TaskInfoData taskInfoData; CallbackInfoData callbackInfoData; #endif - + started = true; // Main system loop while (1) { + // get values to be used for led handling + FlightStatusGet((FlightStatusData *)¤tFlightStatus); + currentAlarmLevel = AlarmsGetHighestSeverity(); // Update the system statistics - - cycleCount = cycleCount > 0 ? cycleCount - 1 : 7; -// if(cycleCount == 1){ updateStats(); // Update the system alarms updateSystemAlarms(); @@ -230,35 +274,10 @@ static void systemTask(__attribute__((unused)) void *parameters) // } #endif // } - // Flash the heartbeat LED -#if defined(PIOS_LED_HEARTBEAT) - uint8_t armingStatus; - FlightStatusArmedGet(&armingStatus); - if ((armingStatus == FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x1)) || - (armingStatus != FLIGHTSTATUS_ARMED_ARMED && (cycleCount & 0x4))) { - PIOS_LED_On(PIOS_LED_HEARTBEAT); - } else { - PIOS_LED_Off(PIOS_LED_HEARTBEAT); - } - - DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF); -#endif /* PIOS_LED_HEARTBEAT */ - - // Turn on the error LED if an alarm is set -#if defined(PIOS_LED_ALARM) - if (AlarmsHasCritical()) { - PIOS_LED_On(PIOS_LED_ALARM); - } else if ((AlarmsHasErrors() && (cycleCount & 0x1)) || - (!AlarmsHasErrors() && AlarmsHasWarnings() && (cycleCount & 0x4))) { - PIOS_LED_On(PIOS_LED_ALARM); - } else { - PIOS_LED_Off(PIOS_LED_ALARM); - } -#endif /* PIOS_LED_ALARM */ UAVObjEvent ev; - int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2); + int delayTime = SYSTEM_UPDATE_PERIOD_MS; #if defined(PIOS_INCLUDE_RFM22B) @@ -649,43 +668,146 @@ static void updateSystemAlarms() } /** - * Called by the RTOS when the CPU is idle, used to measure the CPU idle time. + * Called by the RTOS when the CPU is idle, */ void vApplicationIdleHook(void) -{} +{ + static portTickType lastRunTimestamp; + if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { + return; + } + lastRunTimestamp = xTaskGetTickCount(); + // the led will show various status information, subdivided in three phases + // - Notification + // - Alarm + // - Flight status + // they are shown using the above priority + // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end + + static enum { + STATUS_NOTIFY, + STATUS_ALARM, + STATUS_FLIGHTMODE, + STATUS_LENGHT + } status; + + static uint8_t cycleCount; + cycleCount++; + // a blink last 2 cycles. + static uint8_t blinkCount; + blinkCount = (cycleCount & 0xF) >> 1; + + if (cycleCount & 0x08) { + // add a short pause between each phase + if (cycleCount > 0xA) { + cycleCount = 0xFF; + status = (status + 1) % STATUS_LENGHT; + } + HEARTBEAT_LED_OFF(); + ALARM_LED_OFF(); + return; + } + + if (status == STATUS_NOTIFY) { + // Not implemented yet + status++; + } + + // Handles Alarm display + if (status == STATUS_ALARM) { +#if defined(PIOS_LED_ALARM) + if (currentAlarmLevel > SYSTEMALARMS_ALARM_OK) { + if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { + // 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(); + } + } + } else { + status++; + } +#else /* if defined(PIOS_LED_ALARM) */ + // no alarms, handle next phase + status++; + // #endif +#endif /* PIOS_LED_ALARM */ + } + + // **** Handles flightmode display + if (status == STATUS_FLIGHTMODE) { + uint8_t flightmode = currentFlightStatus.FlightMode; + + // Flash the heartbeat LED +#if defined(PIOS_LED_HEARTBEAT) + + if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { + // Slow blink + if (blinkCount < 3) { + HEARTBEAT_LED_ON(); + } else { + HEARTBEAT_LED_OFF(); + } + } else { + if ((blinkCount < BLINK_COUNT(flightmode)) && + (cycleCount & 0x1)) { + // red led will be active active in last 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 */ + } /** * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 -void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, - __attribute__((unused)) signed portCHAR *pcTaskName) -{ - stackOverflow = STACKOVERFLOW_CRITICAL; + void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, + __attribute__((unused)) signed portCHAR *pcTaskName) + { + stackOverflow = STACKOVERFLOW_CRITICAL; #if DEBUG_STACK_OVERFLOW - static volatile bool wait_here = true; - while (wait_here) { - ; - } - wait_here = true; + static volatile bool wait_here = true; + while (wait_here) { + ; + } + wait_here = true; #endif -} + } /** * Called by the RTOS when a malloc call fails. */ #define DEBUG_MALLOC_FAILURES 0 -void vApplicationMallocFailedHook(void) -{ - mallocFailed = true; + void vApplicationMallocFailedHook(void) + { + mallocFailed = true; #if DEBUG_MALLOC_FAILURES - static volatile bool wait_here = true; - while (wait_here) { - ; - } - wait_here = true; + static volatile bool wait_here = true; + while (wait_here) { + ; + } + wait_here = true; #endif -} + } /** * @} From 9ebb66fbce4a5f900e67aec4e720d6cc1a4112ed Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 12 Apr 2014 22:00:26 +0200 Subject: [PATCH 03/10] OP-1302 filter out telemetry alarms --- flight/libraries/alarms.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 5dc3cda2e..7f2688a73 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -244,7 +244,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) // Go through alarms and check if any are of the given severity or higher for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { - if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) { + if (n != SYSTEMALARMS_ALARM_TELEMETRY && cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) { xSemaphoreGiveRecursive(lock); return 1; } @@ -272,7 +272,8 @@ SystemAlarmsAlarmOptions AlarmsGetHighestSeverity() // Go through alarms and find the highest severity uint32_t n = 0; while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) { - if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) { + if (n != SYSTEMALARMS_ALARM_TELEMETRY && + cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) { highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n]; } n++; From 5d637fab87a0aa72886cb8117df26c6dc4b16f91 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 13 Apr 2014 11:54:25 +0200 Subject: [PATCH 04/10] OP-1302 move stuffs away from systemmod --- flight/libraries/inc/notification.h | 37 ++++ flight/libraries/notification.c | 197 ++++++++++++++++++ flight/modules/System/systemmod.c | 168 +-------------- .../boards/revolution/firmware/Makefile | 4 + 4 files changed, 242 insertions(+), 164 deletions(-) create mode 100644 flight/libraries/inc/notification.h create mode 100644 flight/libraries/notification.c diff --git a/flight/libraries/inc/notification.h b/flight/libraries/inc/notification.h new file mode 100644 index 000000000..62a9d1c58 --- /dev/null +++ b/flight/libraries/inc/notification.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * + * @file notification.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief notification library + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef NOTIFICATION_H +#define NOTIFICATION_H + +#define LED_BLINK_PERIOD_MS 200 + +// update the status snapshot used by notifcations +void NotificationUpdateStatus(); + +// run the led notifications +void NotificationOnboardLedsRun(); + +#endif /* NOTIFICATION_H */ diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c new file mode 100644 index 000000000..f7126f08e --- /dev/null +++ b/flight/libraries/notification.c @@ -0,0 +1,197 @@ +/** + ****************************************************************************** + * + * @file notification.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief brief goes here. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#include "inc/notification.h" +#include +#include +#include +#include + +#ifdef PIOS_LED_ALARM +#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) +#define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM) +#else +#define ALARM_LED_ON() +#define ALARM_LED_OFF() +#endif + +#ifdef PIOS_LED_HEARTBEAT +#define HEARTBEAT_LED_ON() PIOS_LED_On(PIOS_LED_HEARTBEAT) +#define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT) +#else +#define HEARTBEAT_LED_ON() +#define HEARTBEAT_LED_OFF() +#endif + +#define ALARM_BLINK_COUNT(x) \ + (x == SYSTEMALARMS_ALARM_OK ? 0 : \ + x == SYSTEMALARMS_ALARM_WARNING ? 1 : \ + x == SYSTEMALARMS_ALARM_ERROR ? 2 : \ + x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0) + + +#define BLINK_COUNT(x) \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 3 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 4 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 3 : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 4 : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 4 : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 3 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? 3 : 1) + +#define BLINK_RED(x) \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? false : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? false : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? false : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? true : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? true : false) + +// led notification handling +static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK; +static volatile FlightStatusData currentFlightStatus; +static volatile bool started = false; + +void NotificationUpdateStatus(){ + started = true; + // get values to be used for led handling + FlightStatusGet((FlightStatusData *)¤tFlightStatus); + currentAlarmLevel = AlarmsGetHighestSeverity(); +} + +void NotificationOnboardLedsRun(){ + static portTickType lastRunTimestamp; + + if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { + return; + } + lastRunTimestamp = xTaskGetTickCount(); + // the led will show various status information, subdivided in three phases + // - Notification + // - Alarm + // - Flight status + // they are shown using the above priority + // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end + + static enum { + STATUS_NOTIFY, + STATUS_ALARM, + STATUS_FLIGHTMODE, + STATUS_LENGHT + } status; + + static uint8_t cycleCount; + cycleCount++; + // a blink last 2 cycles. + static uint8_t blinkCount; + blinkCount = (cycleCount & 0xF) >> 1; + + if (cycleCount & 0x08) { + // add a short pause between each phase + if (cycleCount > 0xA) { + cycleCount = 0xFF; + status = (status + 1) % STATUS_LENGHT; + } + HEARTBEAT_LED_OFF(); + ALARM_LED_OFF(); + return; + } + + if (status == STATUS_NOTIFY) { + // Not implemented yet + status++; + } + + // Handles Alarm display + if (status == STATUS_ALARM) { +#if defined(PIOS_LED_ALARM) + if (currentAlarmLevel > SYSTEMALARMS_ALARM_OK) { + if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { + // 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(); + } + } + } else { + status++; + } +#else /* if defined(PIOS_LED_ALARM) */ + // no alarms, handle next phase + status++; + // #endif +#endif /* PIOS_LED_ALARM */ + } + + // **** Handles flightmode display + if (status == STATUS_FLIGHTMODE) { + uint8_t flightmode = currentFlightStatus.FlightMode; + + // Flash the heartbeat LED +#if defined(PIOS_LED_HEARTBEAT) + + if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { + // Slow blink + if (blinkCount < 3) { + HEARTBEAT_LED_ON(); + } else { + HEARTBEAT_LED_OFF(); + } + } else { + if ((blinkCount < BLINK_COUNT(flightmode)) && + (cycleCount & 0x1)) { + // red led will be active active in last 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 */ +} diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index d785615c8..1ad8b188e 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -42,7 +42,7 @@ #include // private includes #include "inc/systemmod.h" - +#include "notification.h" // UAVOs #include @@ -73,7 +73,6 @@ // Private constants #define SYSTEM_UPDATE_PERIOD_MS 500 -#define LED_BLINK_PERIOD_MS 200 #if defined(PIOS_SYSTEM_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE @@ -81,55 +80,6 @@ #define STACK_SIZE_BYTES 1024 #endif -#ifdef PIOS_LED_ALARM -#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) -#define ALARM_LED_OFF() PIOS_LED_Off(PIOS_LED_ALARM) -#else -#define ALARM_LED_ON() -#define ALARM_LED_OFF() -#endif - -#ifdef PIOS_LED_HEARTBEAT -#define HEARTBEAT_LED_ON() PIOS_LED_On(PIOS_LED_HEARTBEAT) -#define HEARTBEAT_LED_OFF() PIOS_LED_Off(PIOS_LED_HEARTBEAT) -#else -#define HEARTBEAT_LED_ON() -#define HEARTBEAT_LED_OFF() -#endif - -#define ALARM_BLINK_COUNT(x) \ - (x == SYSTEMALARMS_ALARM_OK ? 0 : \ - x == SYSTEMALARMS_ALARM_WARNING ? 1 : \ - x == SYSTEMALARMS_ALARM_ERROR ? 2 : \ - x == SYSTEMALARMS_ALARM_CRITICAL ? 0 : 0) - - -#define BLINK_COUNT(x) \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 4 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 4 : \ - x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 4 : \ - x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_POI ? 3 : 1) - -#define BLINK_RED(x) \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? false : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? false : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? false : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_LAND ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_POI ? true : false) - #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -142,11 +92,6 @@ static bool mallocFailed; static HwSettingsData bootHwSettings; static struct PIOS_FLASHFS_Stats fsStats; -// led notification handling -static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK; -static volatile FlightStatusData currentFlightStatus; -static volatile bool started = false; - // Private functions static void objectUpdatedCb(UAVObjEvent *ev); static void hwSettingsUpdatedCb(UAVObjEvent *ev); @@ -248,12 +193,9 @@ static void systemTask(__attribute__((unused)) void *parameters) TaskInfoData taskInfoData; CallbackInfoData callbackInfoData; #endif - started = true; // Main system loop while (1) { - // get values to be used for led handling - FlightStatusGet((FlightStatusData *)¤tFlightStatus); - currentAlarmLevel = AlarmsGetHighestSeverity(); + NotificationUpdateStatus(); // Update the system statistics updateStats(); // Update the system alarms @@ -672,110 +614,8 @@ static void updateSystemAlarms() */ void vApplicationIdleHook(void) { - static portTickType lastRunTimestamp; - - if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { - return; - } - lastRunTimestamp = xTaskGetTickCount(); - // the led will show various status information, subdivided in three phases - // - Notification - // - Alarm - // - Flight status - // they are shown using the above priority - // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end - - static enum { - STATUS_NOTIFY, - STATUS_ALARM, - STATUS_FLIGHTMODE, - STATUS_LENGHT - } status; - - static uint8_t cycleCount; - cycleCount++; - // a blink last 2 cycles. - static uint8_t blinkCount; - blinkCount = (cycleCount & 0xF) >> 1; - - if (cycleCount & 0x08) { - // add a short pause between each phase - if (cycleCount > 0xA) { - cycleCount = 0xFF; - status = (status + 1) % STATUS_LENGHT; - } - HEARTBEAT_LED_OFF(); - ALARM_LED_OFF(); - return; - } - - if (status == STATUS_NOTIFY) { - // Not implemented yet - status++; - } - - // Handles Alarm display - if (status == STATUS_ALARM) { -#if defined(PIOS_LED_ALARM) - if (currentAlarmLevel > SYSTEMALARMS_ALARM_OK) { - if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { - // 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(); - } - } - } else { - status++; - } -#else /* if defined(PIOS_LED_ALARM) */ - // no alarms, handle next phase - status++; - // #endif -#endif /* PIOS_LED_ALARM */ - } - - // **** Handles flightmode display - if (status == STATUS_FLIGHTMODE) { - uint8_t flightmode = currentFlightStatus.FlightMode; - - // Flash the heartbeat LED -#if defined(PIOS_LED_HEARTBEAT) - - if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { - // Slow blink - if (blinkCount < 3) { - HEARTBEAT_LED_ON(); - } else { - HEARTBEAT_LED_OFF(); - } - } else { - if ((blinkCount < BLINK_COUNT(flightmode)) && - (cycleCount & 0x1)) { - // red led will be active active in last 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 */ - } + NotificationOnboardLedsRun(); +} /** * Called by the RTOS when a stack overflow is detected. */ diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 5b89372a1..93aea56d0 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -53,6 +53,8 @@ MODULES += Telemetry OPTMODULES += ComUsbBridge +SRC += $(FLIGHTLIB)/notification.c + # Include all camera options CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF @@ -96,7 +98,9 @@ endif # Optional component libraries include $(FLIGHTLIB)/rscode/library.mk + #include $(FLIGHTLIB)/PyMite/pymite.mk + include $(ROOT_DIR)/make/apps-defs.mk include $(ROOT_DIR)/make/common-defs.mk From 132aa83dff90044da7c61808be44e76dc6bdd82e Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 13 Apr 2014 12:38:07 +0200 Subject: [PATCH 05/10] OP-1302 Fix other targets --- flight/targets/boards/coptercontrol/firmware/Makefile | 4 ++++ flight/targets/boards/osd/firmware/Makefile | 2 ++ flight/targets/boards/revoproto/firmware/Makefile | 2 ++ flight/targets/boards/simposix/firmware/Makefile | 2 ++ 4 files changed, 10 insertions(+) diff --git a/flight/targets/boards/coptercontrol/firmware/Makefile b/flight/targets/boards/coptercontrol/firmware/Makefile index 6c0038305..c0cf05285 100644 --- a/flight/targets/boards/coptercontrol/firmware/Makefile +++ b/flight/targets/boards/coptercontrol/firmware/Makefile @@ -48,9 +48,13 @@ OPTMODULES += Osd/osdoutput #OPTMODULES += Altitude #OPTMODULES += Fault +SRC += $(FLIGHTLIB)/notification.c + # Include all camera options CDEFS += -DUSE_INPUT_LPF -DUSE_GIMBAL_LPF -DUSE_GIMBAL_FF + + # Erase flash firmware should be buildable from command line ifeq ($(ERASE_FLASH), YES) CDEFS += -DERASE_FLASH diff --git a/flight/targets/boards/osd/firmware/Makefile b/flight/targets/boards/osd/firmware/Makefile index 14d863b1b..3744ee56a 100644 --- a/flight/targets/boards/osd/firmware/Makefile +++ b/flight/targets/boards/osd/firmware/Makefile @@ -37,6 +37,8 @@ MODULES += Telemetry OPTMODULES = +SRC += $(FLIGHTLIB)/notification.c + # Some diagnostics CDEFS += -DDIAG_TASKS diff --git a/flight/targets/boards/revoproto/firmware/Makefile b/flight/targets/boards/revoproto/firmware/Makefile index 2e5df1ac3..16e4d7072 100644 --- a/flight/targets/boards/revoproto/firmware/Makefile +++ b/flight/targets/boards/revoproto/firmware/Makefile @@ -50,6 +50,8 @@ MODULES += Osd/osdoutout MODULES += Logging MODULES += Telemetry +SRC += $(FLIGHTLIB)/notification.c + OPTMODULES += ComUsbBridge # Include all camera options diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index beda566a8..d6c1f0a14 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -45,6 +45,8 @@ MODULES += Airspeed MODULES += AltitudeHold #MODULES += OveroSync +SRC += $(FLIGHTLIB)/notification.c + # Paths OPSYSTEM = . BOARDINC = .. From 299f388245d25d5d72593809028a6717c3488701 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 13 Apr 2014 17:42:57 +0200 Subject: [PATCH 06/10] OP-1302 Add a stub pios_notify API for user interaction/notifications right now all notifications produces the same output, the DRAW_ATTENTION sequence --- flight/libraries/inc/notification.h | 29 ++- flight/libraries/notification.c | 233 +++++++++++------- flight/modules/System/systemmod.c | 42 ++-- flight/pios/common/pios_notify.c | 49 ++++ flight/pios/inc/pios_notify.h | 58 +++++ .../targets/boards/simposix/firmware/Makefile | 2 + make/apps-defs.mk | 2 +- 7 files changed, 295 insertions(+), 120 deletions(-) create mode 100644 flight/pios/common/pios_notify.c create mode 100644 flight/pios/inc/pios_notify.h diff --git a/flight/libraries/inc/notification.h b/flight/libraries/inc/notification.h index 62a9d1c58..65b902d1d 100644 --- a/flight/libraries/inc/notification.h +++ b/flight/libraries/inc/notification.h @@ -1,32 +1,35 @@ /** ****************************************************************************** * - * @file notification.h + * @file notification.h * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @brief notification library * -- * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef NOTIFICATION_H #define NOTIFICATION_H -#define LED_BLINK_PERIOD_MS 200 +// period of each blink phase +#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 void NotificationUpdateStatus(); diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index f7126f08e..a7f722bc2 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -1,26 +1,26 @@ /** ****************************************************************************** * - * @file notification.c + * @file notification.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. - * @brief brief goes here. + * @brief notification library. * -- * @see The GNU Public License (GPL) Version 3 * *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "inc/notification.h" @@ -28,6 +28,7 @@ #include #include #include +#include #ifdef PIOS_LED_ALARM #define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM) @@ -53,9 +54,9 @@ #define BLINK_COUNT(x) \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 4 : \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 1 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 2 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 3 : \ x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 2 : \ x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 2 : \ x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 2 : \ @@ -82,27 +83,31 @@ static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK; static volatile FlightStatusData currentFlightStatus; static volatile bool started = false; +static volatile pios_notify_notification nextNotification = NOTIFY_NONE; -void NotificationUpdateStatus(){ +#ifdef PIOS_LED_ALARM +static bool handleAlarms(uint8_t cycleCount, uint8_t blinkCount); +#endif // PIOS_LED_ALARM +static bool handleNotifications(uint8_t cycleCount, pios_notify_notification runningNotification); +static void handleStatus(uint8_t cycleCount, uint8_t blinkCount); + +void NotificationUpdateStatus() +{ started = true; // get values to be used for led handling FlightStatusGet((FlightStatusData *)¤tFlightStatus); currentAlarmLevel = AlarmsGetHighestSeverity(); + if (nextNotification == NOTIFY_NONE) { + nextNotification = PIOS_NOTIFY_GetActiveNotification(true); + } } -void NotificationOnboardLedsRun(){ +void NotificationOnboardLedsRun() +{ static portTickType lastRunTimestamp; - - if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { - return; - } - lastRunTimestamp = xTaskGetTickCount(); - // the led will show various status information, subdivided in three phases - // - Notification - // - Alarm - // - Flight status - // they are shown using the above priority - // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end + static uint8_t blinkCount; // number of blinks since phase start + static uint8_t cycleCount; // cound the number of cycles (half of a blink) + static pios_notify_notification runningNotification = NOTIFY_NONE; static enum { STATUS_NOTIFY, @@ -111,87 +116,145 @@ void NotificationOnboardLedsRun(){ STATUS_LENGHT } status; - static uint8_t cycleCount; - cycleCount++; - // a blink last 2 cycles. - static uint8_t blinkCount; - blinkCount = (cycleCount & 0xF) >> 1; - if (cycleCount & 0x08) { - // add a short pause between each phase - if (cycleCount > 0xA) { - cycleCount = 0xFF; - status = (status + 1) % STATUS_LENGHT; - } - HEARTBEAT_LED_OFF(); - ALARM_LED_OFF(); + if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { return; } + lastRunTimestamp = xTaskGetTickCount(); + + // the led will show various status information, subdivided in three phases + // - Notification + // - Alarm + // - Flight status + // they are shown using the above priority + // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end + + cycleCount++; + + // Notifications are "modal" to other statuses so they takes precedence + if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) { + // Force a notification status + runningNotification = nextNotification; + nextNotification = NOTIFY_NONE; + + status = STATUS_NOTIFY; + cycleCount = 0; // instantly start a notify cycle + } + + // check if a phase has just finished + if (cycleCount & 0x08) { + // add a short pause between each phase + if (cycleCount > 0x8 + LED_PAUSE_BETWEEN_PHASES) { + // ready to start a new phase + cycleCount = 0x0; + + // Notification has been already shown, so clear the running one + if (status == STATUS_NOTIFY) { + runningNotification = NOTIFY_NONE; + } + status = (status + 1) % STATUS_LENGHT; + } else { + HEARTBEAT_LED_OFF(); + ALARM_LED_OFF(); + return; + } + } + + // a blink last 2 cycles. + blinkCount = (cycleCount & 0xF) >> 1; + if (status == STATUS_NOTIFY) { - // Not implemented yet - status++; + if (!handleNotifications(cycleCount, runningNotification)) { + status++; + } } // Handles Alarm display if (status == STATUS_ALARM) { -#if defined(PIOS_LED_ALARM) - if (currentAlarmLevel > SYSTEMALARMS_ALARM_OK) { - if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { - // 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(); - } - } - } else { +#ifdef PIOS_LED_ALARM + if (!handleAlarms(cycleCount, blinkCount)) { status++; } -#else /* if defined(PIOS_LED_ALARM) */ - // no alarms, handle next phase +#else status++; - // #endif -#endif /* PIOS_LED_ALARM */ +#endif // PIOS_LED_ALARM } - // **** Handles flightmode display if (status == STATUS_FLIGHTMODE) { - uint8_t flightmode = currentFlightStatus.FlightMode; + handleStatus(cycleCount, blinkCount); + } +} - // Flash the heartbeat LED -#if defined(PIOS_LED_HEARTBEAT) - - if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { +#if defined(PIOS_LED_ALARM) +static bool handleAlarms(uint8_t cycleCount, uint8_t blinkCount) +{ + if (currentAlarmLevel > SYSTEMALARMS_ALARM_OK) { + if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { // Slow blink - if (blinkCount < 3) { - HEARTBEAT_LED_ON(); + ALARM_LED_OFF(); + if (cycleCount & 0x4) { + ALARM_LED_OFF(); } else { - HEARTBEAT_LED_OFF(); + ALARM_LED_ON(); } } else { - if ((blinkCount < BLINK_COUNT(flightmode)) && + if ((blinkCount < (ALARM_BLINK_COUNT(currentAlarmLevel))) && (cycleCount & 0x1)) { - // red led will be active active in last or last two (4 blinks case) blinks - if (BLINK_RED(flightmode) && - ((blinkCount == BLINK_COUNT(flightmode) - 1) || - blinkCount > 1)) { - ALARM_LED_ON(); - } - HEARTBEAT_LED_ON(); + ALARM_LED_ON(); } else { - HEARTBEAT_LED_OFF(); ALARM_LED_OFF(); } } + return true; + } else { + return false; + } +} +#endif /* PIOS_LED_ALARM */ + + +static bool handleNotifications(uint8_t cycleCount, pios_notify_notification runningNotification) +{ + if (runningNotification == NOTIFY_NONE) { + return false; + } + if (cycleCount & 0x1) { + ALARM_LED_OFF(); + HEARTBEAT_LED_ON(); + } else { + ALARM_LED_ON(); + HEARTBEAT_LED_OFF(); + } + return true; +} + +static void handleStatus(uint8_t cycleCount, uint8_t blinkCount) +{ + // Flash the heartbeat LED +#if defined(PIOS_LED_HEARTBEAT) + uint8_t flightmode = currentFlightStatus.FlightMode; + if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { + // Slow blink + if (blinkCount < 3) { + HEARTBEAT_LED_ON(); + } else { + HEARTBEAT_LED_OFF(); + } + } else { + if ((blinkCount < BLINK_COUNT(flightmode)) && + (cycleCount & 0x1)) { + // red led will be active active in last 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 */ } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 1ad8b188e..c61a3bbc6 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -72,7 +72,7 @@ #endif // Private constants -#define SYSTEM_UPDATE_PERIOD_MS 500 +#define SYSTEM_UPDATE_PERIOD_MS 250 #if defined(PIOS_SYSTEM_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE @@ -80,7 +80,7 @@ #define STACK_SIZE_BYTES 1024 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) // Private types @@ -620,34 +620,34 @@ void vApplicationIdleHook(void) * Called by the RTOS when a stack overflow is detected. */ #define DEBUG_STACK_OVERFLOW 0 - void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, - __attribute__((unused)) signed portCHAR *pcTaskName) - { - stackOverflow = STACKOVERFLOW_CRITICAL; +void vApplicationStackOverflowHook(__attribute__((unused)) xTaskHandle *pxTask, + __attribute__((unused)) signed portCHAR *pcTaskName) +{ + stackOverflow = STACKOVERFLOW_CRITICAL; #if DEBUG_STACK_OVERFLOW - static volatile bool wait_here = true; - while (wait_here) { - ; - } - wait_here = true; -#endif + static volatile bool wait_here = true; + while (wait_here) { + ; } + wait_here = true; +#endif +} /** * Called by the RTOS when a malloc call fails. */ #define DEBUG_MALLOC_FAILURES 0 - void vApplicationMallocFailedHook(void) - { - mallocFailed = true; +void vApplicationMallocFailedHook(void) +{ + mallocFailed = true; #if DEBUG_MALLOC_FAILURES - static volatile bool wait_here = true; - while (wait_here) { - ; - } - wait_here = true; -#endif + static volatile bool wait_here = true; + while (wait_here) { + ; } + wait_here = true; +#endif +} /** * @} diff --git a/flight/pios/common/pios_notify.c b/flight/pios/common/pios_notify.c new file mode 100644 index 000000000..bf83c22eb --- /dev/null +++ b/flight/pios/common/pios_notify.c @@ -0,0 +1,49 @@ +/** + ****************************************************************************** + * + * @file pios_notify.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Handles user notifications. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "pios_notify.h" + +static volatile pios_notify_notification currentNotification = NOTIFY_NONE; +static volatile pios_notify_priority currentPriority; + + +void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority) +{ + if (currentNotification == NOTIFY_NONE || currentPriority < priority) { + currentPriority = priority; + currentNotification = notification; + } +} + +pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear) +{ + pios_notify_notification ret = currentNotification; + + if (clear && ret != NOTIFY_NONE) { + currentNotification = NOTIFY_NONE; + } + return ret; +} diff --git a/flight/pios/inc/pios_notify.h b/flight/pios/inc/pios_notify.h new file mode 100644 index 000000000..1e800df5c --- /dev/null +++ b/flight/pios/inc/pios_notify.h @@ -0,0 +1,58 @@ +/** + ****************************************************************************** + * + * @file pios_notify.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Handles user notifications. + * -- + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ +#ifndef PIOS_NOTIFY_H_ +#define PIOS_NOTIFY_H_ +#include + +typedef enum { + NOTIFY_NONE = 0, + NOTIFY_OK, + NOTIFY_NOK, + NOTIFY_DRAW_ATTENTION +} pios_notify_notification; + +typedef enum { + NOTIFY_PRIORITY_CRITICAL = 2, + NOTIFY_PRIORITY_REGULAR = 1, + NOTIFY_PRIORITY_LOW = 0, +} pios_notify_priority; + +/** + * start a new notification. If a notification is active it will be overwritten if its priority is lower than the new one. + * The new will be discarded otherwise + * @param notification kind of notification + * @param priority priority of the new notification + */ +void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority); + +/** + * retrieve any active notification + * @param clear + * @return + */ +pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear); + +#endif /* PIOS_NOTIFY_H_ */ diff --git a/flight/targets/boards/simposix/firmware/Makefile b/flight/targets/boards/simposix/firmware/Makefile index d6c1f0a14..5169ef268 100644 --- a/flight/targets/boards/simposix/firmware/Makefile +++ b/flight/targets/boards/simposix/firmware/Makefile @@ -104,6 +104,8 @@ SRC += $(PIOSCORECOMMON)/pios_dosfs_logfs.c SRC += $(PIOSCORECOMMON)/pios_debuglog.c SRC += $(PIOSCORECOMMON)/pios_callbackscheduler.c SRC += $(PIOSCORECOMMON)/pios_deltatime.c +SRC += $(PIOSCORECOMMON)/pios_notify.c + ## PIOS Hardware include $(PIOS)/posix/library.mk diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 20853c568..88dfe4121 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -99,7 +99,7 @@ SRC += $(PIOSCOMMON)/pios_usb_util.c ## PIOS system code SRC += $(PIOSCOMMON)/pios_task_monitor.c SRC += $(PIOSCOMMON)/pios_callbackscheduler.c - +SRC += $(PIOSCOMMON)/pios_notify.c ## Misc library functions SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/sanitycheck.c From 20508314f25f96c296bc6998da443dd148a15c6b Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sun, 13 Apr 2014 17:56:20 +0200 Subject: [PATCH 07/10] OP-1302 use pios_notify api to alert user of ongoing CF gyro calibration --- flight/libraries/notification.c | 57 ++++++++++++----------- flight/modules/Attitude/attitude.c | 4 +- flight/modules/StateEstimation/filtercf.c | 4 +- 3 files changed, 35 insertions(+), 30 deletions(-) diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index a7f722bc2..1b5a9c752 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -116,13 +116,11 @@ void NotificationOnboardLedsRun() STATUS_LENGHT } status; - if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 2)) { return; } lastRunTimestamp = xTaskGetTickCount(); - // the led will show various status information, subdivided in three phases // - Notification // - Alarm @@ -131,25 +129,24 @@ void NotificationOnboardLedsRun() // a phase last exactly 8 cycles (so bit 1<<4 is used to determine if a phase end cycleCount++; - // Notifications are "modal" to other statuses so they takes precedence if (status != STATUS_NOTIFY && nextNotification != NOTIFY_NONE) { - // Force a notification status + // read next notification to show runningNotification = nextNotification; nextNotification = NOTIFY_NONE; - + // Force a notification status status = STATUS_NOTIFY; cycleCount = 0; // instantly start a notify cycle } // check if a phase has just finished if (cycleCount & 0x08) { - // add a short pause between each phase + // add a pause between each phase if (cycleCount > 0x8 + LED_PAUSE_BETWEEN_PHASES) { // ready to start a new phase cycleCount = 0x0; - // Notification has been already shown, so clear the running one + // Notification has been just shown, cleanup if (status == STATUS_NOTIFY) { runningNotification = NOTIFY_NONE; } @@ -161,11 +158,12 @@ void NotificationOnboardLedsRun() } } - // a blink last 2 cycles. + // a blink last 2 cycles(on and off cycle). blinkCount = (cycleCount & 0xF) >> 1; if (status == STATUS_NOTIFY) { if (!handleNotifications(cycleCount, runningNotification)) { + // no notifications, advance status++; } } @@ -174,12 +172,15 @@ void NotificationOnboardLedsRun() if (status == STATUS_ALARM) { #ifdef PIOS_LED_ALARM if (!handleAlarms(cycleCount, blinkCount)) { + // no alarms, advance status++; } #else + // no alarms leds, advance status++; #endif // PIOS_LED_ALARM } + // **** Handles flightmode display if (status == STATUS_FLIGHTMODE) { handleStatus(cycleCount, blinkCount); @@ -189,27 +190,27 @@ void NotificationOnboardLedsRun() #if defined(PIOS_LED_ALARM) static bool handleAlarms(uint8_t cycleCount, uint8_t blinkCount) { - if (currentAlarmLevel > SYSTEMALARMS_ALARM_OK) { - if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { - // 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; - } else { + if (currentAlarmLevel == SYSTEMALARMS_ALARM_OK) { return false; } + + if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { + // 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; } #endif /* PIOS_LED_ALARM */ @@ -244,7 +245,7 @@ static void handleStatus(uint8_t cycleCount, uint8_t blinkCount) } else { if ((blinkCount < BLINK_COUNT(flightmode)) && (cycleCount & 0x1)) { - // red led will be active active in last or last two (4 blinks case) blinks + // 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)) { diff --git a/flight/modules/Attitude/attitude.c b/flight/modules/Attitude/attitude.c index e004c04b3..92f07ae01 100644 --- a/flight/modules/Attitude/attitude.c +++ b/flight/modules/Attitude/attitude.c @@ -63,7 +63,7 @@ #include "taskinfo.h" #include "CoordinateConversions.h" - +#include // Private constants #define STACK_SIZE_BYTES 540 @@ -252,6 +252,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) rollPitchBiasRate = 0.01f; accel_filter_enabled = false; init = 0; + PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (zero_during_arming && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { accelKp = 1.0f; accelKi = 0.0f; @@ -259,6 +260,7 @@ static void AttitudeTask(__attribute__((unused)) void *parameters) rollPitchBiasRate = 0.01f; accel_filter_enabled = false; init = 0; + PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsAccelKiGet(&accelKi); diff --git a/flight/modules/StateEstimation/filtercf.c b/flight/modules/StateEstimation/filtercf.c index 992eef8ef..0daea0928 100644 --- a/flight/modules/StateEstimation/filtercf.c +++ b/flight/modules/StateEstimation/filtercf.c @@ -42,7 +42,7 @@ #include #include - +#include // Private constants #define STACK_REQUIRED 512 @@ -295,6 +295,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel this->accel_filter_enabled = false; this->rollPitchBiasRate = 0.01f; this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f; + PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKi = 0.0f; @@ -303,6 +304,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel this->rollPitchBiasRate = 0.01f; this->attitudeSettings.MagKp = this->magCalibrated ? 1.0f : 0.0f; this->init = 0; + PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); } else if (this->init == 0) { // Reload settings (all the rates) AttitudeSettingsGet(&this->attitudeSettings); From ee91207ede61b56715df0dc0434fcb6561fe73b0 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Wed, 16 Apr 2014 07:57:15 +0200 Subject: [PATCH 08/10] OP-1302 simplify the code adding some pattern "tables" --- flight/libraries/inc/notification.h | 4 +- flight/libraries/notification.c | 219 +++++++++++++++------------- 2 files changed, 117 insertions(+), 106 deletions(-) diff --git a/flight/libraries/inc/notification.h b/flight/libraries/inc/notification.h index 65b902d1d..2053a9609 100644 --- a/flight/libraries/inc/notification.h +++ b/flight/libraries/inc/notification.h @@ -27,9 +27,7 @@ #define NOTIFICATION_H // period of each blink phase -#define LED_BLINK_PERIOD_MS 200 -// Define the pause in half blink periods to be added between phases -#define LED_PAUSE_BETWEEN_PHASES 3 +#define LED_BLINK_PERIOD_MS 200 // update the status snapshot used by notifcations void NotificationUpdateStatus(); diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index 1b5a9c752..4df6f71bb 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -46,38 +46,82 @@ #define HEARTBEAT_LED_OFF() #endif -#define ALARM_BLINK_COUNT(x) \ +#define BLINK_R_ALARM_PATTERN(x) \ (x == SYSTEMALARMS_ALARM_OK ? 0 : \ - x == SYSTEMALARMS_ALARM_WARNING ? 1 : \ - x == SYSTEMALARMS_ALARM_ERROR ? 2 : \ + x == SYSTEMALARMS_ALARM_WARNING ? 0b0000000001000000 : \ + 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) -#define BLINK_COUNT(x) \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 1 : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 2 : \ - x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 4 : \ - x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 4 : \ - x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 3 : \ - x == FLIGHTSTATUS_FLIGHTMODE_POI ? 3 : 1) +#define BLINK_B_FM_ARMED_PATTERN(x) \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0000000000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0000000000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0000000000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000010000100001 : 0b0000000000000001) -#define BLINK_RED(x) \ - (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? false : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? false : \ - x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? false : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_LAND ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? true : \ - x == FLIGHTSTATUS_FLIGHTMODE_POI ? true : false) +#define BLINK_R_FM_ARMED_PATTERN(x) \ + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0000000000100000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0000000000100000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0000000000100000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000010000000000 : \ + 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 static volatile SystemAlarmsAlarmOptions currentAlarmLevel = SYSTEMALARMS_ALARM_OK; @@ -86,10 +130,10 @@ static volatile bool started = false; static volatile pios_notify_notification nextNotification = NOTIFY_NONE; #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 -static bool handleNotifications(uint8_t cycleCount, pios_notify_notification runningNotification); -static void handleStatus(uint8_t cycleCount, uint8_t blinkCount); +static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern); +static void handleStatus(uint16_t *r_pattern, uint16_t *b_pattern); void NotificationUpdateStatus() { @@ -105,8 +149,9 @@ void NotificationUpdateStatus() void NotificationOnboardLedsRun() { static portTickType lastRunTimestamp; - static uint8_t blinkCount; // number of blinks since phase start - static uint8_t cycleCount; // cound the number of cycles (half of a blink) + static uint16_t r_pattern; + static uint16_t b_pattern; + static uint8_t cycleCount; // count the number of cycles static pios_notify_notification runningNotification = NOTIFY_NONE; static enum { @@ -116,7 +161,7 @@ void NotificationOnboardLedsRun() STATUS_LENGHT } 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; } @@ -140,29 +185,19 @@ void NotificationOnboardLedsRun() } // check if a phase has just finished - if (cycleCount & 0x08) { - // add a pause between each phase - if (cycleCount > 0x8 + LED_PAUSE_BETWEEN_PHASES) { - // ready to start a new phase - cycleCount = 0x0; - - // Notification has been just shown, cleanup - if (status == STATUS_NOTIFY) { - runningNotification = NOTIFY_NONE; - } - status = (status + 1) % STATUS_LENGHT; - } else { - HEARTBEAT_LED_OFF(); - ALARM_LED_OFF(); - return; + if (cycleCount & 0x10) { + HEARTBEAT_LED_OFF(); + ALARM_LED_OFF(); + cycleCount = 0x0; + // Notification has been just shown, cleanup + if (status == STATUS_NOTIFY) { + runningNotification = NOTIFY_NONE; } + status = (status + 1) % STATUS_LENGHT; } - // a blink last 2 cycles(on and off cycle). - blinkCount = (cycleCount & 0xF) >> 1; - if (status == STATUS_NOTIFY) { - if (!handleNotifications(cycleCount, runningNotification)) { + if (!cycleCount && !handleNotifications(runningNotification, &r_pattern, &b_pattern)) { // no notifications, advance status++; } @@ -171,7 +206,7 @@ void NotificationOnboardLedsRun() // Handles Alarm display if (status == STATUS_ALARM) { #ifdef PIOS_LED_ALARM - if (!handleAlarms(cycleCount, blinkCount)) { + if (!cycleCount && !handleAlarms(&r_pattern, &b_pattern)) { // no alarms, advance status++; } @@ -182,80 +217,58 @@ void NotificationOnboardLedsRun() } // **** Handles flightmode display - if (status == STATUS_FLIGHTMODE) { - handleStatus(cycleCount, blinkCount); + if (status == STATUS_FLIGHTMODE && !cycleCount) { + 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) -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) { return false; } - - if (currentAlarmLevel == SYSTEMALARMS_ALARM_CRITICAL) { - // 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(); - } - } + *b_pattern = BLINK_B_ALARM_PATTERN(currentAlarmLevel); + *r_pattern = BLINK_R_ALARM_PATTERN(currentAlarmLevel); return true; } #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) { return false; } - if (cycleCount & 0x1) { - ALARM_LED_OFF(); - HEARTBEAT_LED_ON(); - } else { - ALARM_LED_ON(); - HEARTBEAT_LED_OFF(); - } + *b_pattern = BLINK_B_NOTIFY_PATTERN(runningNotification); + *r_pattern = BLINK_R_NOTIFY_PATTERN(runningNotification); 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 -#if defined(PIOS_LED_HEARTBEAT) uint8_t flightmode = currentFlightStatus.FlightMode; + if (currentFlightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { - // Slow blink - if (blinkCount < 3) { - HEARTBEAT_LED_ON(); - } else { - HEARTBEAT_LED_OFF(); - } + *b_pattern = BLINK_B_FM_DISARMED_PATTERN(flightmode); + *r_pattern = BLINK_R_FM_DISARMED_PATTERN(flightmode); } else { - if ((blinkCount < BLINK_COUNT(flightmode)) && - (cycleCount & 0x1)) { - // 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(); - } + *b_pattern = BLINK_B_FM_ARMED_PATTERN(flightmode); + *r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode); } -#endif /* PIOS_LED_HEARTBEAT */ } From d42debba50b535dfabfd2dee98ba5735f40b1545 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Sat, 26 Apr 2014 20:48:30 +0200 Subject: [PATCH 09/10] OP-1302 Remove the special case for Telemetry in Alarm.c and shut up the telemetry warning in telemetry.c when disconnected --- flight/libraries/alarms.c | 5 ++--- flight/modules/Telemetry/telemetry.c | 5 ++--- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/flight/libraries/alarms.c b/flight/libraries/alarms.c index 7f2688a73..5dc3cda2e 100644 --- a/flight/libraries/alarms.c +++ b/flight/libraries/alarms.c @@ -244,7 +244,7 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity) // Go through alarms and check if any are of the given severity or higher for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) { - if (n != SYSTEMALARMS_ALARM_TELEMETRY && cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) { + if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) { xSemaphoreGiveRecursive(lock); return 1; } @@ -272,8 +272,7 @@ SystemAlarmsAlarmOptions AlarmsGetHighestSeverity() // Go through alarms and find the highest severity uint32_t n = 0; while (n < SYSTEMALARMS_ALARM_NUMELEM && highest != SYSTEMALARMS_ALARM_CRITICAL) { - if (n != SYSTEMALARMS_ALARM_TELEMETRY && - cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) { + if (cast_struct_to_array(alarmsData, alarmsData.Actuator)[n] > highest) { highest = cast_struct_to_array(alarmsData, alarmsData.Actuator)[n]; } n++; diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 55c201369..d91ef131e 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -650,11 +650,10 @@ static void updateTelemetryStats() flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; } - // Update the telemetry alarm + // TODO: check whether is there any error condition worth raising an alarm + // Disconnection is actually a normal (non)working status so it is not raising alarms anymore. if (flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { AlarmsClear(SYSTEMALARMS_ALARM_TELEMETRY); - } else { - AlarmsSet(SYSTEMALARMS_ALARM_TELEMETRY, SYSTEMALARMS_ALARM_ERROR); } // Update object From 94beebc3d037056d52f05d4dcfbfb0b4e6d500ed Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 8 May 2014 22:26:12 +0200 Subject: [PATCH 10/10] OP-1302 Fixes for current next. Show flightmode everytime it changes when disarmed --- flight/libraries/notification.c | 90 ++++++++++++++++++++------------- 1 file changed, 56 insertions(+), 34 deletions(-) diff --git a/flight/libraries/notification.c b/flight/libraries/notification.c index 4df6f71bb..9015e4dd3 100644 --- a/flight/libraries/notification.c +++ b/flight/libraries/notification.c @@ -62,9 +62,9 @@ (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000001 : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000100001 : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000010000100001 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0000000000100001 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0000000000100001 : \ - x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0000000000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000100001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000010000100001 : \ x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000100001 : \ x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100010001 : \ x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100010001 : \ @@ -75,9 +75,9 @@ (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \ x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD ? 0b0000000000100000 : \ - x == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO ? 0b0000000000100000 : \ - x == FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL ? 0b0000000000100000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000001 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000001 : \ x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000010000000000 : \ x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0001000100000000 : \ x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0001000100000000 : \ @@ -85,31 +85,33 @@ 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) + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000001100011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000110001100011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000001100011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000110001100011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110001100011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100110011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100110011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110001100011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110001100011 : 0b0000000000000011) #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) + (x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED1 ? 0b0000000000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED2 ? 0b0000000000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED3 ? 0b0000000000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED4 ? 0b0000000000000011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED5 ? 0b0000000000000011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_STABILIZED6 ? 0b0000000000000011 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD ? 0b0000110000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE ? 0b0011001100000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_LAND ? 0b0011001100000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER ? 0b0000110000000000 : \ + x == FLIGHTSTATUS_FLIGHTMODE_POI ? 0b0000110000000000 : 0b0000110000000000) +#define BLINK_B_HEARTBEAT_PATTERN 0b0001111111111111 +#define BLINK_R_HEARTBEAT_PATTERN 0 #define BLINK_B_NOTIFY_PATTERN(x) \ (x == NOTIFY_NONE ? 0 : \ @@ -133,8 +135,8 @@ static volatile pios_notify_notification nextNotification = NOTIFY_NONE; static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern); #endif // PIOS_LED_ALARM static bool handleNotifications(pios_notify_notification runningNotification, uint16_t *r_pattern, uint16_t *b_pattern); -static void handleStatus(uint16_t *r_pattern, uint16_t *b_pattern); - +static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern); +static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern); void NotificationUpdateStatus() { started = true; @@ -152,12 +154,13 @@ void NotificationOnboardLedsRun() static uint16_t r_pattern; static uint16_t b_pattern; static uint8_t cycleCount; // count the number of cycles + static uint8_t lastFlightMode = -1; + static bool forceShowFlightMode = false; static pios_notify_notification runningNotification = NOTIFY_NONE; - static enum { STATUS_NOTIFY, STATUS_ALARM, - STATUS_FLIGHTMODE, + STATUS_FLIGHTMODE, // flightMode/HeartBeat STATUS_LENGHT } status; @@ -182,6 +185,13 @@ void NotificationOnboardLedsRun() // Force a notification status status = STATUS_NOTIFY; cycleCount = 0; // instantly start a notify cycle + } else { + if (lastFlightMode != currentFlightStatus.FlightMode) { + status = STATUS_FLIGHTMODE; + lastFlightMode = currentFlightStatus.FlightMode; + cycleCount = 0; // instantly start a flightMode cycle + forceShowFlightMode = true; + } } // check if a phase has just finished @@ -189,6 +199,7 @@ void NotificationOnboardLedsRun() HEARTBEAT_LED_OFF(); ALARM_LED_OFF(); cycleCount = 0x0; + forceShowFlightMode = false; // Notification has been just shown, cleanup if (status == STATUS_NOTIFY) { runningNotification = NOTIFY_NONE; @@ -218,7 +229,11 @@ void NotificationOnboardLedsRun() // **** Handles flightmode display if (status == STATUS_FLIGHTMODE && !cycleCount) { - handleStatus(&r_pattern, &b_pattern); + if (forceShowFlightMode || currentFlightStatus.Armed != FLIGHTSTATUS_ARMED_DISARMED) { + handleFlightMode(&r_pattern, &b_pattern); + } else { + handleHeartbeat(&r_pattern, &b_pattern); + } } // led output @@ -259,7 +274,7 @@ static bool handleNotifications(pios_notify_notification runningNotification, ui return true; } -static void handleStatus(uint16_t *r_pattern, uint16_t *b_pattern) +static void handleFlightMode(uint16_t *r_pattern, uint16_t *b_pattern) { // Flash the heartbeat LED uint8_t flightmode = currentFlightStatus.FlightMode; @@ -272,3 +287,10 @@ static void handleStatus(uint16_t *r_pattern, uint16_t *b_pattern) *r_pattern = BLINK_R_FM_ARMED_PATTERN(flightmode); } } + +static void handleHeartbeat(uint16_t *r_pattern, uint16_t *b_pattern) +{ + // Flash the heartbeat LED + *b_pattern = BLINK_B_HEARTBEAT_PATTERN; + *r_pattern = BLINK_R_HEARTBEAT_PATTERN; +}