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

Merge remote-tracking branch 'origin/amorale/OP-1379_ext_led_notification_module_new' into next

Browse Source 
Conflicts:
	Makefile
	flight/targets/boards/discoveryf4bare/firmware/Makefile
	flight/targets/boards/revolution/firmware/Makefile
This commit is contained in:
Alessio Morale 2014-09-29 21:26:31 +02:00
commit 9e0596c2c5
29 changed files with 1315 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -638,7 +638,7 @@ uavo-collections_clean:
#
##############################
ALL_UNITTESTS := logfs math
ALL_UNITTESTS := logfs math lednotification
# Build the directory for the unit tests
UT_OUT_DIR := $(BUILD_DIR)/unit_tests

33
flight/libraries/inc/lednotification.h Normal file
View File

@ -0,0 +1,33 @@
/**
******************************************************************************
*
* @file lednotification.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief led 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 LEDNOTIFICATION_H_
#define LEDNOTIFICATION_H_
#include <optypes.h>
void LedNotificationExtLedsRun();
#endif /* LEDNOTIFICATION_H_ */

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

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

64
flight/libraries/inc/optypes.h Normal file
View File

@ -0,0 +1,64 @@
/**
******************************************************************************
*
* @file optypes.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief OP Generic data type 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 UTIL_H
#define UTIL_H
#include <stdint.h>
typedef struct {
uint8_t R;
uint8_t G;
uint8_t B;
} Color_t;
extern const Color_t Color_Off;
extern const Color_t Color_Black;
extern const Color_t Color_Red;
extern const Color_t Color_Lime;
extern const Color_t Color_Blue;
extern const Color_t Color_Yellow;
extern const Color_t Color_Cian;
extern const Color_t Color_Magenta;
extern const Color_t Color_Navy;
extern const Color_t Color_Green;
extern const Color_t Color_Purple;
extern const Color_t Color_Teal;
extern const Color_t Color_Orange;
extern const Color_t Color_White;
#define COLOR_BLACK { .R = 0x00, .G = 0x00, .B = 0x00 }
#define COLOR_OFF COLOR_BLACK
#define COLOR_RED { .R = 0xFF, .G = 0x00, .B = 0x00 }
#define COLOR_LIME { .R = 0x00, .G = 0xFF, .B = 0x00 }
#define COLOR_BLUE { .R = 0x00, .G = 0x00, .B = 0xFF }
#define COLOR_YELLOW { .R = 0xFF, .G = 0xFF, .B = 0x00 }
#define COLOR_CIAN { .R = 0x00, .G = 0xFF, .B = 0xFF }
#define COLOR_MAGENTA { .R = 0xFF, .G = 0x00, .B = 0xFF }
#define COLOR_NAVY { .R = 0x00, .G = 0x00, .B = 0x80 }
#define COLOR_GREEN { .R = 0x00, .G = 0x80, .B = 0x00 }
#define COLOR_PURPLE { .R = 0x80, .G = 0x00, .B = 0x80 }
#define COLOR_TEAL { .R = 0x00, .G = 0x80, .B = 0x80 }
#define COLOR_ORANGE { .R = 0xFF, .G = 0xA5, .B = 0x00 }
#define COLOR_WHITE { .R = 0xAA, .G = 0xAA, .B = 0xAA }
#endif /* UTIL_H */

259
flight/libraries/lednotification.c Normal file
View File

