From d96f9ed2b7ee60c9591f13514adcec0ca4900b67 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 12 Nov 2017 16:00:18 +0000 Subject: [PATCH] Split crsf protocol into interface directory --- src/main/interface/crsf_protocol.h | 76 ++++++++++++++++++++++++++++++ src/main/rx/crsf.h | 54 ++------------------- src/main/telemetry/crsf.c | 27 +++++------ src/main/telemetry/crsf.h | 6 ++- src/main/telemetry/msp_shared.c | 3 +- src/main/telemetry/msp_shared.h | 2 +- src/test/unit/rx_crsf_unittest.cc | 2 +- 7 files changed, 100 insertions(+), 70 deletions(-) create mode 100644 src/main/interface/crsf_protocol.h diff --git a/src/main/interface/crsf_protocol.h b/src/main/interface/crsf_protocol.h new file mode 100644 index 000000000..0594f2fd1 --- /dev/null +++ b/src/main/interface/crsf_protocol.h @@ -0,0 +1,76 @@ +/* + * Crossfire constants provided by Team Black Sheep under terms of the 2-Clause BSD License + */ + +#pragma once + +#include +#include + +#define CRSF_BAUDRATE 420000 + +enum { CRSF_SYNC_BYTE = 0xC8 }; + +enum { CRSF_FRAME_SIZE_MAX = 64 }; // 62 bytes frame plus 2 bytes frame header() +enum { CRSF_PAYLOAD_SIZE_MAX = CRSF_FRAME_SIZE_MAX - 6 }; + +typedef enum { + CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, + CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, + CRSF_FRAMETYPE_ATTITUDE = 0x1E, + CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, + // Extended Header Frames, range: 0x28 to 0x96 + CRSF_FRAMETYPE_DEVICE_PING = 0x28, + CRSF_FRAMETYPE_DEVICE_INFO = 0x29, + CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B, + CRSF_FRAMETYPE_PARAMETER_READ = 0x2C, + CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D, + CRSF_FRAMETYPE_COMMAND = 0x32, + // MSP commands + CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command + CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary + CRSF_FRAMETYPE_MSP_WRITE = 0x7C // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) +} crsfFrameType_e; + +enum { + CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, + CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, + CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. + CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, +}; + +enum { + CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field + CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field + CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field + CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field + CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined + CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4, // length of Extended Dest/Origin, TYPE and CRC fields combined + CRSF_FRAME_LENGTH_NON_PAYLOAD = 4, // combined length of all fields except payload +}; + +enum { + CRSF_FRAME_TX_MSP_FRAME_SIZE = 58, + CRSF_FRAME_RX_MSP_FRAME_SIZE = 8, + CRSF_FRAME_ORIGIN_DEST_SIZE = 2, +}; + +typedef enum { + CRSF_ADDRESS_BROADCAST = 0x00, + CRSF_ADDRESS_USB = 0x10, + CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80, + CRSF_ADDRESS_RESERVED1 = 0x8A, + CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, + CRSF_ADDRESS_GPS = 0xC2, + CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, + CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8, + CRSF_ADDRESS_RESERVED2 = 0xCA, + CRSF_ADDRESS_RACE_TAG = 0xCC, + CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, + CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, + CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE +} crsfAddress_e; + diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 2c25f6870..84c50f33d 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -17,61 +17,13 @@ #pragma once -#define CRSF_BAUDRATE 420000 +#include "interface/crsf_protocol.h" + + #define CRSF_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO) #define CRSF_PORT_MODE MODE_RXTX #define CRSF_MAX_CHANNEL 16 -#define CRSF_MSP_RX_BUF_SIZE 128 -#define CRSF_MSP_TX_BUF_SIZE 128 - -typedef enum { - CRSF_FRAMETYPE_GPS = 0x02, - CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, - CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, - CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, - CRSF_FRAMETYPE_ATTITUDE = 0x1E, - CRSF_FRAMETYPE_FLIGHT_MODE = 0x21, - CRSF_FRAMETYPE_DEVICE_PING = 0x28, - CRSF_FRAMETYPE_DEVICE_INFO = 0x29, - CRSF_FRAMETYPE_MSP_REQ = 0x7A, // response request using msp sequence as command - CRSF_FRAMETYPE_MSP_RESP = 0x7B, // reply with 58 byte chunked binary - CRSF_FRAMETYPE_MSP_WRITE = 0x7C // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit) -} crsfFrameType_e; - -enum { - CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, - CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, - CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, - CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE = 22, // 11 bits per channel * 16 channels = 22 bytes. - CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6, - CRSF_FRAME_TX_MSP_FRAME_SIZE = 58, - CRSF_FRAME_RX_MSP_FRAME_SIZE = 8, - CRSF_FRAME_ORIGIN_DEST_SIZE = 2, - CRSF_FRAME_LENGTH_ADDRESS = 1, // length of ADDRESS field - CRSF_FRAME_LENGTH_FRAMELENGTH = 1, // length of FRAMELENGTH field - CRSF_FRAME_LENGTH_TYPE = 1, // length of TYPE field - CRSF_FRAME_LENGTH_CRC = 1, // length of CRC field - CRSF_FRAME_LENGTH_TYPE_CRC = 2, // length of TYPE and CRC fields combined - CRSF_FRAME_LENGTH_EXT_TYPE_CRC = 4 // length of Extended Dest/Origin, TYPE and CRC fields combined -}; - -enum { - CRSF_ADDRESS_BROADCAST = 0x00, - CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8, - CRSF_ADDRESS_RESERVED1 = 0x8A, - CRSF_ADDRESS_CURRENT_SENSOR = 0xC0, - CRSF_ADDRESS_TBS_BLACKBOX = 0xC4, - CRSF_ADDRESS_BETAFLIGHT = 0xC8, - CRSF_ADDRESS_RESERVED2 = 0xCA, - CRSF_ADDRESS_RACE_TAG = 0xCC, - CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA, - CRSF_ADDRESS_CRSF_RECEIVER = 0xEC, - CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE -}; - -#define CRSF_PAYLOAD_SIZE_MAX 60 -#define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + 4) typedef struct crsfFrameDef_s { uint8_t deviceAddress; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 2e0165136..da70f1626 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -23,11 +23,11 @@ #ifdef USE_TELEMETRY -#include "config/feature.h" #include "build/atomic.h" #include "build/build_config.h" #include "build/version.h" +#include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" @@ -39,27 +39,26 @@ #include "drivers/nvic.h" -#include "sensors/battery.h" +#include "fc/config.h" +#include "fc/rc_modes.h" +#include "fc/runtime_config.h" + +#include "flight/imu.h" + +#include "interface/crsf_protocol.h" #include "io/gps.h" #include "io/serial.h" -#include "fc/rc_modes.h" -#include "fc/runtime_config.h" - -#include "io/gps.h" - -#include "flight/imu.h" - -#include "rx/rx.h" #include "rx/crsf.h" +#include "sensors/battery.h" +#include "sensors/sensors.h" + #include "telemetry/telemetry.h" #include "telemetry/crsf.h" #include "telemetry/msp_shared.h" -#include "fc/config.h" -#include "sensors/sensors.h" #define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz #define CRSF_DEVICEINFO_VERSION 0x01 @@ -297,7 +296,7 @@ void crsfFrameDeviceInfo(sbuf_t *dst) { sbufWriteU8(dst, 0); sbufWriteU8(dst, CRSF_FRAMETYPE_DEVICE_INFO); sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); - sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); + sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); sbufWriteStringWithZeroTerminator(dst, buff); for (unsigned int ii=0; ii<12; ii++) { sbufWriteU8(dst, 0x00); @@ -340,7 +339,7 @@ void crsfSendMspResponse(uint8_t *payload) sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); - sbufWriteU8(dst, CRSF_ADDRESS_BETAFLIGHT); + sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_FRAME_SIZE); crsfFinalize(dst); } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 01712be54..83fe327ae 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -21,7 +21,11 @@ #include #include "common/time.h" -#include "rx/crsf.h" +#include "interface/crsf_protocol.h" + +#define CRSF_MSP_RX_BUF_SIZE 128 +#define CRSF_MSP_TX_BUF_SIZE 128 + void initCrsfTelemetry(void); bool checkCrsfTelemetryState(void); diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 67bcb842f..c5406be73 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -12,8 +12,7 @@ #include "interface/fc_msp.h" -#include "rx/crsf.h" - +#include "telemetry/crsf.h" #include "telemetry/msp_shared.h" #include "telemetry/smartport.h" diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index c4c23b023..fad6b912d 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -1,7 +1,7 @@ #pragma once #include "common/streambuf.h" -#include "rx/crsf.h" +#include "telemetry/crsf.h" #include "telemetry/smartport.h" typedef void (*mspResponseFnPtr)(uint8_t *payload); diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 2c1c9e02c..8b1a9e450 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -286,6 +286,6 @@ serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; void crsfScheduleDeviceInfoResponse(void) {}; -void crsfScheduleMspResponse(mspPackage_t *package) { UNUSED(package); }; +void crsfScheduleMspResponse(void) {}; bool bufferMspFrame(uint8_t *, int) {return true;} }