mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-11-29 07:24:13 +01:00
Merge remote-tracking branch 'origin/next' into thread/OP-1512_ESC_Cal
Conflicts: ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
This commit is contained in:
m_thread
commit
ada27f2568
2
Makefile
2
Makefile
@ -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
33
flight/libraries/inc/lednotification.h
Normal 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_ */
|
||||
@ -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
64
flight/libraries/inc/optypes.h
Normal 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
259
flight/libraries/lednotification.c
Normal 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]);
|
||||
}
|
||||
}
|
||||
@ -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
42
flight/libraries/optypes.c
Normal 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;
|
||||
@ -245,7 +245,9 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
if (gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX) {
|
||||
char *buffer = 0;
|
||||
uint16_t count = 0;
|
||||
ubx_autoconfig_run(&buffer, &count);
|
||||
uint8_t status;
|
||||
GPSPositionSensorStatusGet(&status);
|
||||
ubx_autoconfig_run(&buffer, &count, status != GPSPOSITIONSENSOR_STATUS_NOGPS);
|
||||
// Something to send?
|
||||
if (count) {
|
||||
PIOS_COM_SendBuffer(gpsPort, (uint8_t *)buffer, count);
|
||||
@ -266,11 +268,17 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
{
|
||||
#if defined(PIOS_INCLUDE_GPS_UBX_PARSER) && !defined(PIOS_GPS_MINIMAL)
|
||||
int32_t ac_status = ubx_autoconfig_get_status();
|
||||
static uint8_t lastStatus = 0;
|
||||
|
||||
gpspositionsensor.AutoConfigStatus =
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_DISABLED ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DISABLED :
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_DONE ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_DONE :
|
||||
ac_status == UBX_AUTOCONFIG_STATUS_ERROR ? GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR :
|
||||
GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_RUNNING;
|
||||
if (gpspositionsensor.AutoConfigStatus != lastStatus) {
|
||||
GPSPositionSensorAutoConfigStatusSet(&gpspositionsensor.AutoConfigStatus);
|
||||
lastStatus = gpspositionsensor.AutoConfigStatus;
|
||||
}
|
||||
#endif
|
||||
res = parse_ubx_stream(c, gps_rx_buffer, &gpspositionsensor, &gpsRxStats);
|
||||
}
|
||||
@ -290,7 +298,8 @@ static void gpsTask(__attribute__((unused)) void *parameters)
|
||||
|
||||
// Check for GPS timeout
|
||||
timeNowMs = xTaskGetTickCount() * portTICK_RATE_MS;
|
||||
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS) {
|
||||
if ((timeNowMs - timeOfLastUpdateMs) >= GPS_TIMEOUT_MS ||
|
||||
(gpsSettings.DataProtocol == GPSSETTINGS_DATAPROTOCOL_UBX && gpspositionsensor.AutoConfigStatus == GPSPOSITIONSENSOR_AUTOCONFIGSTATUS_ERROR)) {
|
||||
// we have not received any valid GPS sentences for a while.
|
||||
// either the GPS is not plugged in or a hardware problem or the GPS has locked up.
|
||||
uint8_t status = GPSPOSITIONSENSOR_STATUS_NOGPS;
|
||||
|
||||
@ -515,6 +515,14 @@ uint32_t parse_ubx_message(struct UBXPacket *ubx, GPSPositionSensorData *GpsPosi
|
||||
GPSPositionSensorSet(GpsPosition);
|
||||
msgtracker.msg_received = NONE_RECEIVED;
|
||||
id = GPSPOSITIONSENSOR_OBJID;
|
||||
} else {
|
||||
uint8_t status;
|
||||
GPSPositionSensorStatusGet(&status);
|
||||
if (status == GPSPOSITIONSENSOR_STATUS_NOGPS) {
|
||||
// Some ubx thing has been received so GPS is there
|
||||
status = GPSPOSITIONSENSOR_STATUS_NOFIX;
|
||||
GPSPositionSensorStatusSet(&status);
|
||||
}
|
||||
}
|
||||
return id;
|
||||
}
|
||||
|
||||
@ -33,13 +33,20 @@
|
||||
|
||||
// defines
|
||||
// TODO: NEO8 max rate is for Rom version, flash is limited to 10Hz, need to handle that.
|
||||
#define UBX_MAX_RATE_VER8 18
|
||||
#define UBX_MAX_RATE_VER7 10
|
||||
#define UBX_MAX_RATE 5
|
||||
|
||||
#define UBX_REPLY_TIMEOUT (500 * 1000)
|
||||
#define UBX_MAX_RETRIES 5
|
||||
#define UBX_MAX_RATE_VER8 18
|
||||
#define UBX_MAX_RATE_VER7 10
|
||||
#define UBX_MAX_RATE 5
|
||||
|
||||
// time to wait before reinitializing the fsm due to disconnection
|
||||
#define UBX_CONNECTION_TIMEOUT (2000 * 1000)
|
||||
// times between retries in case an error does occurs
|
||||
#define UBX_ERROR_RETRY_TIMEOUT (1000 * 1000)
|
||||
// timeout for ack reception
|
||||
#define UBX_REPLY_TIMEOUT (500 * 1000)
|
||||
// max retries in case of timeout
|
||||
#define UBX_MAX_RETRIES 5
|
||||
// pause between each configuration step
|
||||
#define UBX_STEP_WAIT_TIME (10 * 1000)
|
||||
// types
|
||||
typedef enum {
|
||||
UBX_AUTOCONFIG_STATUS_DISABLED = 0,
|
||||
@ -179,7 +186,7 @@ typedef union {
|
||||
} message;
|
||||
} __attribute__((packed)) UBXSentPacket_t;
|
||||
|
||||
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send);
|
||||
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected);
|
||||
void ubx_autoconfig_set(ubx_autoconfig_settings_t config);
|
||||
int32_t ubx_autoconfig_get_status();
|
||||
#endif /* UBX_AUTOCONFIG_H_ */
|
||||
|
||||
@ -46,6 +46,7 @@ typedef enum {
|
||||
typedef struct {
|
||||
initSteps_t currentStep; // Current configuration "fsm" status
|
||||
uint32_t lastStepTimestampRaw; // timestamp of last operation
|
||||
uint32_t lastConnectedRaw; // timestamp of last time gps was connected
|
||||
UBXSentPacket_t working_packet; // outbound "buffer"
|
||||
ubx_autoconfig_settings_t currentSettings;
|
||||
int8_t lastConfigSent; // index of last configuration string sent
|
||||
@ -227,7 +228,7 @@ void config_save(uint16_t *bytes_to_send)
|
||||
{
|
||||
memset(status->working_packet.buffer, 0, sizeof(UBXSentHeader_t) + sizeof(ubx_cfg_cfg_t));
|
||||
// mask LSB=ioPort|msgConf|infMsg|navConf|rxmConf|||||rinvConf|antConf|....|= MSB
|
||||
status->working_packet.message.payload.cfg_cfg.saveMask = 0x01 | 0x08; // msgConf + navConf
|
||||
status->working_packet.message.payload.cfg_cfg.saveMask = 0x02 | 0x08; // msgConf + navConf
|
||||
status->working_packet.message.payload.cfg_cfg.deviceMask = UBX_CFG_CFG_ALL_DEVICES_MASK;
|
||||
*bytes_to_send = prepare_packet(&status->working_packet, UBX_CLASS_CFG, UBX_ID_CFG_CFG, sizeof(ubx_cfg_cfg_t));
|
||||
status->requiredAck.clsID = UBX_CLASS_CFG;
|
||||
@ -279,21 +280,50 @@ static void enable_sentences(__attribute__((unused)) uint16_t *bytes_to_send)
|
||||
}
|
||||
}
|
||||
|
||||
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send, bool gps_connected)
|
||||
{
|
||||
*bytes_to_send = 0;
|
||||
*buffer = (char *)status->working_packet.buffer;
|
||||
if (!status || !enabled) {
|
||||
if (!status || !enabled || status->currentStep == INIT_STEP_DISABLED) {
|
||||
return; // autoconfig not enabled
|
||||
}
|
||||
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) < UBX_STEP_WAIT_TIME) {
|
||||
return;
|
||||
}
|
||||
// when gps is disconnected it will replay the autoconfig sequence.
|
||||
if (!gps_connected) {
|
||||
if (status->currentStep == INIT_STEP_DONE) {
|
||||
// if some operation is in progress and it is not running into issues it maybe that
|
||||
// the disconnection is only due to wrong message rates, so reinit only when done.
|
||||
// errors caused by disconnection are handled by error retry logic
|
||||
if (PIOS_DELAY_DiffuS(status->lastConnectedRaw) > UBX_CONNECTION_TIMEOUT) {
|
||||
status->currentStep = INIT_STEP_START;
|
||||
return;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// reset connection timer
|
||||
status->lastConnectedRaw = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
|
||||
switch (status->currentStep) {
|
||||
case INIT_STEP_ERROR: // TODO: what to do? retries after a while? maybe gps was disconnected (this can be detected)?
|
||||
case INIT_STEP_ERROR:
|
||||
if (PIOS_DELAY_DiffuS(status->lastStepTimestampRaw) > UBX_ERROR_RETRY_TIMEOUT) {
|
||||
status->currentStep = INIT_STEP_START;
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
}
|
||||
return;
|
||||
|
||||
case INIT_STEP_DISABLED:
|
||||
case INIT_STEP_DONE:
|
||||
return;
|
||||
|
||||
case INIT_STEP_START:
|
||||
case INIT_STEP_ASK_VER:
|
||||
// clear ack
|
||||
ubxLastAck.clsID = 0x00;
|
||||
ubxLastAck.msgID = 0x00;
|
||||
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
build_request(&status->working_packet, UBX_CLASS_MON, UBX_ID_MON_VER, bytes_to_send);
|
||||
status->currentStep = INIT_STEP_WAIT_VER;
|
||||
@ -348,6 +378,7 @@ void ubx_autoconfig_run(char * *buffer, uint16_t *bytes_to_send)
|
||||
status->retryCount++;
|
||||
if (status->retryCount > UBX_MAX_RETRIES) {
|
||||
status->currentStep = INIT_STEP_ERROR;
|
||||
status->lastStepTimestampRaw = PIOS_DELAY_GetRaw();
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -374,12 +405,16 @@ void ubx_autoconfig_set(ubx_autoconfig_settings_t config)
|
||||
status->currentSettings = config;
|
||||
status->currentStep = INIT_STEP_START;
|
||||
enabled = true;
|
||||
} else {
|
||||
if (!status) {
|
||||
status->currentStep = INIT_STEP_DISABLED;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int32_t ubx_autoconfig_get_status()
|
||||
{
|
||||
if (!status) {
|
||||
if (!status || !enabled) {
|
||||
return UBX_AUTOCONFIG_STATUS_DISABLED;
|
||||
}
|
||||
switch (status->currentStep) {
|
||||
|
||||
33
flight/modules/Notify/inc/notify.h
Normal file
33
flight/modules/Notify/inc/notify.h
Normal 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
222
flight/modules/Notify/inc/sequences.h
Normal 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] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_MANUAL],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED1] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED1],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED2] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED2],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED3] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED3],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED4] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED4],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED5] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED5],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_STABILIZED6] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_STABILIZED6],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_AUTO],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_AUTOTUNE],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOFPV] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIOLOS] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POSITIONVARIONSEW] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_RTH],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_LAND] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS],
|
||||
[FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[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
136
flight/modules/Notify/notify.c
Normal 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(¬ifications[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(
|
||||
¬ifications[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;
|
||||
}
|
||||
}
|
||||
@ -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.
|
||||
|
||||
@ -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;
|
||||
}
|
||||
|
||||
@ -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_ */
|
||||
|
||||
@ -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
136
flight/pios/inc/pios_ws2811_cfg.h
Normal 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_ */
|
||||
@ -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
|
||||
|
||||
@ -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")));
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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))
|
||||
|
||||
|
||||
@ -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
1
flight/tests/lednotification/FreeRTOS.h
Normal file
@ -0,0 +1 @@
|
||||
#include <stdlib.h>
|
||||
41
flight/tests/lednotification/Makefile
Normal file
41
flight/tests/lednotification/Makefile
Normal 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
11
flight/tests/lednotification/openpilot.h
Normal 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
13
flight/tests/lednotification/pios.h
Normal 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
6
flight/tests/lednotification/pios_config.h
Normal 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
34
flight/tests/lednotification/pios_mem.h
Normal 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
153
flight/tests/lednotification/unittest.cpp
Normal 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(¬ification, 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]));
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 160 KiB After Width: | Height: | Size: 165 KiB |
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 239 KiB After Width: | Height: | Size: 256 KiB |
@ -282,8 +282,8 @@
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Setup "TurboPWM" here: usual value is 400 Hz for multirotor airframes.
|
||||
Leave at 50Hz for fixed wing.</string>
|
||||
<string>Setup "RapidESC" here: usual value is 500 Hz for multirotor airframes.
|
||||
</string>
|
||||
</property>
|
||||
<item>
|
||||
<property name="text">
|
||||
@ -320,6 +320,11 @@ Leave at 50Hz for fixed wing.</string>
|
||||
<string>400</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>500</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="3">
|
||||
@ -340,8 +345,8 @@ Leave at 50Hz for fixed wing.</string>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Setup "TurboPWM" here: usual value is 400 Hz for multirotor airframes.
|
||||
Leave at 50Hz for fixed wing.</string>
|
||||
<string>Setup "RapidESC" here: usual value is 500 Hz for multirotor airframes.
|
||||
</string>
|
||||
</property>
|
||||
<item>
|
||||
<property name="text">
|
||||
@ -378,6 +383,11 @@ Leave at 50Hz for fixed wing.</string>
|
||||
<string>400</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>500</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="1">
|
||||
@ -420,8 +430,8 @@ Leave at 50Hz for fixed wing.</string>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Setup "TurboPWM" here: usual value is 400 Hz for multirotor airframes.
|
||||
Leave at 50Hz for fixed wing.</string>
|
||||
<string>Setup "RapidESC" here: usual value is 500 Hz for multirotor airframes.
|
||||
</string>
|
||||
</property>
|
||||
<item>
|
||||
<property name="text">
|
||||
@ -458,6 +468,11 @@ Leave at 50Hz for fixed wing.</string>
|
||||
<string>400</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>500</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="5">
|
||||
@ -478,8 +493,8 @@ Leave at 50Hz for fixed wing.</string>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Setup "TurboPWM" here: usual value is 400 Hz for multirotor airframes.
|
||||
Leave at 50Hz for fixed wing.</string>
|
||||
<string>Setup "RapidESC" here: usual value is 500 Hz for multirotor airframes.
|
||||
</string>
|
||||
</property>
|
||||
<item>
|
||||
<property name="text">
|
||||
@ -516,6 +531,11 @@ Leave at 50Hz for fixed wing.</string>
|
||||
<string>400</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>500</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="0" column="7">
|
||||
@ -546,8 +566,8 @@ Leave at 50Hz for fixed wing.</string>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Setup "TurboPWM" here: usual value is 400 Hz for multirotor airframes.
|
||||
Leave at 50Hz for fixed wing.</string>
|
||||
<string>Setup "RapidESC" here: usual value is 500 Hz for multirotor airframes.
|
||||
</string>
|
||||
</property>
|
||||
<item>
|
||||
<property name="text">
|
||||
@ -584,6 +604,11 @@ Leave at 50Hz for fixed wing.</string>
|
||||
<string>400</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>500</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
<item row="1" column="7">
|
||||
@ -604,8 +629,8 @@ Leave at 50Hz for fixed wing.</string>
|
||||
</size>
|
||||
</property>
|
||||
<property name="toolTip">
|
||||
<string>Setup "TurboPWM" here: usual value is 400 Hz for multirotor airframes.
|
||||
Leave at 50Hz for fixed wing.</string>
|
||||
<string>Setup "RapidESC" here: usual value is 500 Hz for multirotor airframes.
|
||||
</string>
|
||||
</property>
|
||||
<item>
|
||||
<property name="text">
|
||||
@ -642,6 +667,11 @@ Leave at 50Hz for fixed wing.</string>
|
||||
<string>400</string>
|
||||
</property>
|
||||
</item>
|
||||
<item>
|
||||
<property name="text">
|
||||
<string>500</string>
|
||||
</property>
|
||||
</item>
|
||||
</widget>
|
||||
</item>
|
||||
</layout>
|
||||
|
||||
@ -26,8 +26,7 @@ p, li { white-space: pre-wrap; }
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">To set an optimal configuration of the output signals powering your motors, the wizard needs to know what type of Electronic Speed Controllers (ESCs) you will use and what their capabilities are.</span></p>
|
||||
<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">Please select one of the options below. If you are unsure about the capabilities of your ESCs, just leave the default option selected and continue the wizard.</span></p>
|
||||
<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"><br /></p>
|
||||
<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-size:10pt;">To read more regarding ESC refresh rates, please see </span><a href="http://wiki.openpilot.org/x/HIEu"><span style=" font-family:'Lucida Grande'; font-size:13pt; text-decoration: underline; color:#0000ff;">this article</span></a><span style=" font-size:10pt;"> in the OpenPilot Wiki</span></p></body></html></string>
|
||||
<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-size:10pt;"><br /></p></body></html></string>
|
||||
</property>
|
||||
<property name="alignment">
|
||||
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop</set>
|
||||
@ -113,12 +112,12 @@ p, li { white-space: pre-wrap; }
|
||||
<string notr="true">QToolButton { border: none }</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>Turbo PWM</string>
|
||||
<string>Rapid ESC</string>
|
||||
</property>
|
||||
<property name="icon">
|
||||
<iconset resource="../wizardResources.qrc">
|
||||
<normaloff>:/setupwizard/resources/bttn-turbo-up.png</normaloff>
|
||||
<normalon>:/setupwizard/resources/bttn-turbo-down.png</normalon>:/setupwizard/resources/bttn-turbo-up.png</iconset>
|
||||
<normaloff>:/setupwizard/resources/bttn-rapid-up.png</normaloff>
|
||||
<normalon>:/setupwizard/resources/bttn-rapid-dwn.png</normalon>:/setupwizard/resources/bttn-rapid-up.png</iconset>
|
||||
</property>
|
||||
<property name="iconSize">
|
||||
<size>
|
||||
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 5.6 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 6.4 KiB |
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 2.2 MiB After Width: | Height: | Size: 2.3 MiB |
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 181 KiB After Width: | Height: | Size: 288 KiB |
File diff suppressed because one or more lines are too long
|
Before Width: | Height: | Size: 271 KiB After Width: | Height: | Size: 421 KiB |
@ -371,7 +371,7 @@ QString SetupWizard::getSummaryText()
|
||||
summary.append(tr("Standard ESC (50 Hz)"));
|
||||
break;
|
||||
case ESC_RAPID:
|
||||
summary.append(tr("Rapid ESC (400 Hz)"));
|
||||
summary.append(tr("Rapid ESC (500 Hz)"));
|
||||
break;
|
||||
default:
|
||||
summary.append(tr("Unknown"));
|
||||
|
||||
@ -29,6 +29,8 @@
|
||||
<file>resources/bttn-calculate-up.png</file>
|
||||
<file>resources/bttn-turbo-down.png</file>
|
||||
<file>resources/bttn-turbo-up.png</file>
|
||||
<file>resources/bttn-rapid-up.png</file>
|
||||
<file>resources/bttn-rapid-dwn.png</file>
|
||||
<file>resources/bttn-servo-digital-dwn.png</file>
|
||||
<file>resources/bttn-servo-digital-up.png</file>
|
||||
<file>resources/bttn-servo-standard-dwn.png</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)))
|
||||
|
||||
@ -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
|
||||
|
||||
@ -10,7 +10,7 @@
|
||||
<field name="Yaw" units="degrees" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="100"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="130"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user