mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
OP-1290 fixes in telemetry (and pios_board initialization)
This commit is contained in:
Corvus Corax
parent
8bb1d0148c
commit
bb91039f51
@ -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
|
||||
*/
|
||||
static uint32_t getComPort(bool input)
|
||||
// #ifdef PIOS_INCLUDE_RFM22B
|
||||
// static uint32_t getComPort( bool input)
|
||||
// #else
|
||||
static uint32_t getComPort(__attribute__((unused)) bool input)
|
||||
// #endif
|
||||
{
|
||||
#ifndef PIOS_INCLUDE_RFM22B
|
||||
input = false;
|
||||
#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;
|
||||
|
||||
@ -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.
|
||||
|
||||
@ -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)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user