rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

OP-1379 - Initial Notify module. Supports:

Browse Source 
1) predefined seqences associated with each flightmode. 2) fixed table with alarm index and assciated led warn/error sequences/repetition rate.
This commit is contained in:
Alessio Morale 2014-08-13 15:18:17 +02:00
parent f556a5e335
commit 376aa0052f
4 changed files with 593 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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_ */

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

@ -0,0 +1,417 @@
/**
******************************************************************************
*
* @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>
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_DISARMED = 18,
} NotifySequences;
typedef struct {
uint32_t timeBetweenNotifications;
uint8_t alarmIndex;
uint8_t warnNotification;
uint8_t errorNotification;
} AlarmDefinition_t;
// consts
const LedSequence_t notifications[] = {
[NOTIFY_SEQUENCE_DISARMED] = {
.repeats = -1,
.steps = {
{
.time_off = 1000,
.time_on = 1000,
.color = COLOR_BLUE,
.repeats = 1,
},
{
.time_off = 1000,
.time_on = 1000,
.color = COLOR_LIME,
.repeats = 1,
},
{
.time_off = 1000,
.time_on = 1000,
.color = COLOR_RED,
.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 = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
{
.time_off = 500,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_LAND] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_BLUE,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_BLUE,
.repeats = 1,
},
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ARMED_FM_AUTO] = {
.repeats = -1,
.steps = {
{
.time_off = 100,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 2,
},
{
.time_off = 500,
.time_on = 200,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_WARN_GPS] = {
.repeats = 2,
.steps = {
{
.time_off = 300,
.time_on = 300,
.color = COLOR_ORANGE,
.repeats = 2,
},
{
.time_off = 300,
.time_on = 300,
.color = COLOR_YELLOW,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_ERROR_GPS] = {
.repeats = 2,
.steps = {
{
.time_off = 300,
.time_on = 300,
.color = COLOR_RED,
.repeats = 2,
},
{
.time_off = 300,
.time_on = 300,
.color = COLOR_YELLOW,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_WARN_BATTERY] = {
.repeats = 1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_ORANGE,
.repeats = 10,
},
},
},
[NOTIFY_SEQUENCE_ALM_ERROR_BATTERY] = {
.repeats = 1,
.steps = {
{
.time_off = 100,
.time_on = 100,
.color = COLOR_RED,
.repeats = 10,
},
},
},
[NOTIFY_SEQUENCE_ALM_MAG] = {
.repeats = 1,
.steps = {
{
.time_off = 300,
.time_on = 300,
.color = COLOR_RED,
.repeats = 2,
},
{
.time_off = 300,
.time_on = 300,
.color = COLOR_GREEN,
.repeats = 1,
},
},
},
[NOTIFY_SEQUENCE_ALM_CONFIG] = {
.repeats = -1,
.steps = {
{
.time_off = 500,
.time_on = 100,
.color = COLOR_RED,
.repeats = 1,
},
},
},
};
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],
};
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_ALM_MAG,
.errorNotification = NOTIFY_SEQUENCE_ALM_MAG,
},
{
.timeBetweenNotifications = 15000,
.alarmIndex = SYSTEMALARMS_ALARM_BATTERY,
.warnNotification = NOTIFY_SEQUENCE_ALM_WARN_BATTERY,
.errorNotification = NOTIFY_SEQUENCE_ALM_ERROR_BATTERY,
},
};
const uint8_t alarmsMapSize = NELEMENTS(alarmsMap);
#endif /* SEQUENCES_H_ */

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

@ -0,0 +1,142 @@
/**
******************************************************************************
*
* @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
*/
/**
* Input objects: ExampleObject1, ExampleSettings
* Output object: ExampleObject2
*
* This module executes in response to ExampleObject1 updates. When the
* module is triggered it will update the data of ExampleObject2.
*
* No threads are used in this example.
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#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>
#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)
{
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);
updatedCb(0);
static UAVObjEvent ev;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicCallbackCreate(&ev, onTimerCb, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
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 < current_time) {
uint8_t sequence = (alarm == SYSTEMALARMS_ALARM_WARNING) ? warn_sequence : error_sequence;
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;
}
}

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

@ -50,6 +50,7 @@ MODULES += FixedWingPathFollower
MODULES += Osd/osdoutout
MODULES += Logging
MODULES += Telemetry
MODULES += Notify
OPTMODULES += ComUsbBridge