diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 6aef8f14f31d..fb480b10f1b9 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -530,7 +530,7 @@ CONFIG_BOARD_LOOPSPERMSEC=2000 # Interrupt options # CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=500 +CONFIG_ARCH_INTERRUPTSTACK=0 CONFIG_ARCH_HAVE_HIPRI_INTERRUPT=y # CONFIG_ARCH_HIPRI_INTERRUPT is not set diff --git a/src/lib/rc/CMakeLists.txt b/src/lib/rc/CMakeLists.txt index bb39f92c3f1e..21059b9ac452 100644 --- a/src/lib/rc/CMakeLists.txt +++ b/src/lib/rc/CMakeLists.txt @@ -35,10 +35,11 @@ px4_add_module( COMPILE_FLAGS -Wno-unused-result SRCS - st24.c - sumd.c - sbus.c - dsm.c + st24.cpp + sumd.cpp + sbus.cpp + dsm.cpp + common_rc.cpp DEPENDS platforms__common ) diff --git a/src/lib/rc/common_rc.cpp b/src/lib/rc/common_rc.cpp new file mode 100644 index 000000000000..92d8c80c8279 --- /dev/null +++ b/src/lib/rc/common_rc.cpp @@ -0,0 +1,4 @@ + +#include "common_rc.h" + +__EXPORT uint8_t __rc_decode_buf[RC_LIB_MAX_BUFFER_SIZE]; diff --git a/src/lib/rc/common_rc.h b/src/lib/rc/common_rc.h new file mode 100644 index 000000000000..a0be38d3a827 --- /dev/null +++ b/src/lib/rc/common_rc.h @@ -0,0 +1,8 @@ + +#pragma once + +#include + +#define RC_LIB_MAX_BUFFER_SIZE 80 + +extern uint8_t __rc_decode_buf[RC_LIB_MAX_BUFFER_SIZE]; diff --git a/src/lib/rc/dsm.c b/src/lib/rc/dsm.cpp similarity index 97% rename from src/lib/rc/dsm.c rename to src/lib/rc/dsm.cpp index d87711184f2d..137a9a44d65c 100644 --- a/src/lib/rc/dsm.c +++ b/src/lib/rc/dsm.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file dsm.c + * @file dsm.cpp * * Serial protocol decoder for the Spektrum DSM* family of protocols. * @@ -49,6 +49,7 @@ #include #include "dsm.h" +#include "common_rc.h" #include #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) || defined(__PX4_QURT) @@ -68,8 +69,13 @@ static enum DSM_DECODE_STATE { static int dsm_fd = -1; /**< File handle to the DSM UART */ static hrt_abstime dsm_last_rx_time; /**< Timestamp when we last received data */ static hrt_abstime dsm_last_frame_time; /**< Timestamp for start of last valid dsm frame */ -static uint8_t dsm_frame[DSM_BUFFER_SIZE]; /**< DSM dsm frame receive buffer */ -static uint8_t dsm_buf[DSM_FRAME_SIZE * 2]; +static uint8_t *dsm_frame = &__rc_decode_buf[0]; /**< DSM_BUFFER_SIZE DSM dsm frame receive buffer */ +static uint8_t *dsm_buf = &__rc_decode_buf[DSM_BUFFER_SIZE]; //[DSM_FRAME_SIZE * 2]; + +// Ensure there is enough space +static_assert(sizeof(__rc_decode_buf) > (DSM_BUFFER_SIZE + DSM_FRAME_SIZE * 2), + "__rc_decode_buf is too small for DSM protocol"); + static uint16_t dsm_chan_buf[DSM_MAX_CHANNEL_COUNT]; static unsigned dsm_partial_frame_count; /**< Count of bytes received for current dsm frame */ static unsigned dsm_channel_shift = 0; /**< Channel resolution, 0=unknown, 1=10 bit, 2=11 bit */ diff --git a/src/lib/rc/sbus.c b/src/lib/rc/sbus.cpp similarity index 98% rename from src/lib/rc/sbus.c rename to src/lib/rc/sbus.cpp index 4f0475165cd8..459f746f5a6b 100644 --- a/src/lib/rc/sbus.c +++ b/src/lib/rc/sbus.cpp @@ -49,6 +49,7 @@ #endif #include "sbus.h" +#include "common_rc.h" #include #define SBUS_DEBUG_LEVEL 0 /* Set debug output level */ @@ -116,7 +117,11 @@ static enum SBUS2_DECODE_STATE { SBUS2_DECODE_STATE_SBUS2_DATA2 = 0x34 } sbus_decode_state = SBUS2_DECODE_STATE_DESYNC; -static uint8_t sbus_frame[SBUS_FRAME_SIZE + (SBUS_FRAME_SIZE / 2)]; +static uint8_t *sbus_frame = &__rc_decode_buf[0]; + +// Ensure there is enough space +static_assert(sizeof(__rc_decode_buf) > SBUS_BUFFER_SIZE, + "__rc_decode_buf is too small for SBUS protocol"); static unsigned partial_frame_count; static unsigned sbus1_frame_delay = (1000U * 1000U) / SBUS1_DEFAULT_RATE_HZ; diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.cpp similarity index 88% rename from src/lib/rc/st24.c rename to src/lib/rc/st24.cpp index d1f74e04dec4..feb15fc58616 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.cpp @@ -42,6 +42,7 @@ #include #include #include "st24.h" +#include "common_rc.h" enum ST24_DECODE_STATE { ST24_DECODE_STATE_UNSYNCED = 0, @@ -74,7 +75,11 @@ const char *decode_states[] = {"UNSYNCED", static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; static uint8_t _rxlen; -static ReceiverFcPacket _rxpacket; +static ReceiverFcPacket *_rxpacket = (ReceiverFcPacket *) &__rc_decode_buf[0]; + +// Ensure there is enough space +static_assert(sizeof(__rc_decode_buf) > sizeof(ReceiverFcPacket), + "__rc_decode_buf is too small for ST protocol"); uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len) { @@ -132,8 +137,8 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan case ST24_DECODE_STATE_GOT_STX2: /* ensure no data overflow failure or hack is possible */ - if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) { - _rxpacket.length = byte; + if ((unsigned)byte <= sizeof(_rxpacket->length) + sizeof(_rxpacket->type) + sizeof(_rxpacket->st24_data)) { + _rxpacket->length = byte; _rxlen = 0; _decode_state = ST24_DECODE_STATE_GOT_LEN; @@ -144,35 +149,35 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan break; case ST24_DECODE_STATE_GOT_LEN: - _rxpacket.type = byte; + _rxpacket->type = byte; _rxlen++; _decode_state = ST24_DECODE_STATE_GOT_TYPE; break; case ST24_DECODE_STATE_GOT_TYPE: - _rxpacket.st24_data[_rxlen - 1] = byte; + _rxpacket->st24_data[_rxlen - 1] = byte; _rxlen++; - if (_rxlen == (_rxpacket.length - 1)) { + if (_rxlen == (_rxpacket->length - 1)) { _decode_state = ST24_DECODE_STATE_GOT_DATA; } break; case ST24_DECODE_STATE_GOT_DATA: - _rxpacket.crc8 = byte; + _rxpacket->crc8 = byte; _rxlen++; - if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) { + if (st24_common_crc8((uint8_t *) & (_rxpacket->length), _rxlen) == _rxpacket->crc8) { ret = 0; /* decode the actual packet */ - switch (_rxpacket.type) { + switch (_rxpacket->type) { case ST24_PACKET_TYPE_CHANNELDATA12: { - ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data; + ChannelData12 *d = (ChannelData12 *)_rxpacket->st24_data; // Scale from 0..255 to 100%. *rssi = d->rssi * (100.0f / 255.0f); @@ -201,7 +206,7 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan break; case ST24_PACKET_TYPE_CHANNELDATA24: { - ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data; + ChannelData24 *d = (ChannelData24 *)&_rxpacket->st24_data; // Scale from 0..255 to 100%. *rssi = d->rssi * (100.0f / 255.0f); @@ -231,7 +236,7 @@ int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *lost_count, uint16_t *chan case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: { - // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data; + // ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket->st24_data; /* we silently ignore this data for now, as it is unused */ ret = 5; } diff --git a/src/lib/rc/sumd.c b/src/lib/rc/sumd.cpp similarity index 80% rename from src/lib/rc/sumd.c rename to src/lib/rc/sumd.cpp index 0c829cee2ca7..091238096045 100644 --- a/src/lib/rc/sumd.c +++ b/src/lib/rc/sumd.cpp @@ -42,7 +42,7 @@ #include #include #include "sumd.h" - +#include "common_rc.h" enum SUMD_DECODE_STATE { SUMD_DECODE_STATE_UNSYNCED = 0, @@ -88,8 +88,11 @@ bool _debug = false; static enum SUMD_DECODE_STATE _decode_state = SUMD_DECODE_STATE_UNSYNCED; static uint8_t _rxlen; -static ReceiverFcPacketHoTT _rxpacket; +static ReceiverFcPacketHoTT *_rxpacket = (ReceiverFcPacketHoTT *) &__rc_decode_buf[0]; +// Ensure there is enough space +static_assert(sizeof(__rc_decode_buf) > sizeof(ReceiverFcPacketHoTT), + "__rc_decode_buf is too small for SUMD protocol"); uint16_t sumd_crc16(uint16_t crc, uint8_t value) { @@ -122,7 +125,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe } if (byte == SUMD_HEADER_ID) { - _rxpacket.header = byte; + _rxpacket->header = byte; _sumd = true; _rxlen = 0; _crc16 = 0x0000; @@ -144,7 +147,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_HEADER: if (byte == SUMD_ID_SUMD || byte == SUMD_ID_FAILSAFE || byte == SUMD_ID_SUMH) { - _rxpacket.status = byte; + _rxpacket->status = byte; if (byte == SUMD_ID_SUMH) { _sumd = false; @@ -171,7 +174,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_STATE: if (byte >= 2 && byte <= SUMD_MAX_CHANNELS) { - _rxpacket.length = byte; + _rxpacket->length = byte; if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); @@ -194,7 +197,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe break; case SUMD_DECODE_STATE_GOT_LEN: - _rxpacket.sumd_data[_rxlen] = byte; + _rxpacket->sumd_data[_rxlen] = byte; if (_sumd) { _crc16 = sumd_crc16(_crc16, byte); @@ -205,7 +208,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe _rxlen++; - if (_rxlen <= ((_rxpacket.length * 2))) { + if (_rxlen <= ((_rxpacket->length * 2))) { if (_debug) { printf(" SUMD_DECODE_STATE_GOT_DATA[%d]: %x\n", _rxlen - 2, byte) ; } @@ -222,7 +225,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe break; case SUMD_DECODE_STATE_GOT_DATA: - _rxpacket.crc16_high = byte; + _rxpacket->crc16_high = byte; if (_debug) { printf(" SUMD_DECODE_STATE_GOT_CRC16[1]: %x [%x]\n", byte, ((_crc16 >> 8) & 0xff)) ; @@ -238,7 +241,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe break; case SUMD_DECODE_STATE_GOT_CRC16_BYTE_1: - _rxpacket.crc16_low = byte; + _rxpacket->crc16_low = byte; if (_debug) { printf(" SUMD_DECODE_STATE_GOT_CRC16[2]: %x [%x]\n", byte, (_crc16 & 0xff)) ; @@ -249,7 +252,7 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe break; case SUMD_DECODE_STATE_GOT_CRC16_BYTE_2: - _rxpacket.telemetry = byte; + _rxpacket->telemetry = byte; if (_debug) { printf(" SUMD_DECODE_STATE_GOT_SUMH_TELEMETRY: %x\n", byte) ; @@ -261,24 +264,24 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe case SUMD_DECODE_STATE_GOT_CRC: if (_sumd) { - _rxpacket.crc16_low = byte; + _rxpacket->crc16_low = byte; if (_debug) { printf(" SUMD_DECODE_STATE_GOT_CRC[2]: %x [%x]\n\n", byte, (_crc16 & 0xff)) ; } - if (_crc16 == (uint16_t)(_rxpacket.crc16_high << 8) + _rxpacket.crc16_low) { + if (_crc16 == (uint16_t)(_rxpacket->crc16_high << 8) + _rxpacket->crc16_low) { _crcOK = true; } } else { - _rxpacket.crc8 = byte; + _rxpacket->crc8 = byte; if (_debug) { printf(" SUMD_DECODE_STATE_GOT_CRC8_SUMH: %x [%x]\n\n", byte, _crc8) ; } - if (_crc8 == _rxpacket.crc8) { + if (_crc8 == _rxpacket->crc8) { _crcOK = true; } } @@ -312,38 +315,38 @@ int sumd_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channe *rssi = 100; /* failsafe flag */ - *failsafe = (_rxpacket.status == SUMD_ID_FAILSAFE); + *failsafe = (_rxpacket->status == SUMD_ID_FAILSAFE); /* received Channels */ - if ((uint16_t)_rxpacket.length > max_chan_count) { - _rxpacket.length = (uint8_t) max_chan_count; + if ((uint16_t)_rxpacket->length > max_chan_count) { + _rxpacket->length = (uint8_t) max_chan_count; } - *channel_count = (uint16_t)_rxpacket.length; + *channel_count = (uint16_t)_rxpacket->length; /* decode the actual packet */ /* reorder first 4 channels */ /* ch1 = roll -> sumd = ch2 */ - channels[0] = (uint16_t)((_rxpacket.sumd_data[1 * 2 + 1] << 8) | _rxpacket.sumd_data[1 * 2 + 2]) >> 3; + channels[0] = (uint16_t)((_rxpacket->sumd_data[1 * 2 + 1] << 8) | _rxpacket->sumd_data[1 * 2 + 2]) >> 3; /* ch2 = pitch -> sumd = ch2 */ - channels[1] = (uint16_t)((_rxpacket.sumd_data[2 * 2 + 1] << 8) | _rxpacket.sumd_data[2 * 2 + 2]) >> 3; + channels[1] = (uint16_t)((_rxpacket->sumd_data[2 * 2 + 1] << 8) | _rxpacket->sumd_data[2 * 2 + 2]) >> 3; /* ch3 = throttle -> sumd = ch2 */ - channels[2] = (uint16_t)((_rxpacket.sumd_data[0 * 2 + 1] << 8) | _rxpacket.sumd_data[0 * 2 + 2]) >> 3; + channels[2] = (uint16_t)((_rxpacket->sumd_data[0 * 2 + 1] << 8) | _rxpacket->sumd_data[0 * 2 + 2]) >> 3; /* ch4 = yaw -> sumd = ch2 */ - channels[3] = (uint16_t)((_rxpacket.sumd_data[3 * 2 + 1] << 8) | _rxpacket.sumd_data[3 * 2 + 2]) >> 3; + channels[3] = (uint16_t)((_rxpacket->sumd_data[3 * 2 + 1] << 8) | _rxpacket->sumd_data[3 * 2 + 2]) >> 3; /* we start at channel 5(index 4) */ unsigned chan_index = 4; - for (i = 4; i < _rxpacket.length; i++) { + for (i = 4; i < _rxpacket->length; i++) { if (_debug) { - printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket.sumd_data[i * 2 + 1], _rxpacket.sumd_data[i * 2 + 2], - ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3, - ((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3); + printf("ch[%d] : %x %x [ %x %d ]\n", i + 1, _rxpacket->sumd_data[i * 2 + 1], _rxpacket->sumd_data[i * 2 + 2], + ((_rxpacket->sumd_data[i * 2 + 1] << 8) | _rxpacket->sumd_data[i * 2 + 2]) >> 3, + ((_rxpacket->sumd_data[i * 2 + 1] << 8) | _rxpacket->sumd_data[i * 2 + 2]) >> 3); } - channels[chan_index] = (uint16_t)((_rxpacket.sumd_data[i * 2 + 1] << 8) | _rxpacket.sumd_data[i * 2 + 2]) >> 3; + channels[chan_index] = (uint16_t)((_rxpacket->sumd_data[i * 2 + 1] << 8) | _rxpacket->sumd_data[i * 2 + 2]) >> 3; /* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */ //channels[chan_index] = (uint16_t)(channels[chan_index] * SUMD_SCALE_FACTOR + .5f) + SUMD_SCALE_OFFSET; diff --git a/src/modules/px4iofirmware/CMakeLists.txt b/src/modules/px4iofirmware/CMakeLists.txt index 095ebc16f248..b29d5b1f8668 100644 --- a/src/modules/px4iofirmware/CMakeLists.txt +++ b/src/modules/px4iofirmware/CMakeLists.txt @@ -78,10 +78,11 @@ set(srcs ${PX4_SOURCE_DIR}/src/drivers/stm32/drv_hrt.c ${PX4_SOURCE_DIR}/src/drivers/stm32/drv_io_timer.c ${PX4_SOURCE_DIR}/src/drivers/stm32/drv_pwm_servo.c - ${PX4_SOURCE_DIR}/src/lib/rc/dsm.c - ${PX4_SOURCE_DIR}/src/lib/rc/sbus.c - ${PX4_SOURCE_DIR}/src/lib/rc/st24.c - ${PX4_SOURCE_DIR}/src/lib/rc/sumd.c + ${PX4_SOURCE_DIR}/src/lib/rc/dsm.cpp + ${PX4_SOURCE_DIR}/src/lib/rc/sbus.cpp + ${PX4_SOURCE_DIR}/src/lib/rc/st24.cpp + ${PX4_SOURCE_DIR}/src/lib/rc/sumd.cpp + ${PX4_SOURCE_DIR}/src/lib/rc/common_rc.cpp ${PX4_SOURCE_DIR}/src/modules/systemlib/mixer/mixer.cpp ${PX4_SOURCE_DIR}/src/modules/systemlib/mixer/mixer_group.cpp ${PX4_SOURCE_DIR}/src/modules/systemlib/mixer/mixer_helicopter.cpp diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 830e1519cbae..7eb302ed6eb8 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -88,6 +88,7 @@ enum mixer_source { MIX_FAILSAFE, MIX_OVERRIDE_FMU_OK }; + static volatile mixer_source source; static int mixer_callback(uintptr_t handle,