@ -0,0 +1,259 @@
/**
******************************************************************************
*
* @file lednotification.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief led 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
*/
#include "inc/lednotification.h"
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <FreeRTOS.h>
#include <pios.h>
#include <pios_notify.h>
#include <pios_ws2811.h>
// Private defines
// Maximum number of notifications enqueued when a higher priority notification is added
#define MAX_BACKGROUND_NOTIFICATIONS 6
#define MAX_HANDLED_LED 1
#define BACKGROUND_SEQUENCE 0
#define RESET_STEP -1
#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS)
// Private data types definition
// this is the status for a single notification led set
typedef struct {
int8_t queued_priorities[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
LedSequence_t queued_sequences[MAX_BACKGROUND_NOTIFICATIONS]; // slot 0 is reserved for background
uint32_t next_run_time;
uint32_t sequence_starting_time;
int8_t active_sequence_num; // active queued sequence or BACKGROUND_SEQUENCE
bool running; // is this led running?
bool step_phase_on; // true = step on phase, false = step 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_sequence_rep; // next sequence repetition counter (valid if sequence.repeats > 1)
uint8_t led_set_start; // first target led for this set
uint8_t led_set_end; // last target led for this set
} NotifierLedStatus_t;
static bool led_status_initialized = false;
NotifierLedStatus_t led_status[MAX_HANDLED_LED];
static void InitExtLed()
{
memset(led_status, 0, sizeof(NotifierLedStatus_t) * MAX_HANDLED_LED);
const uint32_t now = GET_CURRENT_MILLIS;
for (uint8_t l = 0; l < MAX_HANDLED_LED; l++) {
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
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
led_status[l].queued_priorities[i] = NOTIFY_PRIORITY_BACKGROUND;
}
}
}
/**
* restart current sequence
*/
static void restart_sequence(NotifierLedStatus_t *status, bool immediate)
{
status->next_sequence_step = 0;
status->next_step_rep = 0;
status->step_phase_on = true;
status->running = true;
if (immediate) {
uint32_t currentTime = GET_CURRENT_MILLIS;
status->next_run_time = currentTime;
}
status->sequence_starting_time = status->next_run_time;
}
/**
* modify background sequence or enqueue a new sequence to play
*/
static void push_queued_sequence(ExtLedNotification_t *new_notification, NotifierLedStatus_t *status)
{
int8_t updated_sequence;
int8_t lowest_priority_index = -1;
int8_t lowest_priority = new_notification->priority;
if (new_notification->priority == NOTIFY_PRIORITY_BACKGROUND) {
lowest_priority_index = BACKGROUND_SEQUENCE;
} else {
// slot 0 is reserved for Background sequence
for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
if (status->queued_priorities[i] < lowest_priority) {
lowest_priority_index = i;
lowest_priority = status->queued_priorities[i];
}
}
}
// no items with priority lower than the one we are trying to enqueue. skip
if (lowest_priority_index < 0) {
return;
}
status->queued_priorities[lowest_priority_index] = new_notification->priority;
status->queued_sequences[lowest_priority_index] = new_notification->sequence;
updated_sequence = lowest_priority_index;;
// check whether we should preempt the current notification and play this new one
if (status->queued_priorities[status->active_sequence_num] < new_notification->priority) {
status->active_sequence_num = updated_sequence;
}
if (status->active_sequence_num == updated_sequence) {
restart_sequence(status, true);
}
}
static bool pop_queued_sequence(NotifierLedStatus_t *status)
{
if (status->active_sequence_num > BACKGROUND_SEQUENCE) {
// set the last active slot as empty
status->queued_priorities[status->active_sequence_num] = NOTIFY_PRIORITY_BACKGROUND;
// search the highest priority item
int8_t highest_priority_index = BACKGROUND_SEQUENCE;
int8_t highest_priority = NOTIFY_PRIORITY_BACKGROUND;
for (int8_t i = 1; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
if (status->queued_priorities[i] > highest_priority) {
highest_priority_index = i;
highest_priority = status->queued_priorities[i];
}
}
// set the next sequence to activate (or BACKGROUND_SEQUENCE when all slots are empty)
status->active_sequence_num = highest_priority_index;
return highest_priority_index != BACKGROUND_SEQUENCE;
}
// background sequence was completed
return false;
}
/**
* advance current sequence pointers for next step
*/
static void advance_sequence(NotifierLedStatus_t *status)
{
LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
uint8_t step = status->next_sequence_step;
LedStep_t *currentStep = &activeSequence->steps[step];
// Next step will be the OFF phase, so just update the time and
if (status->step_phase_on) {
// next will be the off phase
status->next_run_time += currentStep->time_on;
status->step_phase_on = false;
// check if off phase should be skipped
if (currentStep->time_off != 0) {
return;
}
}
// next step is ON phase. check whether to repeat current step or move to next one
status->next_run_time += currentStep->time_off;
status->step_phase_on = true;
if (status->next_step_rep + 1 < currentStep->repeats) {
// setup next repetition
status->next_step_rep++;
return;
}
// move to next step
LedStep_t *nextStep = (step + 1 < NOTIFY_SEQUENCE_MAX_STEPS) ? &activeSequence->steps[step + 1] : 0;
// next step is null, check whether sequence must be repeated or it must move to lower priority queued or background sequences
if (NOTIFY_IS_NULL_STEP(nextStep)) {
if (activeSequence->repeats == -1 || status->next_sequence_rep + 1 < activeSequence->repeats) {
status->next_sequence_rep++;
// restart the sequence
restart_sequence(status, false);
return;
}
if (status->active_sequence_num != BACKGROUND_SEQUENCE) {
// no repeat, pop enqueued or background sequences
pop_queued_sequence(status);
restart_sequence(status, false);
status->next_sequence_rep = 0;
} else {
status->running = false;
}
} else {
status->next_step_rep = 0;
status->next_sequence_step++;
}
}
/**
* run a led set
*/
static void run_led(NotifierLedStatus_t *status)
{
const uint32_t currentTime = GET_CURRENT_MILLIS;
if (!status->running || currentTime < status->next_run_time) {
return;
}
status->next_run_time = currentTime;
uint8_t step = status->next_sequence_step;
LedSequence_t *activeSequence = &status->queued_sequences[status->active_sequence_num];
const Color_t color = status->step_phase_on ? activeSequence->steps[step].color : Color_Off;
for (uint8_t i = status->led_set_start; i <= status->led_set_end; i++) {
PIOS_WS2811_setColorRGB(color, i, false);
}
PIOS_WS2811_Update();
advance_sequence(status);
}
void LedNotificationExtLedsRun()
{
// handle incoming sequences
if (!led_status_initialized) {
InitExtLed();
led_status_initialized = true;
}
static ExtLedNotification_t *newNotification;
newNotification = PIOS_NOTIFY_GetNewExtLedSequence(true);
if (newNotification) {
push_queued_sequence(newNotification, &led_status[0]);
}
// Run Leds
for (uint8_t i = 0; i < MAX_HANDLED_LED; i++) {
run_led(&led_status[i]);
}
}

15
flight/libraries/notification.c
View File

