rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Merge branch 'corvuscorax/OP-1290_telemetry_fixes' into next

Browse Source
This commit is contained in:
Corvus Corax 2014-04-06 16:02:54 +02:00
commit 3ea328d0a0
6 changed files with 93 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
flight/libraries/alarms.c
View File

@ -32,11 +32,17 @@
#include "inc/alarms.h"
// Private constants
#ifndef PIOS_ALARM_GRACETIME
// alarm cannot be turned off for at least 1000 milliseconds
// to prevent event system overload through flapping alarms
#define PIOS_ALARM_GRACETIME 1000
#endif // PIOS_ALARM_GRACETIME
// Private types
// Private variables
static xSemaphoreHandle lock;
static volatile uint16_t lastAlarmChange[SYSTEMALARMS_ALARM_NUMELEM] = { 0 }; // this deliberately overflows every 2^16 milliseconds to save memory
// Private functions
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
@ -63,7 +69,7 @@ int32_t AlarmsInitialize(void)
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@ -74,10 +80,14 @@ int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
SystemAlarmsSet(&alarms);
SystemAlarmsAlarmGet(&alarms);
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms, alarms.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms, alarms.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms, alarms.Actuator)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsAlarmSet(&alarms);
}
// Release lock
@ -110,10 +120,14 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
// Read alarm and update its severity only if it was changed
SystemAlarmsGet(&alarms);
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity) {
uint16_t flightTime = (uint16_t)xTaskGetTickCount() * (uint16_t)portTICK_RATE_MS; // this deliberately overflows every 2^16 milliseconds to save memory
if ((flightTime - lastAlarmChange[alarm] > PIOS_ALARM_GRACETIME &&
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] != severity)
|| cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] < severity) {
cast_struct_to_array(alarms.ExtendedAlarmStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = status;
cast_struct_to_array(alarms.ExtendedAlarmSubStatus, alarms.ExtendedAlarmStatus.BootFault)[alarm] = subStatus;
cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm] = severity;
lastAlarmChange[alarm] = flightTime;
SystemAlarmsSet(&alarms);
}
@ -129,7 +143,7 @@ int32_t ExtendedAlarmsSet(SystemAlarmsAlarmElem alarm,
*/
SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Check that this is a valid alarm
if (alarm >= SYSTEMALARMS_ALARM_NUMELEM) {
@ -137,8 +151,8 @@ SystemAlarmsAlarmOptions AlarmsGet(SystemAlarmsAlarmElem alarm)
}
// Read alarm
SystemAlarmsGet(&alarms);
return cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[alarm];
SystemAlarmsAlarmGet(&alarms);
return cast_struct_to_array(alarms, alarms.Actuator)[alarm];
}
/**
@ -220,17 +234,17 @@ int32_t AlarmsHasCritical()
*/
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
{
SystemAlarmsData alarms;
SystemAlarmsAlarmData alarms;
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
// Read alarms
SystemAlarmsGet(&alarms);
SystemAlarmsAlarmGet(&alarms);
// Go through alarms and check if any are of the given severity or higher
for (uint32_t n = 0; n < SYSTEMALARMS_ALARM_NUMELEM; ++n) {
if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[n] >= severity) {
if (cast_struct_to_array(alarms, alarms.Actuator)[n] >= severity) {
xSemaphoreGiveRecursive(lock);
return 1;
}

6
flight/modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -503,7 +503,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors.Lowpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError > 0 && // we are too slow already
@ -513,7 +513,7 @@ static uint8_t updateFixedDesiredAttitude()
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedwingpathfollowerStatus.Errors.Highpower = 0;
if (powerCommand >= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
if (fixedwingpathfollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedwingpathfollowerSettings.ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0 && // we ARE going up
descentspeedDesired > 0 && // we WANT to go down
airspeedError < 0 && // we are too fast already
@ -555,7 +555,7 @@ static uint8_t updateFixedDesiredAttitude()
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors.Pitchcontrol = 0;
if (pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
if (fixedwingpathfollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedwingpathfollowerSettings.PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0 && // we ARE going down
descentspeedDesired < 0 && // we WANT to go up
airspeedError < 0 && // we are too fast already

65
flight/modules/Telemetry/telemetry.c
View File

@ -54,6 +54,9 @@
// Private variables
static uint32_t telemetryPort;
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t radioPort;
#endif
static xQueueHandle queue;
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
@ -80,6 +83,7 @@ static void telemetryTxTask(void *parameters);
static void telemetryRxTask(void *parameters);
#ifdef PIOS_INCLUDE_RFM22B
static void radioRxTask(void *parameters);
static int32_t transmitRadioData(uint8_t *data, int32_t length);
#endif
static int32_t transmitData(uint8_t *data, int32_t length);
static void registerObject(UAVObjHandle obj);
@ -140,13 +144,16 @@ int32_t TelemetryInitialize(void)
// Update telemetry settings
telemetryPort = PIOS_COM_TELEM_RF;
#ifdef PIOS_INCLUDE_RFM22B
radioPort = PIOS_COM_RF;
#endif
HwSettingsInitialize();
updateSettings();
// Initialise UAVTalk
uavTalkCon = UAVTalkInitialize(&transmitData);
#ifdef PIOS_INCLUDE_RFM22B
radioUavTalkCon = UAVTalkInitialize(&transmitData);
radioUavTalkCon = UAVTalkInitialize(&transmitRadioData);
#endif
// Create periodic event that will be used to update the telemetry stats
@ -375,17 +382,28 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters)
* Tries to empty the high priority queue before handling any standard priority item
*/
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
// Loop forever
while (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
// empty priority queue, non-blocking
while (xQueueReceive(priorityQueue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#endif
// Wait for queue message
// check regular queue and process update - non-blocking
if (xQueueReceive(queue, &ev, 0) == pdTRUE) {
// Process event
processObjEvent(&ev);
// if both queues are empty, wait on priority queue for updates (1 tick) then repeat cycle
} else if (xQueueReceive(priorityQueue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#else
// wait on queue for updates (1 tick) then repeat cycle
if (xQueueReceive(queue, &ev, 1) == pdTRUE) {
// Process event
processObjEvent(&ev);
}
#endif /* if defined(PIOS_TELEM_PRIORITY_QUEUE) */
}
}
@ -424,12 +442,12 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
{
// Task loop
while (1) {
if (telemetryPort) {
if (radioPort) {
// Block until data are available
uint8_t serial_data[1];
uint16_t bytes_to_process;
bytes_to_process = PIOS_COM_ReceiveBuffer(telemetryPort, serial_data, sizeof(serial_data), 500);
bytes_to_process = PIOS_COM_ReceiveBuffer(radioPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]);
@ -440,6 +458,23 @@ static void radioRxTask(__attribute__((unused)) void *parameters)
}
}
}
/**
* Transmit data buffer to the radioport.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
* \return -1 on failure
* \return number of bytes transmitted on success
*/
static int32_t transmitRadioData(uint8_t *data, int32_t length)
{
if (radioPort) {
return PIOS_COM_SendBuffer(radioPort, data, length);
}
return -1;
}
#endif /* PIOS_INCLUDE_RFM22B */
/**
@ -676,17 +711,25 @@ static void updateSettings()
* Determine input/output com port as highest priority available
* @param[in] input Returns the approproate input com port if true, else the appropriate output com port
*/
#ifdef PIOS_INCLUDE_RFM22B
static uint32_t getComPort(bool input)
{
#ifndef PIOS_INCLUDE_RFM22B
input = false;
#else
static uint32_t getComPort(__attribute__((unused)) bool input)
#endif
{
#if defined(PIOS_INCLUDE_USB)
// if USB is connected, USB takes precedence for telemetry
if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) {
return PIOS_COM_TELEM_USB;
} else
#endif /* PIOS_INCLUDE_USB */
if (!input && PIOS_COM_Available(telemetryPort)) {
#ifdef PIOS_INCLUDE_RFM22B
// PIOS_COM_RF input is handled by a separate RX thread and therefore must be ignored
if (input && telemetryPort == PIOS_COM_RF) {
return 0;
} else
#endif /* PIOS_INCLUDE_RFM22B */
if (PIOS_COM_Available(telemetryPort)) {
return telemetryPort;
} else {
return 0;

9
flight/targets/boards/revolution/firmware/pios_board.c
View File

@ -224,6 +224,7 @@ uint32_t pios_com_debug_id;
uint32_t pios_com_gps_id = 0;
uint32_t pios_com_telem_usb_id = 0;
uint32_t pios_com_telem_rf_id = 0;
uint32_t pios_com_rf_id = 0;
uint32_t pios_com_bridge_id = 0;
uint32_t pios_com_overo_id = 0;
uint32_t pios_com_hkosd_id = 0;
@ -231,7 +232,7 @@ uint32_t pios_com_hkosd_id = 0;
uint32_t pios_com_vcp_id = 0;
#if defined(PIOS_INCLUDE_RFM22B)
uint32_t pios_rfm22b_id = 0;
uint32_t pios_rfm22b_id = 0;
#endif
uintptr_t pios_uavo_settings_fs_id;
@ -753,11 +754,15 @@ void PIOS_Board_Init(void)
uint8_t *tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
if (PIOS_COM_Init(&pios_com_rf_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
/* Set Telemetry to use OPLinkMini if no other telemetry is configured (USB always overrides anyway) */
if (!pios_com_telem_rf_id) {
pios_com_telem_rf_id = pios_com_rf_id;
}
oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED;
// Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex.

2
flight/targets/boards/revolution/pios_board.h
View File

@ -141,6 +141,7 @@ extern uint32_t pios_i2c_flexiport_adapter_id;
// -------------------------
#define PIOS_COM_MAX_DEVS 4
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_rf_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_bridge_id;
@ -149,6 +150,7 @@ extern uint32_t pios_com_hkosd_id;
#define PIOS_COM_GPS (pios_com_gps_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_RF (pios_com_rf_id)
#define PIOS_COM_BRIDGE (pios_com_bridge_id)
#define PIOS_COM_VCP (pios_com_vcp_id)
#define PIOS_COM_OSDHK (pios_com_hkosd_id)

2
shared/uavobjectdefinition/systemalarms.xml
View File

@ -45,7 +45,7 @@
</field>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="manual" period="0"/>
<telemetryflight acked="true" updatemode="throttled" period="100"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="manual" period="0"/>
</object>
</xml>