piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

perf: Process CRSF telemetry by frame (#4038)

Browse Source 
Co-authored-by: 3djc <lesitewebdejc@hotmail.com>
Co-authored-by: Peter Feerick <peter.feerick@gmail.com>
This commit is contained in:
Raphael Coeffic
2023-10-19 13:31:30 +10:00
committed by GitHub
co-authored by 3djc Peter Feerick
parent eaabb2ce97
commit c93695eeaa
31 changed files with 418 additions and 169 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/boards/generic_stm32/module_ports.cpp
+2 -2
View File
@@ -30,7 +30,7 @@
#if defined(INTMODULE_USART)
#define INTMODULE_USART_IRQ_PRIORITY 6
#define INTMODULE_USART_IRQ_PRIORITY 5
static const stm32_usart_t intmoduleUSART = {
.USARTx = INTMODULE_USART,
@@ -223,7 +223,7 @@ extern "C" void EXTMODULE_TIMER_IRQHandler()
DEFINE_STM32_SOFTSERIAL_PORT(ExternalModule, extmoduleTimer);
#endif
#define TELEMETRY_USART_IRQ_PRIORITY 5
#define TELEMETRY_USART_IRQ_PRIORITY 0
#define TELEMETRY_DMA_IRQ_PRIORITY 0
static void _set_sport_input(uint8_t enable)
radio/src/hal/module_driver.h
+3
View File
@@ -56,6 +56,9 @@ struct etx_proto_driver_t {
// Process input data byte (telemetry)
void (*processData)(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len);
// Process input data byte (telemetry)
void (*processFrame)(void* ctx, uint8_t* frame, uint8_t flen, uint8_t* buf, uint8_t* len);
// Some module settings may have been modified
void (*onConfigChange)(void* ctx);
radio/src/hal/serial_driver.h
+11 -3
View File
@@ -53,7 +53,10 @@ typedef struct {
struct etx_serial_callbacks_t {
uint8_t (*on_send)(uint8_t* data);
void (*on_receive)(uint8_t data);
void (*on_idle)();
void* on_idle_ctx;
void (*on_idle)(void* ctx);
void (*on_error)();
};
@@ -90,7 +93,12 @@ typedef struct {
// Fetch a byte by its index from the end of the buffer
int (*getLastByte)(void* ctx, uint32_t idx, uint8_t* data);
// Return the number of unread bytes
int (*getBufferedBytes)(void* ctx);
int (*copyRxBuffer)(void* ctx, uint8_t* buf, uint32_t len);
// Clear internal buffer
void (*clearRxBuffer)(void* ctx);
@@ -108,7 +116,7 @@ typedef struct {
// Callbacks
void (*setReceiveCb)(void* ctx, void (*on_receive)(uint8_t*, uint32_t));
void (*setIdleCb)(void* ctx, void (*on_idle)());
void (*setIdleCb)(void* ctx, void (*on_idle)(void*), void* param);
void (*setBaudrateCb)(void* ctx, void (*on_set_baudrate)(uint32_t));
} etx_serial_driver_t;
radio/src/pulses/afhds2.cpp
+2
View File
@@ -93,4 +93,6 @@ const etx_proto_driver_t Afhds2InternalDriver = {
.deinit = afhds2DeInit,
.sendPulses = afhds2SendPulses,
.processData = afhds2ProcessData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
radio/src/pulses/afhds3.cpp
+2
View File
@@ -1165,6 +1165,8 @@ etx_proto_driver_t ProtoDriver = {
.deinit = deinitModule,
.sendPulses = sendPulses,
.processData = processTelemetryData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
} // namespace afhds3
radio/src/pulses/crossfire.cpp
+145 -60
View File
@@ -19,6 +19,11 @@
* GNU General Public License for more details.
*/
#if !defined(SIMU)
#include "stm32_exti_driver.h"
#include "stm32_hal_ll.h"
#endif
#include "opentx.h"
#include "mixer_scheduler.h"
#include "hal/module_driver.h"
@@ -35,6 +40,7 @@
#define CROSSFIRE_CENTER_CH_OFFSET(ch) (0)
#endif
#define MIN_FRAME_LEN 3
uint8_t createCrossfireModelIDFrame(uint8_t moduleIdx, uint8_t * frame)
{
@@ -112,8 +118,8 @@ static void crossfireSetupMixerScheduler(uint8_t module)
static bool _checkFrameCRC(uint8_t* rxBuffer)
{
uint8_t len = rxBuffer[1];
uint8_t crc = crc8(&rxBuffer[2], len-1);
return (crc == rxBuffer[len+1]);
uint8_t crc = crc8(&rxBuffer[2], len - 1);
return (crc == rxBuffer[len + 1]);
}
static void crossfireSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nChannels)
@@ -136,77 +142,90 @@ static void crossfireSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, u
static bool _lenIsSane(uint8_t len)
{
// packet len must be at least 3 bytes (type+payload+crc) and 2 bytes < MAX (hdr+len)
return (len > 2 && len < TELEMETRY_RX_PACKET_SIZE-1);
// packet len must be at least 3 bytes (type + payload + crc)
// and 2 bytes < MAX (hdr + len)
return (len > 2 && len < TELEMETRY_RX_PACKET_SIZE - 1);
}
static void _seekStart(uint8_t* buffer, uint8_t* len)
static void crossfireProcessFrame(void* ctx, uint8_t* frame, uint8_t frame_len,
uint8_t* buf, uint8_t* p_len)
{
// Bad telemetry packets frequently are just truncated packets, with the start
// of a new packet contained in the data. This causes multiple packet drops as
// the parser tries to resync.
// Search through the rxBuffer for a sync byte, shift the contents if found
// and reduce rxBufferCount
for (uint8_t idx = 1; idx < *len; ++idx) {
uint8_t data = buffer[idx];
if (data == RADIO_ADDRESS || data == UART_SYNC) {
uint8_t remain = *len - idx;
// If there's at least 2 bytes, check the length for validity too
if (remain > 1 && !_lenIsSane(buffer[idx + 1])) continue;
if (frame_len < MIN_FRAME_LEN) return;
// TRACE("Found 0x%02x with %u remain", data, remain);
// copy the data to the front of the buffer
for (uint8_t src = idx; src < *len; ++src) {
buffer[src - idx] = buffer[src];
// De-Fragmentation:
// It is assumed here that a continuation chunk
// will not belong to multiple CRSF frames.
uint8_t& len = *p_len;
if (len > 0) {
uint8_t unfrag_len = buf[1] + 2;
uint8_t defrag_len = len + frame_len;
if (defrag_len <= unfrag_len && defrag_len <= TELEMETRY_RX_PACKET_SIZE) {
// If we're not going to overshoot
// the intended frame length,
// let's reassemble it
memcpy(buf + len, frame, frame_len);
len = defrag_len;
frame_len = 0;
// frame complete?
if (defrag_len < unfrag_len) {
TRACE("[XF] frag cont frame (%d < %d)", defrag_len, unfrag_len);
return;
} else {
TRACE("[XF] frame complete");
}
} else {
TRACE("[XF] overshoot (%d > %d)", defrag_len, unfrag_len);
}
}
*len = remain;
if (frame_len > 0) {
memcpy(buf, frame, frame_len);
len = frame_len;
uint8_t unfrag_len = buf[1] + 2;
if (!_lenIsSane(unfrag_len)) {
TRACE("[XF] pkt len error (%d)", unfrag_len);
len = 0;
return;
} // if found sync
}
if (len < unfrag_len) {
TRACE("[XF] frag frame (%d < %d)", len, unfrag_len);
return;
}
}
// Not found, clear the buffer
*len = 0;
}
uint8_t* p_buf = buf;
while (len >= MIN_FRAME_LEN) {
uint8_t pkt_len = p_buf[1] + 2;
if (pkt_len > len) {
TRACE("[XF] length error (%d > %d)", pkt_len, len);
len = 0;
return;
}
static void crossfireProcessData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len)
{
if (*len == 0 && data != RADIO_ADDRESS && data != UART_SYNC) {
TRACE("[XF] address 0x%02X error", data);
return;
}
if (*len == 1 && !_lenIsSane(data)) {
TRACE("[XF] length 0x%02X error", data);
*len = 0;
return;
}
if (*len < TELEMETRY_RX_PACKET_SIZE) {
buffer[(*len)++] = data;
}
else {
TRACE("[XF] array size %d error", *len);
*len = 0;
}
// rxBuffer[1] holds the packet length-2, check if the whole packet was received
while (*len > 4 && (buffer[1]+2) == *len) {
if (_checkFrameCRC(buffer)) {
#if defined(BLUETOOTH) // TODO: generic telemetry mirror to BT
if (p_buf[0] != RADIO_ADDRESS && p_buf[0] != UART_SYNC) {
TRACE("[XF] address 0x%02X error", p_buf[0]);
} else if (!_checkFrameCRC(p_buf)) {
TRACE("[XF] CRC error ");
} else {
#if defined(BLUETOOTH)
// TODO: generic telemetry mirror to BT
if (g_eeGeneral.bluetoothMode == BLUETOOTH_TELEMETRY &&
bluetooth.state == BLUETOOTH_STATE_CONNECTED) {
bluetooth.write(buffer, *len);
bluetooth.write(p_buf, pkt_len);
}
#endif
auto mod_st = (etx_module_state_t*)ctx;
processCrossfireTelemetryFrame(modulePortGetModule(mod_st));
*len = 0;
}
else {
TRACE("[XF] CRC error ");
_seekStart(buffer, len); // adjusts len
auto module = modulePortGetModule(mod_st);
processCrossfireTelemetryFrame(module, p_buf, pkt_len);
}
p_buf += pkt_len;
len -= pkt_len;
}
}
@@ -217,6 +236,31 @@ static const etx_serial_init crsfSerialParams = {
.polarity = ETX_Pol_Normal,
};
#if !defined(SIMU)
#if defined(INTERNAL_MODULE_CRSF)
static void _crsf_intmodule_frame_received(void*)
{
telemetryFrameTrigger_ISR(INTERNAL_MODULE, &CrossfireDriver);
}
#endif
#if defined(HARDWARE_EXTERNAL_MODULE)
static void _crsf_extmodule_frame_received()
{
telemetryFrameTrigger_ISR(EXTERNAL_MODULE, &CrossfireDriver);
}
// proxy trigger to avoid calling
// FreeRTOS methods from ISR with prio 0
static void _soft_irq_trigger(void*)
{
EXTI->SWIER = TELEMETRY_RX_FRAME_EXTI_LINE;
}
#endif
#endif // !SIMU
static void* crossfireInit(uint8_t module)
{
etx_module_state_t* mod_st = nullptr;
@@ -224,17 +268,45 @@ static void* crossfireInit(uint8_t module)
#if defined(INTERNAL_MODULE_CRSF)
if (module == INTERNAL_MODULE) {
params.baudrate = INT_CROSSFIRE_BAUDRATE;
mod_st = modulePortInitSerial(module, ETX_MOD_PORT_UART, &params, false);
if (mod_st) {
auto drv = modulePortGetSerialDrv(mod_st->rx);
auto ctx = modulePortGetCtx(mod_st->rx);
auto& rx_count = getTelemetryRxBufferCount(INTERNAL_MODULE);
rx_count = 0;
#if !defined(SIMU)
if (drv && ctx && drv->setIdleCb) {
drv->setIdleCb(ctx, _crsf_intmodule_frame_received, 0);
}
#endif
}
}
#endif
#if defined(HARDWARE_EXTERNAL_MODULE)
if (module == EXTERNAL_MODULE) {
params.baudrate = EXT_CROSSFIRE_BAUDRATE;
mod_st = modulePortInitSerial(module, ETX_MOD_PORT_SPORT, &params, false);
if (mod_st) {
auto drv = modulePortGetSerialDrv(mod_st->rx);
auto ctx = modulePortGetCtx(mod_st->rx);
auto& rx_count = getTelemetryRxBufferCount(EXTERNAL_MODULE);
rx_count = 0;
#if !defined(SIMU)
if (drv && ctx && drv->setIdleCb) {
drv->setIdleCb(ctx, _soft_irq_trigger, 0);
stm32_exti_enable(TELEMETRY_RX_FRAME_EXTI_LINE, 0,
_crsf_extmodule_frame_received);
}
#endif
}
}
#endif
@@ -248,6 +320,17 @@ static void* crossfireInit(uint8_t module)
static void crossfireDeInit(void* ctx)
{
auto mod_st = (etx_module_state_t*)ctx;
#if !defined(SIMU)
if (mod_st && (modulePortGetModule(mod_st) == EXTERNAL_MODULE)) {
auto drv = modulePortGetSerialDrv(mod_st->rx);
auto ctx = modulePortGetCtx(mod_st->rx);
if (drv && ctx && drv->setIdleCb) {
stm32_exti_disable(TELEMETRY_RX_FRAME_EXTI_LINE);
}
}
#endif
modulePortDeInit(mod_st);
}
@@ -256,5 +339,7 @@ const etx_proto_driver_t CrossfireDriver = {
.init = crossfireInit,
.deinit = crossfireDeInit,
.sendPulses = crossfireSendPulses,
.processData = crossfireProcessData,
.processData = nullptr,
.processFrame = crossfireProcessFrame,
.onConfigChange = nullptr,
};
radio/src/pulses/dsm2.cpp
+2
View File
@@ -296,6 +296,7 @@ const etx_proto_driver_t DSM2Driver = {
.deinit = dsmDeInit,
.sendPulses = dsm2SendPulses,
.processData = nullptr,
.processFrame = nullptr,
.onConfigChange = dsm2ConfigChange,
};
@@ -305,5 +306,6 @@ const etx_proto_driver_t DSMPDriver = {
.deinit = dsmDeInit,
.sendPulses = dsmpSendPulses,
.processData = dsmpProcessData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
radio/src/pulses/ghost.cpp
+2
View File
@@ -252,4 +252,6 @@ const etx_proto_driver_t GhostDriver = {
.deinit = ghostDeInit,
.sendPulses = ghostSendPulses,
.processData = ghostProcessData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
radio/src/pulses/multi.cpp
+2
View File
@@ -286,6 +286,8 @@ const etx_proto_driver_t MultiDriver = {
.deinit = multiDeInit,
.sendPulses = multiSendPulses,
.processData = multiProcessData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
static void sendChannels(uint8_t*& p_buf, uint8_t module)
radio/src/pulses/ppm.cpp
+1
View File
@@ -219,5 +219,6 @@ const etx_proto_driver_t PpmDriver = {
.deinit = ppmDeInit,
.sendPulses = ppmSendPulses,
.processData = ppmProcessTelemetryData,
.processFrame = nullptr,
.onConfigChange = ppmOnConfigChange,
};
radio/src/pulses/pxx1.cpp
+13 -14
View File
@@ -257,26 +257,26 @@ static const etx_serial_init pxx1SerialCfg = {
.polarity = ETX_Pol_Normal,
};
static const etx_serial_driver_t* _sport_drv;
static void* _sport_ctx;
static void pxx1SportSensorPolling()
static void pxx1SportSensorPolling(void* param)
{
if (outputTelemetryBuffer.destination != TELEMETRY_ENDPOINT_SPORT)
return;
auto mod_st = (etx_module_state_t*)param;
auto drv = modulePortGetSerialDrv(mod_st->rx);
auto ctx = modulePortGetCtx(mod_st->rx);
// Match sensor polling from the module
// -> detect <0x7E [Physical ID]> as the last sequence
uint8_t b;
if (_sport_drv->getLastByte(_sport_ctx, 2, &b) <= 0 || b != START_STOP)
return;
if (_sport_drv->getLastByte(_sport_ctx, 1, &b) <= 0 ||
if (drv->getBufferedBytes(ctx) != 2 ||
drv->getLastByte(ctx, 2, &b) <= 0 || b != START_STOP ||
drv->getLastByte(ctx, 1, &b) <= 0 ||
b != outputTelemetryBuffer.sport.physicalId)
return;
_sport_drv->sendBuffer(_sport_ctx,
outputTelemetryBuffer.data + 1,
outputTelemetryBuffer.size - 1);
drv->sendBuffer(ctx, outputTelemetryBuffer.data + 1,
outputTelemetryBuffer.size - 1);
outputTelemetryBuffer.reset();
}
@@ -333,14 +333,11 @@ static void* pxx1Init(uint8_t module)
rxCfg.baudrate = FRSKY_SPORT_BAUDRATE;
rxCfg.direction = ETX_Dir_TX_RX;
// TODO: handle init errors properly
if (modulePortInitSerial(module, ETX_MOD_PORT_SPORT, &rxCfg, false) != nullptr) {
auto drv = modulePortGetSerialDrv(mod_st->rx);
auto ctx = modulePortGetCtx(mod_st->rx);
if (drv && ctx && drv->setIdleCb) {
_sport_drv = drv;
_sport_ctx = ctx;
drv->setIdleCb(ctx, pxx1SportSensorPolling);
drv->setIdleCb(ctx, pxx1SportSensorPolling, mod_st);
}
}
@@ -417,4 +414,6 @@ const etx_proto_driver_t Pxx1Driver = {
.deinit = pxx1DeInit,
.sendPulses = pxx1SendPulses,
.processData = pxx1ProcessData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
radio/src/pulses/pxx2.cpp
+2
View File
@@ -841,4 +841,6 @@ const etx_proto_driver_t Pxx2Driver = {
.deinit = pxx2DeInit,
.sendPulses = pxx2SendPulses,
.processData = pxx2ProcessData,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
radio/src/pulses/sbus.cpp
+2
View File
@@ -154,4 +154,6 @@ const etx_proto_driver_t SBusDriver = {
.deinit = sbusDeInit,
.sendPulses = sbusSendPulses,
.processData = nullptr,
.processFrame = nullptr,
.onConfigChange = nullptr,
};
radio/src/targets/common/arm/stm32/CMakeLists.txt
+6
View File
@@ -28,7 +28,9 @@ set(STM32USB_SRC
set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
../common/arm/stm32/rtc_driver.cpp
../common/arm/stm32/timers_driver.cpp
../common/arm/stm32/delays_driver.cpp
../common/arm/stm32/trainer_driver.cpp
../common/arm/stm32/usb_bsp.c
../common/arm/stm32/usbd_desc.c
../common/arm/stm32/usbd_usr.cpp
@@ -40,6 +42,8 @@ set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
../common/arm/stm32/stm32_i2c_driver.cpp
../common/arm/stm32/stm32_serial_driver.cpp
../common/arm/stm32/stm32_softserial_driver.cpp
../common/arm/stm32/stm32_pulse_driver.cpp
../common/arm/stm32/stm32_usart_driver.cpp
../common/arm/stm32/module_timer_driver.cpp
../common/arm/stm32/stm32_switch_driver.cpp
../common/arm/stm32/stm32_adc.cpp
@@ -47,7 +51,9 @@ set(FIRMWARE_TARGET_SRC ${FIRMWARE_TARGET_SRC}
../common/arm/stm32/stm32_timer.cpp
../common/arm/stm32/stm32_dma.cpp
../common/arm/stm32/stm32_gpio_driver.cpp
../common/arm/stm32/stm32_exti_driver.cpp
../common/arm/stm32/mixer_scheduler_driver.cpp
../common/arm/stm32/pwr_driver.cpp
)
if(IMU)
radio/src/targets/common/arm/stm32/flysky_gimbal_driver.cpp
+2 -2
View File
@@ -119,7 +119,7 @@ static void _fs_parse(STRUCT_HALL *hallBuffer, unsigned char ch)
static volatile bool _fs_gimbal_detected;
static void flysky_gimbal_loop()
static void flysky_gimbal_loop(void*)
{
uint8_t byte;
@@ -164,7 +164,7 @@ bool flysky_gimbal_init()
_fs_gimbal_detected = false;
_fs_usart_ctx = STM32SerialDriver.init(REF_STM32_SERIAL_PORT(FSGimbal), &cfg);
STM32SerialDriver.setIdleCb(_fs_usart_ctx, flysky_gimbal_loop);
STM32SerialDriver.setIdleCb(_fs_usart_ctx, flysky_gimbal_loop, 0);
// Wait 70ms for FlySky gimbals to respond. According to LA trace, minimally 23ms is required
for (uint8_t i = 0; i < 70; i++) {
radio/src/targets/common/arm/stm32/stm32_exti_driver.cpp
+24
View File
@@ -71,6 +71,14 @@
_DEFINE_EXTI_IRQ_HANDLER(EXTI2_IRQ, 2, 2);
#endif
#if defined(USE_EXTI3_IRQ)
_DEFINE_EXTI_IRQ_HANDLER(EXTI3_IRQ, 3, 3);
#endif
#if defined(USE_EXTI4_IRQ)
_DEFINE_EXTI_IRQ_HANDLER(EXTI4_IRQ, 4, 4);
#endif
#if defined(USE_EXTI9_5_IRQ)
_DEFINE_EXTI_IRQ_HANDLER(EXTI9_5_IRQ, 5, 9);
#endif
@@ -110,6 +118,14 @@ void stm32_exti_enable(uint32_t line, uint8_t trigger, stm32_exti_handler_t cb)
_SET_EXTI_IRQ_HANDLER(EXTI2_IRQ, 2, 2, line_pos, cb);
#endif
#if defined(USE_EXTI3_IRQ)
_SET_EXTI_IRQ_HANDLER(EXTI3_IRQ, 3, 3, line_pos, cb);
#endif
#if defined(USE_EXTI4_IRQ)
_SET_EXTI_IRQ_HANDLER(EXTI4_IRQ, 4, 4, line_pos, cb);
#endif
#if defined(USE_EXTI9_5_IRQ)
_SET_EXTI_IRQ_HANDLER(EXTI9_5_IRQ, 5, 9, line_pos, cb);
#endif
@@ -131,6 +147,14 @@ void stm32_exti_disable(uint32_t line)
_CLEAR_EXTI_IRQ_HANDLER(EXTI2_IRQ, 2, 2, line_pos);
#endif
#if defined(USE_EXTI3_IRQ)
_CLEAR_EXTI_IRQ_HANDLER(EXTI3_IRQ, 3, 3, line_pos);
#endif
#if defined(USE_EXTI4_IRQ)
_CLEAR_EXTI_IRQ_HANDLER(EXTI4_IRQ, 4, 4, line_pos);
#endif
#if defined(USE_EXTI9_5_IRQ)
_CLEAR_EXTI_IRQ_HANDLER(EXTI9_5_IRQ, 5, 9, line_pos);
#endif
radio/src/targets/common/arm/stm32/stm32_serial_driver.cpp
+83 -2
View File
@@ -22,6 +22,8 @@
#include "stm32_serial_driver.h"
#include <string.h>
#define MIN(a, b) ((a) < (b) ? (a) : (b))
// Serial buffer state
struct stm32_buffer_state {
volatile uint32_t ridx;
@@ -141,7 +143,7 @@ static inline void _usart_isr_handler(_STM32_USART n)
#endif
#if defined (USART2)
DEFINE_USART_IRQ(USART2);
DEFINE_USART_IRQ(USART2);
#endif
#if defined (USART3)
@@ -416,6 +418,82 @@ static int stm32_serial_get_last_byte(void* ctx, uint32_t idx, uint8_t* data)
return 1;
}
static int stm32_serial_get_buffered_bytes(void* ctx)
{
auto st = (stm32_serial_state*)ctx;
if (!st) return -1;
auto sp = st->sp;
const auto& rx_buf = sp->rx_buffer;
auto buf_len = rx_buf.length;
if (!buf_len) return -1;
uint32_t widx;
auto usart = sp->usart;
const auto& buf_st = st->rx_buf;
if (LL_USART_IsEnabledDMAReq_RX(usart->USARTx)) {
auto dma = usart->rxDMA;
auto stream = usart->rxDMA_Stream;
widx = buf_len - LL_DMA_GetDataLength(dma, stream);
} else {
widx = buf_st.widx;
}
return (widx - buf_st.ridx) & (buf_len - 1);
}
static inline void _copy_buffer_chunk(const stm32_serial_buffer& rx_buf,
stm32_buffer_state& buf_st,
uint8_t* buf, uint32_t len)
{
memcpy(buf, rx_buf.buffer + buf_st.ridx, len);
buf_st.ridx = (buf_st.ridx + len) & (rx_buf.length - 1);
}
static int stm32_serial_copy_rx_buffer(void* ctx, uint8_t* buf, uint32_t len)
{
auto st = (stm32_serial_state*)ctx;
if (!st) return -1;
auto sp = st->sp;
const auto& rx_buf = sp->rx_buffer;
auto buf_len = rx_buf.length;
if (!buf_len) return -1;
uint32_t widx;
auto usart = sp->usart;
auto& buf_st = st->rx_buf;
if (LL_USART_IsEnabledDMAReq_RX(usart->USARTx)) {
auto dma = usart->rxDMA;
auto stream = usart->rxDMA_Stream;
widx = buf_len - LL_DMA_GetDataLength(dma, stream);
} else {
widx = buf_st.widx;
}
if (buf_st.ridx == widx) return 0;
int res = 0;
if (buf_st.ridx > widx) {
auto cp_len = MIN(len, buf_len - buf_st.ridx);
_copy_buffer_chunk(rx_buf, buf_st, buf, cp_len);
buf += cp_len;
len -= cp_len;
res += cp_len;
}
if (buf_st.ridx < widx) {
auto cp_len = MIN(len, widx - buf_st.ridx);
_copy_buffer_chunk(rx_buf, buf_st, buf, cp_len);
len -= cp_len;
res += cp_len;
}
return res;
}
static void stm32_serial_clear_rx_buffer(void* ctx)
{
auto st = (stm32_serial_state*)ctx;
@@ -464,12 +542,13 @@ static void stm32_serial_hw_option(void* ctx, uint32_t option)
stm32_usart_set_hw_option(usart, option);
}
static void stm32_serial_set_idle_cb(void* ctx, void (*on_idle)())
static void stm32_serial_set_idle_cb(void* ctx, void (*on_idle)(void*), void* param)
{
auto st = (stm32_serial_state*)ctx;
if (!st) return;
st->callbacks.on_idle = on_idle;
st->callbacks.on_idle_ctx = param;
uint32_t enabled = (on_idle != nullptr);
stm32_usart_set_idle_irq(st->sp->usart, enabled);
@@ -485,6 +564,8 @@ const etx_serial_driver_t STM32SerialDriver = {
.enableRx = stm32_enable_rx,
.getByte = stm32_serial_get_byte,
.getLastByte = stm32_serial_get_last_byte,
.getBufferedBytes = stm32_serial_get_buffered_bytes,
.copyRxBuffer = stm32_serial_copy_rx_buffer,
.clearRxBuffer = stm32_serial_clear_rx_buffer,
.getBaudrate = stm32_serial_get_baudrate,
.setBaudrate = stm32_serial_set_baudrate,
radio/src/targets/common/arm/stm32/stm32_usart_driver.cpp
+1 -1
View File
@@ -603,7 +603,7 @@ void stm32_usart_isr(const stm32_usart_t* usart, etx_serial_callbacks_t* cb)
if (LL_USART_IsEnabledIT_IDLE(usart->USARTx) && idle) {
// SR clear sequence
status = LL_USART_ReadReg(usart->USARTx, DR);
if (cb->on_idle) cb->on_idle();
if (cb->on_idle) cb->on_idle(cb->on_idle_ctx);
}
}
radio/src/targets/horus/CMakeLists.txt
-7
View File
@@ -261,15 +261,8 @@ set(FIRMWARE_TARGET_SRC
set(FIRMWARE_SRC
${FIRMWARE_SRC}
hal/adc_driver.cpp
targets/common/arm/stm32/stm32_exti_driver.cpp
targets/common/arm/stm32/timers_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp
targets/common/arm/stm32/stm32_usart_driver.cpp
targets/common/arm/stm32/rotary_encoder_driver.cpp
targets/common/arm/stm32/pwr_driver.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/trainer_driver.cpp
targets/common/arm/stm32/ads79xx.cpp
targets/common/arm/stm32/diskio_sdio.cpp
targets/common/arm/stm32/spi_flash.cpp
radio/src/targets/horus/hal.h
+5
View File
@@ -550,6 +550,11 @@
#define TELEMETRY_TIMER_IRQn TIM1_TRG_COM_TIM11_IRQn
#define TELEMETRY_TIMER_IRQHandler TIM1_TRG_COM_TIM11_IRQHandler
// Software IRQ (Prio 5 -> FreeRTOS compatible)
#define TELEMETRY_RX_FRAME_EXTI_LINE LL_EXTI_LINE_4
#define USE_EXTI4_IRQ
#define EXTI4_IRQ_Priority 5
// USB
#define USB_RCC_AHB1Periph_GPIO RCC_AHB1Periph_GPIOA
#define USB_GPIO GPIOA
radio/src/targets/nv14/CMakeLists.txt
-7
View File
@@ -145,13 +145,6 @@ set(FIRMWARE_TARGET_SRC
set(FIRMWARE_SRC
${FIRMWARE_SRC}
hal/adc_driver.cpp
targets/common/arm/stm32/stm32_exti_driver.cpp
targets/common/arm/stm32/timers_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp
targets/common/arm/stm32/stm32_usart_driver.cpp
targets/common/arm/stm32/trainer_driver.cpp
targets/common/arm/stm32/pwr_driver.cpp
targets/common/arm/stm32/diskio_sdio.cpp
targets/common/arm/stm32/spi_flash.cpp
targets/common/arm/stm32/diskio_spi_flash.cpp
radio/src/targets/nv14/hal.h
+6
View File
@@ -246,6 +246,12 @@
#define TELEMETRY_TX_POL_INV() TELEMETRY_REV_GPIO->BSRRL = TELEMETRY_TX_REV_GPIO_PIN
#define TELEMETRY_RX_POL_NORM() TELEMETRY_REV_GPIO->BSRRH = TELEMETRY_RX_REV_GPIO_PIN
#define TELEMETRY_RX_POL_INV() TELEMETRY_REV_GPIO->BSRRL = TELEMETRY_RX_REV_GPIO_PIN
// Software IRQ (Prio 5 -> FreeRTOS compatible)
#define TELEMETRY_RX_FRAME_EXTI_LINE LL_EXTI_LINE_4
#define USE_EXTI4_IRQ
#define EXTI4_IRQ_Priority 5
// USB
#define USB_RCC_AHB1Periph_GPIO RCC_AHB1Periph_GPIOA
#define USB_GPIO GPIOA
radio/src/targets/simu/module_drivers.cpp
+2
View File
@@ -102,6 +102,8 @@ const etx_serial_driver_t _fakeSerialDriver = {
.enableRx = nullptr,
.getByte = getByte,
.getLastByte = nullptr,
.getBufferedBytes = nullptr,
.copyRxBuffer = nullptr,
.clearRxBuffer = nullptr,
.getBaudrate = nullptr,
.setBaudrate = nullptr,
radio/src/targets/simu/simpgmspace.cpp
+2
View File
@@ -613,6 +613,8 @@ static const etx_serial_driver_t _fake_drv = {
.enableRx = nullptr,
.getByte = _fake_drv_get_byte,
.getLastByte = nullptr,
.getBufferedBytes = nullptr,
.copyRxBuffer = nullptr,
.clearRxBuffer = nullptr,
.getBaudrate = nullptr,
.setBaudrate = nullptr,
radio/src/targets/taranis/CMakeLists.txt
-6
View File
@@ -589,13 +589,7 @@ set(FIRMWARE_SRC
hal/adc_driver.cpp
targets/common/arm/loadboot.cpp
targets/common/arm/stm32/heartbeat_driver.cpp
targets/common/arm/stm32/timers_driver.cpp
targets/common/arm/stm32/audio_dac_driver.cpp
targets/common/arm/stm32/pwr_driver.cpp
targets/common/arm/stm32/stm32_exti_driver.cpp
targets/common/arm/stm32/stm32_pulse_driver.cpp
targets/common/arm/stm32/stm32_usart_driver.cpp
targets/common/arm/stm32/trainer_driver.cpp
targets/common/arm/stm32/sdcard_spi.cpp
targets/common/arm/stm32/diskio_spi.cpp
)
radio/src/targets/taranis/hal.h
+5
View File
@@ -2153,6 +2153,11 @@
#define TELEMETRY_TIMER_IRQn TIM1_TRG_COM_TIM11_IRQn
#define TELEMETRY_TIMER_IRQHandler TIM1_TRG_COM_TIM11_IRQHandler
// Software IRQ (Prio 5 -> FreeRTOS compatible)
#define TELEMETRY_RX_FRAME_EXTI_LINE LL_EXTI_LINE_4
#define USE_EXTI4_IRQ
#define EXTI4_IRQ_Priority 5
// PCBREV
#if defined(RADIO_X7)
#define PCBREV_RCC_AHB1Periph RCC_AHB1Periph_GPIOA
radio/src/targets/taranis/lcd_driver_spi.cpp
+1
View File
@@ -110,6 +110,7 @@ void lcdHardwareInit()
#endif
LCD_DMA_Stream->FCR = 0x05; // DMA_SxFCR_DMDIS | DMA_SxFCR_FTH_0;
NVIC_SetPriority(LCD_DMA_Stream_IRQn, 7);
NVIC_EnableIRQ(LCD_DMA_Stream_IRQn);
}
radio/src/telemetry/crossfire.cpp
+33 -34
View File
@@ -94,10 +94,10 @@ void processCrossfireTelemetryValue(uint8_t index, int32_t value)
value, sensor.unit, sensor.precision);
}
template<int N>
bool getCrossfireTelemetryValue(uint8_t index, int32_t & value, uint8_t module)
template <int N>
bool getCrossfireTelemetryValue(uint8_t index, int32_t& value,
uint8_t* rxBuffer)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(module);
bool result = false;
uint8_t * byte = &rxBuffer[index];
value = (*byte & 0x80) ? -1 : 0;
@@ -111,11 +111,9 @@ bool getCrossfireTelemetryValue(uint8_t index, int32_t & value, uint8_t module)
return result;
}
void processCrossfireTelemetryFrame(uint8_t module)
void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
uint8_t rxBufferCount)
{
uint8_t * rxBuffer = getTelemetryRxBuffer(module);
uint8_t &rxBufferCount = getTelemetryRxBufferCount(module);
if (telemetryState == TELEMETRY_INIT &&
moduleState[module].counter != CRSF_FRAME_MODELID_SENT) {
moduleState[module].counter = CRSF_FRAME_MODELID;
@@ -126,27 +124,27 @@ void processCrossfireTelemetryFrame(uint8_t module)
int32_t value;
switch(id) {
case CF_VARIO_ID:
if (getCrossfireTelemetryValue<2>(3, value, module))
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer))
processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
break;
case GPS_ID:
if (getCrossfireTelemetryValue<4>(3, value, module))
if (getCrossfireTelemetryValue<4>(3, value, rxBuffer))
processCrossfireTelemetryValue(GPS_LATITUDE_INDEX, value/10);
if (getCrossfireTelemetryValue<4>(7, value, module))
if (getCrossfireTelemetryValue<4>(7, value, rxBuffer))
processCrossfireTelemetryValue(GPS_LONGITUDE_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(11, value, module))
if (getCrossfireTelemetryValue<2>(11, value, rxBuffer))
processCrossfireTelemetryValue(GPS_GROUND_SPEED_INDEX, value);
if (getCrossfireTelemetryValue<2>(13, value, module))
if (getCrossfireTelemetryValue<2>(13, value, rxBuffer))
processCrossfireTelemetryValue(GPS_HEADING_INDEX, value);
if (getCrossfireTelemetryValue<2>(15, value, module))
if (getCrossfireTelemetryValue<2>(15, value, rxBuffer))
processCrossfireTelemetryValue(GPS_ALTITUDE_INDEX, value - 1000);
if (getCrossfireTelemetryValue<1>(17, value, module))
if (getCrossfireTelemetryValue<1>(17, value, rxBuffer))
processCrossfireTelemetryValue(GPS_SATELLITES_INDEX, value);
break;
case BARO_ALT_ID:
if (getCrossfireTelemetryValue<2>(3, value, module)) {
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer)) {
if (value & 0x8000) {
// Altitude in meters
value &= ~(0x8000);
@@ -160,13 +158,14 @@ void processCrossfireTelemetryFrame(uint8_t module)
}
// Length of TBS BARO_ALT has 4 payload bytes with just 2 bytes of altitude
// but support including VARIO if the declared payload length is 6 bytes or more
if (crsfPayloadLen > 5 && getCrossfireTelemetryValue<2>(5, value, module))
if (crsfPayloadLen > 5 &&
getCrossfireTelemetryValue<2>(5, value, rxBuffer))
processCrossfireTelemetryValue(VERTICAL_SPEED_INDEX, value);
break;
case LINK_ID:
for (unsigned int i=0; i<=TX_SNR_INDEX; i++) {
if (getCrossfireTelemetryValue<1>(3+i, value, module)) {
if (getCrossfireTelemetryValue<1>(3+i, value, rxBuffer)) {
if (i == TX_POWER_INDEX) {
static const int32_t power_values[] = {0, 10, 25, 100, 500,
1000, 2000, 250, 50};
@@ -193,38 +192,38 @@ void processCrossfireTelemetryFrame(uint8_t module)
break;
case LINK_RX_ID:
if (getCrossfireTelemetryValue<1>(4, value, module))
if (getCrossfireTelemetryValue<1>(4, value, rxBuffer))
processCrossfireTelemetryValue(RX_RSSI_PERC_INDEX, value);
if (getCrossfireTelemetryValue<1>(7, value, module))
if (getCrossfireTelemetryValue<1>(7, value, rxBuffer))
processCrossfireTelemetryValue(TX_RF_POWER_INDEX, value);
break;
case LINK_TX_ID:
if (getCrossfireTelemetryValue<1>(4, value, module))
if (getCrossfireTelemetryValue<1>(4, value, rxBuffer))
processCrossfireTelemetryValue(TX_RSSI_PERC_INDEX, value);
if (getCrossfireTelemetryValue<1>(7, value, module))
if (getCrossfireTelemetryValue<1>(7, value, rxBuffer))
processCrossfireTelemetryValue(RX_RF_POWER_INDEX, value);
if (getCrossfireTelemetryValue<1>(8, value, module))
if (getCrossfireTelemetryValue<1>(8, value, rxBuffer))
processCrossfireTelemetryValue(TX_FPS_INDEX, value * 10);
break;
case BATTERY_ID:
if (getCrossfireTelemetryValue<2>(3, value, module))
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer))
processCrossfireTelemetryValue(BATT_VOLTAGE_INDEX, value);
if (getCrossfireTelemetryValue<2>(5, value, module))
if (getCrossfireTelemetryValue<2>(5, value, rxBuffer))
processCrossfireTelemetryValue(BATT_CURRENT_INDEX, value);
if (getCrossfireTelemetryValue<3>(7, value, module))
if (getCrossfireTelemetryValue<3>(7, value, rxBuffer))
processCrossfireTelemetryValue(BATT_CAPACITY_INDEX, value);
if (getCrossfireTelemetryValue<1>(10, value, module))
if (getCrossfireTelemetryValue<1>(10, value, rxBuffer))
processCrossfireTelemetryValue(BATT_REMAINING_INDEX, value);
break;
case ATTITUDE_ID:
if (getCrossfireTelemetryValue<2>(3, value, module))
if (getCrossfireTelemetryValue<2>(3, value, rxBuffer))
processCrossfireTelemetryValue(ATTITUDE_PITCH_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(5, value, module))
if (getCrossfireTelemetryValue<2>(5, value, rxBuffer))
processCrossfireTelemetryValue(ATTITUDE_ROLL_INDEX, value/10);
if (getCrossfireTelemetryValue<2>(7, value, module))
if (getCrossfireTelemetryValue<2>(7, value, rxBuffer))
processCrossfireTelemetryValue(ATTITUDE_YAW_INDEX, value/10);
break;
@@ -245,13 +244,13 @@ void processCrossfireTelemetryFrame(uint8_t module)
uint32_t update_interval;
int32_t offset;
if (getCrossfireTelemetryValue<4>(6, (int32_t &)update_interval,
module) &&
getCrossfireTelemetryValue<4>(10, offset, module)) {
rxBuffer) &&
getCrossfireTelemetryValue<4>(10, offset, rxBuffer)) {
// values are in 10th of micro-seconds
update_interval /= 10;
offset /= 10;
TRACE("[XF] Rate: %d, Lag: %d", update_interval, offset);
//TRACE("[XF] Rate: %d, Lag: %d", update_interval, offset);
getModuleSyncStatus(module).update(update_interval, offset);
}
}
@@ -259,8 +258,8 @@ void processCrossfireTelemetryFrame(uint8_t module)
#if defined(LUA)
default:
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(rxBufferCount-2) ) {
for (uint8_t i=1; i<rxBufferCount-1; i++) {
if (luaInputTelemetryFifo && luaInputTelemetryFifo->hasSpace(rxBufferCount - 2)) {
for (uint8_t i = 1; i < rxBufferCount - 1; i++) {
// destination address and CRC are skipped
luaInputTelemetryFifo->push(rxBuffer[i]);
}
radio/src/telemetry/crossfire.h
+4 -2
View File
@@ -100,9 +100,11 @@ enum CrossfireFrames{
CRSF_FRAME_MODELID_SENT
};
void processCrossfireTelemetryFrame(uint8_t module);
void processCrossfireTelemetryFrame(uint8_t module, uint8_t* rxBuffer,
uint8_t rxBufferCount);
void crossfireSetDefault(int index, uint8_t id, uint8_t subId);
uint8_t createCrossfireModelIDFrame(uint8_t * frame);
uint8_t createCrossfireModelIDFrame(uint8_t* frame);
const uint32_t CROSSFIRE_BAUDRATES[] = {
115200,
radio/src/telemetry/telemetry.cpp
+49 -29
View File
@@ -153,30 +153,10 @@ rxStatStruct *getRxStatLabels() {
return &rxStat;
}
// TODO: move to module port driver
//
// static int (*_telemetryGetByte)(void*, uint8_t*) = nullptr;
// static void* _telemetryGetByteCtx = nullptr;
// void telemetrySetGetByte(void* ctx, int (*fct)(void*, uint8_t*))
// {
// _telemetryGetByte = nullptr;
// _telemetryGetByteCtx = ctx;
// _telemetryGetByte = fct;
// }
// static bool telemetryGetByte(uint8_t* data)
// {
// auto _getByte = _telemetryGetByte;
// auto _ctx = _telemetryGetByteCtx;
// if (_getByte) {
// return _getByte(_ctx, data);
// }
// // return sportGetByte(data);
// return false;
// }
// This can only be changed when the mixer is not
// running as the priority of the timer task is
// lower.
volatile uint8_t _telemetryIsPolling = false;
static void (*telemetryMirrorSendByte)(void*, uint8_t) = nullptr;
static void* telemetryMirrorSendByteCtx = nullptr;
@@ -234,6 +214,51 @@ void telemetryStop()
}
}
}
static void _poll_frame(void *pvParameter1, uint32_t ulParameter2)
{
_telemetryIsPolling = true;
auto drv = (const etx_proto_driver_t*)pvParameter1;
auto module = (uint8_t)ulParameter2;
auto mod = pulsesGetModuleDriver(module);
if (!mod || !mod->drv || !mod->ctx || (drv != mod->drv))
return;
auto ctx = mod->ctx;
auto mod_st = (etx_module_state_t*)ctx;
auto serial_drv = modulePortGetSerialDrv(mod_st->rx);
auto serial_ctx = modulePortGetCtx(mod_st->rx);
if (!serial_drv || !serial_ctx || !serial_drv->copyRxBuffer)
return;
uint8_t frame[TELEMETRY_RX_PACKET_SIZE];
int frame_len = serial_drv->copyRxBuffer(serial_ctx, frame, TELEMETRY_RX_PACKET_SIZE);
if (frame_len > 0) {
LOG_TELEMETRY_WRITE_START();
for (int i = 0; i < frame_len; i++) {
telemetryMirrorSend(frame[i]);
LOG_TELEMETRY_WRITE_BYTE(frame[i]);
}
uint8_t* rxBuffer = getTelemetryRxBuffer(module);
uint8_t& rxBufferCount = getTelemetryRxBufferCount(module);
drv->processFrame(ctx, frame, frame_len, rxBuffer, &rxBufferCount);
}
_telemetryIsPolling = false;
}
void telemetryFrameTrigger_ISR(uint8_t module, const etx_proto_driver_t* drv)
{
BaseType_t xHigherPriorityTaskWoken = pdFALSE;
xTimerPendFunctionCallFromISR(_poll_frame, (void*)drv, module, &xHigherPriorityTaskWoken);
portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}
#endif
inline bool isBadAntennaDetected()
@@ -277,11 +302,6 @@ static inline void pollTelemetry(uint8_t module, const etx_proto_driver_t* drv,
}
}
// This can only be changed when the mixer is not
// running as the priority of the timer task is
// lower.
volatile uint8_t _telemetryIsPolling = false;
void telemetryWakeup()
{
_telemetryIsPolling = true;
radio/src/telemetry/telemetry.h
+6
View File
@@ -81,6 +81,12 @@ void telemetryInterrupt10ms();
void telemetryStart();
void telemetryStop();
struct etx_proto_driver_t;
// Call from ISR to schedule telemetry frame
// processing for that module.
void telemetryFrameTrigger_ISR(uint8_t module, const etx_proto_driver_t* drv);
#define TELEMETRY_AVERAGE_COUNT 3
enum {