@ -28,6 +28,11 @@
#include <systemalarms.h>
#include <flightstatus.h>
#include <pios_notify.h>
#include <stdbool.h>
#define GET_CURRENT_MILLIS (xTaskGetTickCount() * portTICK_RATE_MS)
// Private data types definition
#ifdef PIOS_LED_ALARM
#define ALARM_LED_ON() PIOS_LED_On(PIOS_LED_ALARM)
@ -136,6 +141,8 @@ static bool handleAlarms(uint16_t *r_pattern, uint16_t *b_pattern);
static bool handleNotifications(pios_notify_notification runningNotification, 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;
@ -149,7 +156,7 @@ void NotificationUpdateStatus()
void NotificationOnboardLedsRun()
{
static portTickType lastRunTimestamp;
static uint32_t lastRunTimestamp;
static uint16_t r_pattern;
static uint16_t b_pattern;
static uint8_t cycleCount; // count the number of cycles
@ -163,11 +170,13 @@ void NotificationOnboardLedsRun()
STATUS_LENGHT
} status;
if (!started || (xTaskGetTickCount() - lastRunTimestamp) < (LED_BLINK_PERIOD_MS * portTICK_RATE_MS / 4)) {
const uint32_t current_timestamp = GET_CURRENT_MILLIS;
if (!started || (current_timestamp - lastRunTimestamp) < LED_BLINK_PERIOD_MS) {
return;
}
lastRunTimestamp = xTaskGetTickCount();
lastRunTimestamp = current_timestamp;
// the led will show various status information, subdivided in three phases
// - Notification
// - Alarm

42
flight/libraries/optypes.c Normal file
View File

@ -0,0 +1,42 @@
/**
******************************************************************************
*
* @file optypes.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief OP Generic data type 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
*/
#include <optypes.h>
const Color_t Color_Off = COLOR_OFF;
const Color_t Color_Black = COLOR_BLACK;
const Color_t Color_Red = COLOR_RED;
const Color_t Color_Lime = COLOR_LIME;
const Color_t Color_Blue = COLOR_BLUE;
const Color_t Color_Yellow = COLOR_YELLOW;
const Color_t Color_Cian = COLOR_CIAN;
const Color_t Color_Magenta = COLOR_MAGENTA;
const Color_t Color_Navy = COLOR_NAVY;
const Color_t Color_Green = COLOR_GREEN;
const Color_t Color_Purple = COLOR_PURPLE;
const Color_t Color_Teal = COLOR_TEAL;
const Color_t Color_Orange = COLOR_ORANGE;
const Color_t Color_White = COLOR_WHITE;

33
flight/modules/Notify/inc/notify.h Normal file
View File

@ -0,0 +1,33 @@
/**
******************************************************************************
*
* @file notify.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Notify module, show events and status on external led.
*
* @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 NOTIFY_H_
#define NOTIFY_H_
int32_t NotifyInitialize(void);
#endif /* NOTIFY_H_ */

222
flight/modules/Notify/inc/sequences.h Normal file
View File

@ -0,0 +1,222 @@
/**
******************************************************************************
*
* @file sequences.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Notify module, sequences configuration.
*
* @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 SEQUENCES_H_
#define SEQUENCES_H_
#include <optypes.h>
#include <pios_notify.h>
#include <flightstatus.h>
#include <systemalarms.h>
#include <pios_helpers.h>
// This represent the list of basic light sequences, defined later
typedef enum {
NOTIFY_SEQUENCE_ARMED_FM_MANUAL = 0,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1 = 1,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2 = 2,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3 = 3,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4 = 4,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5 = 5,
NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6 = 6,
NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE = 7,
NOTIFY_SEQUENCE_ARMED_FM_GPS = 8,
NOTIFY_SEQUENCE_ARMED_FM_RTH = 9,
NOTIFY_SEQUENCE_ARMED_FM_LAND = 10,
NOTIFY_SEQUENCE_ARMED_FM_AUTO = 11,
NOTIFY_SEQUENCE_ALM_WARN_GPS = 12,
NOTIFY_SEQUENCE_ALM_ERROR_GPS = 13,
NOTIFY_SEQUENCE_ALM_WARN_BATTERY = 14,
NOTIFY_SEQUENCE_ALM_ERROR_BATTERY = 15,
NOTIFY_SEQUENCE_ALM_MAG = 16,
NOTIFY_SEQUENCE_ALM_CONFIG = 17,
NOTIFY_SEQUENCE_ALM_RECEIVER = 18,
NOTIFY_SEQUENCE_DISARMED = 19,
NOTIFY_SEQUENCE_NULL = 255, // skips any signalling for this condition
} NotifySequences;
// This structure determine sequences attached to an alarm
typedef struct {
uint32_t timeBetweenNotifications; // time in milliseconds to wait between each notification iteration
uint8_t alarmIndex; // Index of the alarm, use one of the SYSTEMALARMS_ALARM_XXXXX defines
uint8_t warnNotification; // index of the sequence to be used when alarm is in warning status(pick one from NotifySequences enum)
uint8_t errorNotification; // index of the sequence to be used when alarm is in error status(pick one from NotifySequences enum)
} AlarmDefinition_t;
// 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[] = {
[NOTIFY_SEQUENCE_DISARMED] = { .repeats = -1, .steps = {
{ .time_off = 500, .time_on = 500, .color = COLOR_TEAL, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_MANUAL] = { .repeats = -1, .steps = {
{ .time_off = 900, .time_on = 100, .color = COLOR_YELLOW, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1] = { .repeats = -1, .steps = {
{ .time_off = 900, .time_on = 100, .color = COLOR_BLUE, .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] = { .repeats = -1, .steps = {
{ .time_off = 800, .time_on = 200, .color = COLOR_GREEN, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_RTH] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .color = COLOR_GREEN, .repeats = 1, },
{ .time_off = 100, .time_on = 100, .color = COLOR_YELLOW, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ARMED_FM_LAND] = { .repeats = -1, .steps = {
{ .time_off = 100, .time_on = 100, .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_GREEN, .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_GREEN, .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_PURPLE, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_CONFIG] = { .repeats = 1, .steps = {
{ .time_off = 50, .time_on = 50, .color = COLOR_RED, .repeats = 9, },
{ .time_off = 500, .time_on = 50, .color = COLOR_RED, .repeats = 1, },
}, },
[NOTIFY_SEQUENCE_ALM_RECEIVER] = { .repeats = 1, .steps = {
{ .time_off = 50, .time_on = 50, .color = COLOR_ORANGE, .repeats = 9, },
{ .time_off = 500, .time_on = 50, .color = COLOR_ORANGE, .repeats = 1, },
}, },
};
// List of background sequences attached to each flight mode
const LedSequence_t *flightModeMap[] = {
[FLIGHTSTATUS_FLIGHTMODE_MANUAL] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED1] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED2] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED3] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED4] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED5] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5],
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED6] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6],
[FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_AUTO],
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
[FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_RTH],
[FLIGHTSTATUS_FLIGHTMODE_LAND] = &notifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
[FLIGHTSTATUS_FLIGHTMODE_POI] = &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[] = {
{
.timeBetweenNotifications = 10000,
.alarmIndex = SYSTEMALARMS_ALARM_GPS,
.warnNotification = NOTIFY_SEQUENCE_ALM_WARN_GPS,
.errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_GPS,
},
{
.timeBetweenNotifications = 15000,
.alarmIndex = SYSTEMALARMS_ALARM_MAGNETOMETER,
.warnNotification = NOTIFY_SEQUENCE_NULL,
.errorNotification = NOTIFY_SEQUENCE_ALM_MAG,
},
{
.timeBetweenNotifications = 15000,
.alarmIndex = SYSTEMALARMS_ALARM_BATTERY,
.warnNotification = NOTIFY_SEQUENCE_ALM_WARN_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);
#endif /* SEQUENCES_H_ */

136
flight/modules/Notify/notify.c Normal file
View File

@ -0,0 +1,136 @@
/**
******************************************************************************
*
* @file notify.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Notify module, show events and status on external led.
*
* @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 "openpilot.h"
#include <flightstatus.h>
#include <systemalarms.h>
#include <flightbatterystate.h>
#include <lednotification.h>
#include <optypes.h>
#include <pios_notify.h>
#include <FreeRTOS.h>
#include <task.h>
#include <eventdispatcher.h>
#include "inc/notify.h"
#include "inc/sequences.h"
#include <pios_mem.h>
#include <hwsettings.h>
#define SAMPLE_PERIOD_MS 250
// private types
typedef struct {
uint32_t lastAlarmTime;
uint8_t lastAlarm;
} AlarmStatus_t;
// function declarations
static void updatedCb(UAVObjEvent *ev);
static void onTimerCb(UAVObjEvent *ev);
static void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time,
uint8_t warn_sequence, uint8_t error_sequence,
uint32_t timeBetweenNotifications);
static AlarmStatus_t *alarmStatus;
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);
for (uint8_t i = 0; i < alarmsMapSize; i++) {
alarmStatus[i].lastAlarm = SYSTEMALARMS_ALARM_OK;
alarmStatus[i].lastAlarmTime = 0;
}
FlightStatusConnectCallback(&updatedCb);
static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
updatedCb(0);
}
return 0;
}
MODULE_INITCALL(NotifyInitialize, 0);
void updatedCb(UAVObjEvent *ev)
{
if (!ev || ev->obj == FlightStatusHandle()) {
static uint8_t last_armed = 0xff;
static uint8_t last_flightmode = 0xff;
uint8_t armed;
uint8_t flightmode;
FlightStatusArmedGet(&armed);
FlightStatusFlightModeGet(&flightmode);
if (last_armed != armed || (armed && flightmode != last_flightmode)) {
if (armed) {
PIOS_NOTIFICATION_Default_Ext_Led_Play(flightModeMap[flightmode], NOTIFY_PRIORITY_BACKGROUND);
} else {
PIOS_NOTIFICATION_Default_Ext_Led_Play(&notifications[NOTIFY_SEQUENCE_DISARMED], NOTIFY_PRIORITY_BACKGROUND);
}
}
last_armed = armed;
last_flightmode = flightmode;
}
}
void onTimerCb(__attribute__((unused)) UAVObjEvent *ev)
{
static SystemAlarmsAlarmData alarms;
SystemAlarmsAlarmGet(&alarms);
for (uint8_t i = 0; i < alarmsMapSize; i++) {
uint8_t alarm = ((uint8_t *)&alarms)[alarmsMap[i].alarmIndex];
checkAlarm(alarm,
&alarmStatus[i].lastAlarm,
&alarmStatus[i].lastAlarmTime,
alarmsMap[i].warnNotification,
alarmsMap[i].errorNotification,
alarmsMap[i].timeBetweenNotifications);
}
}
void checkAlarm(uint8_t alarm, uint8_t *last_alarm, uint32_t *last_alm_time, uint8_t warn_sequence, uint8_t error_sequence, uint32_t timeBetweenNotifications)
{
if (alarm > SYSTEMALARMS_ALARM_OK) {
uint32_t current_time = PIOS_DELAY_GetuS();
if (*last_alarm < alarm || *last_alm_time + timeBetweenNotifications * 1000 < current_time) {
uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence : error_sequence;
if (sequence != NOTIFY_SEQUENCE_NULL) {
PIOS_NOTIFICATION_Default_Ext_Led_Play(
&notifications[sequence],
alarm == SYSTEMALARMS_ALARM_WARNING ? NOTIFY_PRIORITY_REGULAR : NOTIFY_PRIORITY_CRITICAL);
}
*last_alarm = alarm;
*last_alm_time = current_time;
}
} else {
*last_alarm = SYSTEMALARMS_ALARM_OK;
}
}

10
flight/modules/System/systemmod.c
View File

@ -41,7 +41,11 @@
#include <openpilot.h>
// private includes
#include "inc/systemmod.h"
#include "notification.h"
#include <notification.h>
#ifdef PIOS_INCLUDE_WS2811
#include <lednotification.h>
#endif
// UAVOs
#include <objectpersistence.h>
@ -54,6 +58,7 @@
#include <callbackinfo.h>
#include <hwsettings.h>
#include <pios_flashfs.h>
#include <pios_notify.h>
#ifdef PIOS_INCLUDE_INSTRUMENTATION
#include <instrumentation.h>
@ -632,6 +637,9 @@ static void updateSystemAlarms()
void vApplicationIdleHook(void)
{
NotificationOnboardLedsRun();
#ifdef PIOS_INCLUDE_WS2811
LedNotificationExtLedsRun();
#endif
}
/**
* Called by the RTOS when a stack overflow is detected.

36
flight/pios/common/pios_notify.c
View File

@ -28,7 +28,8 @@
static volatile pios_notify_notification currentNotification = NOTIFY_NONE;
static volatile pios_notify_priority currentPriority;
static volatile ExtLedNotification_t extLedNotification;
static volatile bool newNotification;
void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_notify_priority priority)
{
@ -47,3 +48,36 @@ pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear)
}
return ret;
}
/*
* Play a sequence on the default external led. Sequences with priority higher than NOTIFY_PRIORITY_LOW
* are repeated only once if repeat = -1
* @param sequence Sequence to be played
* @param priority Priority of the sequence being played
*/
void PIOS_NOTIFICATION_Default_Ext_Led_Play(const LedSequence_t *sequence, pios_notify_priority priority)
{
// alert and alarms are repeated if condition persists. bacground notification instead are set once, so try to prevent loosing any update
if (newNotification && priority != NOTIFY_PRIORITY_BACKGROUND) {
// prevent overwriting higher priority or background notifications
if (extLedNotification.priority == NOTIFY_PRIORITY_BACKGROUND || extLedNotification.priority > priority) {
return;
}
}
extLedNotification.priority = priority;
extLedNotification.sequence = *sequence;
newNotification = true;
}
ExtLedNotification_t *PIOS_NOTIFY_GetNewExtLedSequence(bool clear)
{
if (!newNotification) {
return 0;
}
if (clear) {
newNotification = false;
}
return (ExtLedNotification_t *)&extLedNotification;
}

50
flight/pios/inc/pios_notify.h
View File

@ -26,7 +26,7 @@
#ifndef PIOS_NOTIFY_H_
#define PIOS_NOTIFY_H_
#include <stdbool.h>
#include <optypes.h>
typedef enum {
NOTIFY_NONE = 0,
NOTIFY_OK,
@ -35,11 +35,35 @@ typedef enum {
} pios_notify_notification;
typedef enum {
NOTIFY_PRIORITY_CRITICAL = 2,
NOTIFY_PRIORITY_REGULAR = 1,
NOTIFY_PRIORITY_CRITICAL = 2,
NOTIFY_PRIORITY_REGULAR = 1,
NOTIFY_PRIORITY_LOW = 0,
NOTIFY_PRIORITY_BACKGROUND = -1,
} pios_notify_priority;
// A single led step, Color, time On/Off, repeat count
#define NOTIFY_SEQUENCE_MAX_STEPS 5
typedef struct {
uint16_t time_off;
uint16_t time_on;
Color_t color;
uint8_t repeats;
} LedStep_t;
typedef struct {
int8_t repeats; // -1 for infinite repetitions
LedStep_t steps[NOTIFY_SEQUENCE_MAX_STEPS];
} LedSequence_t;
typedef struct {
LedSequence_t sequence;
pios_notify_priority priority;
} ExtLedNotification_t;
#define NOTIFY_IS_NULL_STEP(x) (!x || (!x->time_off && !x->time_on && !x->repeats))
/**
* 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
@ -55,4 +79,24 @@ void PIOS_NOTIFY_StartNotification(pios_notify_notification notification, pios_n
*/
pios_notify_notification PIOS_NOTIFY_GetActiveNotification(bool clear);
/*
* Play a sequence on the default external led. Sequences with priority higher than NOTIFY_PRIORITY_LOW
* are repeated only once if repeat = -1
* @param sequence Sequence to be played
* @param priority Priority of the sequence being played
*/
void PIOS_NOTIFICATION_Default_Ext_Led_Play(const LedSequence_t *sequence, pios_notify_priority priority);
/*
* Play a sequence on an external rgb led. Sequences with priority higher than NOTIFY_PRIORITY_LOW
* are repeated only once if repeat = -1
* @param sequence Sequence to be played
* @param ledNumber Led number
* @param priority Priority of the sequence being played
*
*/
// void PIOS_NOTIFICATION_Ext_Led_Play(const LedSequence_t sequence, uint8_t ledNumber, pios_notify_priority priority);
ExtLedNotification_t *PIOS_NOTIFY_GetNewExtLedSequence(bool clear);
#endif /* PIOS_NOTIFY_H_ */

114
flight/pios/inc/pios_ws2811.h
View File

@ -29,119 +29,11 @@
#define PIOS_WS2811_H_
#include <stdint.h>
#include <pios_gpio.h>
#include <pios_stm32.h>
#include <pios_gpio_priv.h>
#include <stm32f4xx.h>
#include <stm32f4xx_tim.h>
#include <stm32f4xx_dma.h>
#include <optypes.h>
#define PIOS_WS2811_NUMLEDS 2
#define sign(x) ((x > 0) - (x < 0))
#define PIOS_WS2811_NUMLEDS 2
#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24))
#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord
#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord
#define PIOS_WS2811_TIM_PERIOD 20
// Following times are keept on the lower side to accounts
// for bus contentions and irq response time
#define PIOS_WS2811_T0_HIGH_PERIOD 25 // .35us +/- 150nS
#define PIOS_WS2811_T1_HIGH_PERIOD 60 // .70us +/- 150nS
#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \
{ \
.DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_VeryHigh, }
#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \
{ \
.DMA_BufferSize = 4, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_VeryHigh, }
#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \
{ \
.DMA_BufferSize = 4, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_High }
typedef uint16_t ledbuf_t;
typedef struct Color Color;
struct Color {
uint8_t R;
uint8_t G;
uint8_t B;
};
struct pios_ws2811_pin_cfg {
GPIO_TypeDef *gpio;
GPIO_InitTypeDef gpioInit;
};
struct pios_ws2811_cfg {
TIM_TimeBaseInitTypeDef timerInit;
TIM_TypeDef *timer;
uint8_t timerCh1;
uint8_t timerCh2;
DMA_InitTypeDef dmaInitCh1;
DMA_Stream_TypeDef *streamCh1;
uint32_t dmaItCh1;
DMA_InitTypeDef dmaInitCh2;
DMA_Stream_TypeDef *streamCh2;
uint32_t dmaItCh2;
DMA_InitTypeDef dmaInitUpdate;
DMA_Stream_TypeDef *streamUpdate;
uint32_t dmaItUpdate;
uint16_t dmaSource;
struct stm32_irq irq;
};
void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg);
void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update);
void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update);
void PIOS_WS2811_Update();
void PIOS_WS2811_DMA_irq_handler();
#endif /* PIOS_WS2811_H_ */

136
flight/pios/inc/pios_ws2811_cfg.h Normal file
View File

@ -0,0 +1,136 @@
/**
******************************************************************************
*
* @file pios_ws2811_cfg.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @brief A driver for ws2811 rgb led controller.
* this is a plain PiOS port of the very clever solution
* implemented by Omri Iluz in the chibios driver here:
* https://github.com/omriiluz/WS2812B-LED-Driver-ChibiOS
* @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_WS2811_CFG_H_
#define PIOS_WS2811_CFG_H_
#include <stdint.h>
#include <pios_gpio.h>
#include <pios_stm32.h>
#include <pios_gpio_priv.h>
#include <stm32f4xx.h>
#include <stm32f4xx_tim.h>
#include <stm32f4xx_dma.h>
#include <optypes.h>
#include <pios_ws2811.h>
#define PIOS_WS2811_BUFFER_SIZE (((PIOS_WS2811_NUMLEDS) * 24))
#define PIOS_WS2811_MEMORYDATASIZE DMA_MemoryDataSize_HalfWord
#define PIOS_WS2811_PERIPHERALDATASIZE DMA_PeripheralDataSize_HalfWord
#define PIOS_WS2811_TIM_PERIOD 20
// Following times are keept on the lower side to accounts
// for bus contentions and irq response time
#define PIOS_WS2811_T0_HIGH_PERIOD 32 // .35us +/- 150nS
#define PIOS_WS2811_T1_HIGH_PERIOD 70 // .70us +/- 150nS
#define PIOS_WS2811_DMA_CH1_CONFIG(channel) \
{ \
.DMA_BufferSize = PIOS_WS2811_BUFFER_SIZE, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_VeryHigh, }
#define PIOS_WS2811_DMA_CH2_CONFIG(channel) \
{ \
.DMA_BufferSize = 4, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_VeryHigh, }
#define PIOS_WS2811_DMA_UPDATE_CONFIG(channel) \
{ \
.DMA_BufferSize = 4, \
.DMA_Channel = channel, \
.DMA_DIR = DMA_DIR_MemoryToPeripheral, \
.DMA_FIFOMode = DMA_FIFOMode_Enable, \
.DMA_FIFOThreshold = DMA_FIFOThreshold_Full, \
.DMA_Memory0BaseAddr = 0, \
.DMA_MemoryBurst = DMA_MemoryBurst_INC4, \
.DMA_MemoryDataSize = PIOS_WS2811_MEMORYDATASIZE, \
.DMA_MemoryInc = DMA_MemoryInc_Enable, \
.DMA_Mode = DMA_Mode_Circular, \
.DMA_PeripheralBaseAddr = 0, \
.DMA_PeripheralBurst = DMA_PeripheralBurst_Single, \
.DMA_PeripheralDataSize = PIOS_WS2811_PERIPHERALDATASIZE, \
.DMA_PeripheralInc = DMA_PeripheralInc_Disable, \
.DMA_Priority = DMA_Priority_High }
typedef uint16_t ledbuf_t;
struct pios_ws2811_pin_cfg {
GPIO_TypeDef *gpio;
GPIO_InitTypeDef gpioInit;
};
struct pios_ws2811_cfg {
TIM_TimeBaseInitTypeDef timerInit;
TIM_TypeDef *timer;
uint8_t timerCh1;
uint8_t timerCh2;
DMA_InitTypeDef dmaInitCh1;
DMA_Stream_TypeDef *streamCh1;
uint32_t dmaItCh1;
DMA_InitTypeDef dmaInitCh2;
DMA_Stream_TypeDef *streamCh2;
uint32_t dmaItCh2;
DMA_InitTypeDef dmaInitUpdate;
DMA_Stream_TypeDef *streamUpdate;
uint32_t dmaItUpdate;
uint16_t dmaSource;
struct stm32_irq irq;
};
void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pios_ws2811_pin_cfg *ws2811_pin_cfg);
void PIOS_WS2811_DMA_irq_handler();
#endif /* PIOS_WS2811_H_ */

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

@ -29,7 +29,7 @@
#ifdef PIOS_INCLUDE_WS2811
#include "pios_ws2811.h"
#include "pios_ws2811_cfg.h"
#include <stm32f4xx_rcc.h>
#include <pios_stm32.h>
#include "FreeRTOS.h"
@ -177,7 +177,7 @@ void PIOS_WS2811_Init(const struct pios_ws2811_cfg *ws2811_cfg, const struct pio
fb = (ledbuf_t *)pios_malloc(PIOS_WS2811_BUFFER_SIZE * sizeof(ledbuf_t));
memset(fb, 0, PIOS_WS2811_BUFFER_SIZE * sizeof(ledbuf_t));
Color ledoff = { 0, 0, 0 };
const Color_t ledoff = Color_Off;
for (uint8_t i = 0; i < PIOS_WS2811_NUMLEDS; i++) {
PIOS_WS2811_setColorRGB(ledoff, i, false);
}
@ -302,7 +302,7 @@ void setColor(uint8_t color, ledbuf_t *buf)
* @param led led number
* @param update Perform an update after changing led color
*/
void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update)
void PIOS_WS2811_setColorRGB(Color_t c, uint8_t led, bool update)
{
if (led >= PIOS_WS2811_NUMLEDS) {
return;
@ -310,6 +310,7 @@ void PIOS_WS2811_setColorRGB(Color c, uint8_t led, bool update)
setColor(c.G, fb + (led * 24));
setColor(c.R, fb + 8 + (led * 24));
setColor(c.B, fb + 16 + (led * 24));
if (update) {
PIOS_WS2811_Update();
}
@ -327,6 +328,10 @@ void PIOS_WS2811_Update()
// reset counters for synchronization
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
TIM_Cmd(pios_ws2811_cfg->timer, ENABLE);
}
@ -337,8 +342,12 @@ void PIOS_WS2811_Update()
void PIOS_WS2811_DMA_irq_handler()
{
pios_ws2811_pin_cfg->gpio->BSRRH = dmaSource[0];
pios_ws2811_cfg->timer->CR1 &= (uint16_t) ~TIM_CR1_CEN;
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

2
flight/targets/boards/discoveryf4bare/board_hw_defs.c
View File

@ -1811,7 +1811,7 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
};
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */
#ifdef PIOS_INCLUDE_WS2811
#include <pios_ws2811.h>
#include <pios_ws2811_cfg.h>
#define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD))
void DMA2_Stream1_IRQHandler(void) __attribute__((alias("PIOS_WS2811_irq_handler")));

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

@ -35,6 +35,7 @@ USE_DSP_LIB ?= NO
#MODULES += Airspeed
#MODULES += AltitudeHold
MODULES += Stabilization
MODULES += Notify
MODULES += ManualControl
MODULES += Receiver
MODULES += Actuator
@ -88,6 +89,7 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
SRC += $(FLIGHTLIB)/lednotification.c
## UAVObjects
include ./UAVObjects.inc

2
flight/targets/boards/revolution/board_hw_defs.c
View File

@ -1964,7 +1964,7 @@ const struct pios_usb_hid_cfg pios_usb_hid_cfg = {
#endif /* PIOS_INCLUDE_USB_HID && PIOS_INCLUDE_USB_CDC */
#ifdef PIOS_INCLUDE_WS2811
#include <pios_ws2811.h>
#include <pios_ws2811_cfg.h>
#include <hwsettings.h>
#define PIOS_WS2811_TIM_DIVIDER (PIOS_PERIPHERAL_APB2_CLOCK / (800000 * PIOS_WS2811_TIM_PERIOD))

2
flight/targets/boards/revolution/firmware/Makefile
View File

@ -49,6 +49,7 @@ MODULES += PathFollower
MODULES += Osd/osdoutout
MODULES += Logging
MODULES += Telemetry
MODULES += Notify
OPTMODULES += ComUsbBridge
@ -88,6 +89,7 @@ ifndef TESTAPP
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/insgps13state.c
SRC += $(FLIGHTLIB)/auxmagsupport.c
SRC += $(FLIGHTLIB)/lednotification.c
## UAVObjects
include ./UAVObjects.inc

1
flight/tests/lednotification/FreeRTOS.h Normal file
View File

@ -0,0 +1 @@
#include <stdlib.h>

41
flight/tests/lednotification/Makefile Normal file
View File

@ -0,0 +1,41 @@
###############################################################################
# @file Makefile
# @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012
# Copyright (c) 2013, The OpenPilot Team, http://www.openpilot.org
# @addtogroup
# @{
# @addtogroup
# @{
# @brief Makefile for unit test
###############################################################################
#
# 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 OPENPILOT_IS_COOL
$(error Top level Makefile must be used to build this target)
endif
include $(ROOT_DIR)/make/firmware-defs.mk
EXTRAINCDIRS += $(TOPDIR)
EXTRAINCDIRS += $(PIOS)/inc
EXTRAINCDIRS += $(FLIGHTLIB)/inc
EXTRAINCDIRS += $(FLIGHTLIB)
SRC += $(FLIGHTLIB)/optypes.c
SRC += $(PIOS)/common/pios_notify.c
include $(ROOT_DIR)/make/unittest.mk

11
flight/tests/lednotification/openpilot.h Normal file
View File

@ -0,0 +1,11 @@
#ifndef OPENPILOT_H
#define OPENPILOT_H
#include <stdbool.h>
#define PIOS_Assert(x) \
if (!(x)) { while (1) {; } \
}
#define PIOS_DEBUG_Assert(x) PIOS_Assert(x)
#endif /* OPENPILOT_H */

13
flight/tests/lednotification/pios.h Normal file
View File

@ -0,0 +1,13 @@
#ifndef PIOS_H
#define PIOS_H
/* PIOS Feature Selection */
#include "pios_config.h"
#ifdef PIOS_INCLUDE_FREERTOS
/* FreeRTOS Includes */
#include "FreeRTOS.h"
#endif
#include "pios_mem.h"
#endif /* PIOS_H */

6
flight/tests/lednotification/pios_config.h Normal file
View File

@ -0,0 +1,6 @@
#ifndef PIOS_CONFIG_H
#define PIOS_CONFIG_H
#define PIOS_INCLUDE_FREERTOS
#endif /* PIOS_CONFIG_H */

34
flight/tests/lednotification/pios_mem.h Normal file
View File

@ -0,0 +1,34 @@
/**
******************************************************************************
*
* @file pios_mem.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
* @addtogroup PiOS
* @{
* @addtogroup PiOS
* @{
* @brief PiOS memory allocation API
*****************************************************************************/
/*
* 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_MEM_H
#define PIOS_MEM_H
#define pios_fastheapmalloc(size) (malloc(size))
#define pios_malloc(size) (malloc(size))
#define pios_free(p) (free(p))
#endif /* PIOS_MEM_H */

153
flight/tests/lednotification/unittest.cpp Normal file
View File

@ -0,0 +1,153 @@
#include "gtest/gtest.h"
#include <stdio.h> /* printf */
#include <stdlib.h> /* abort */
#include <string.h> /* memset */
extern "C" {
#define xTaskGetTickCount() 1
#define portTICK_RATE_MS 1
#include "lednotification.c"
void PIOS_WS2811_setColorRGB(__attribute__((unused)) Color_t c, __attribute__((unused)) uint8_t led, __attribute__((unused)) bool update) {}
void PIOS_WS2811_Update() {}
}
class LedNotificationTest : public testing::Test {};
int compare_int8(const void *a, const void *b)
{
const int8_t *da = (const int8_t *)a;
const int8_t *db = (const int8_t *)b;
return (*da > *db) - (*da < *db);
}
void sort_priorities(int8_t *queued_priorities)
{
qsort(queued_priorities, MAX_BACKGROUND_NOTIFICATIONS, sizeof(int8_t), compare_int8);
}
void set_highest_active(NotifierLedStatus_t *status)
{
int8_t highest = NOTIFY_PRIORITY_BACKGROUND;
for (int i = 0; i < MAX_BACKGROUND_NOTIFICATIONS; i++) {
if (status->queued_priorities[i] > highest) {
status->active_sequence_num = i;
highest = status->queued_priorities[i];
}
}
}
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) {
NotifierLedStatus_t status;
init(&status, NOTIFY_PRIORITY_BACKGROUND);
insert(&status, NOTIFY_PRIORITY_LOW);
insert(&status, NOTIFY_PRIORITY_CRITICAL);
insert(&status, NOTIFY_PRIORITY_LOW);
insert(&status, NOTIFY_PRIORITY_CRITICAL);
set_highest_active(&status);
EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_CRITICAL, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
}
TEST_F(LedNotificationTest, TestQueueOrder2) {
NotifierLedStatus_t status;
// Fails because insert_point and first_point will both be -1. This will also cause an array-out-of bounds at:
// 146 status->queued_priorities[insert_point] = new_notification->priority;
// 147 status->queued_sequences[insert_point] = new_notification->sequence;
// 148 updated_sequence = insert_point;
init(&status, NOTIFY_PRIORITY_LOW);
status.queued_priorities[0] = NOTIFY_PRIORITY_BACKGROUND;
insert(&status, NOTIFY_PRIORITY_REGULAR);
set_highest_active(&status);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
}
TEST_F(LedNotificationTest, TestQueueOrder3) {
NotifierLedStatus_t status;
// Fails because queued_priorities[0] _LOW and not _REGULAR. I _thibnk_ this is a bug.
init(&status, NOTIFY_PRIORITY_REGULAR);
status.queued_priorities[0] = NOTIFY_PRIORITY_BACKGROUND;
insert(&status, NOTIFY_PRIORITY_LOW);
set_highest_active(&status);
for (uint8_t i = 0; i < MAX_BACKGROUND_NOTIFICATIONS - 1; i++) {
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
}
}
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);
set_highest_active(&status);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_REGULAR, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_LOW, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
pop_queued_sequence(&status);
EXPECT_EQ(NOTIFY_PRIORITY_BACKGROUND, (status.queued_priorities[status.active_sequence_num]));
}

1
make/apps-defs.mk
View File

@ -111,6 +111,7 @@ SRC += $(MATHLIB)/pid.c
SRC += $(MATHLIB)/mathmisc.c
SRC += $(MATHLIB)/butterworth.c
SRC += $(FLIGHTLIB)/printf-stdarg.c
SRC += $(FLIGHTLIB)/optypes.c
## Modules
SRC += $(foreach mod, $(MODULES), $(sort $(wildcard $(OPMODULEDIR)/$(mod)/*.c)))

4
make/unittest.mk
View File

@ -52,6 +52,10 @@ CXXFLAGS += -g -Wall -Wextra
# Flags passed to the C compiler
CONLYFLAGS += -std=gnu99
# UNIT_TEST allows to for example to have optional test fixture code enabled or private code exposed in application modules.
CFLAGS += -DUNIT_TEST
CPPFLAGS += -DUNIT_TEST
# Common compiler flags
CFLAGS += -O0 -g
CFLAGS += -Wall -Werror