From 2319744bac6f7832b59f4a6e89529e71cb8d5ab8 Mon Sep 17 00:00:00 2001 From: rtlopez Date: Mon, 5 Aug 2024 00:11:41 +0200 Subject: [PATCH] msp over crsf --- lib/Espfc/src/Device/InputCRSF.cpp | 69 ++++---- lib/Espfc/src/Device/InputCRSF.h | 12 +- lib/Espfc/src/Rc/Crsf.cpp | 95 ++++++++-- lib/Espfc/src/Rc/Crsf.h | 39 ++++- lib/Espfc/src/Telemetry/TelemetryCRSF.h | 97 +++++------ lib/Espfc/src/TelemetryManager.h | 9 +- test/test_input_crsf/test_input_crsf.cpp | 212 ++++++++++++++++++----- 7 files changed, 373 insertions(+), 160 deletions(-) diff --git a/lib/Espfc/src/Device/InputCRSF.cpp b/lib/Espfc/src/Device/InputCRSF.cpp index da48324..ff35547 100644 --- a/lib/Espfc/src/Device/InputCRSF.cpp +++ b/lib/Espfc/src/Device/InputCRSF.cpp @@ -1,3 +1,4 @@ +#include #include "InputCRSF.h" #include "Utils/MemoryHelper.h" @@ -14,11 +15,8 @@ int InputCRSF::begin(Device::SerialDevice * serial, TelemetryManager * telemetry _serial = serial; _telemetry = telemetry; _telemetry_next = micros() + TELEMETRY_INTERVAL; - for(size_t i = 0; i < CRSF_FRAME_SIZE_MAX; i++) - { - _frame.data[i] = 0; - if(i < CHANNELS) _channels[i] = 0; - } + std::fill_n((uint8_t*)&_frame, sizeof(_frame), 0); + std::fill_n(_channels, CHANNELS, 0); return 1; } @@ -72,49 +70,50 @@ size_t InputCRSF::getChannelCount() const { return CHANNELS; } bool InputCRSF::needAverage() const { return false; } -void FAST_CODE_ATTR InputCRSF::parse(CrsfFrame& frame, int d) +void FAST_CODE_ATTR InputCRSF::parse(CrsfMessage& msg, int d) { + uint8_t *data = reinterpret_cast(&msg); uint8_t c = (uint8_t)(d & 0xff); switch(_state) { case CRSF_ADDR: - if(c == CRSF_ADDRESS_FLIGHT_CONTROLLER) + if(c == CRSF_SYNC_BYTE) { - frame.data[_idx++] = c; + data[_idx++] = c; _state = CRSF_SIZE; } break; case CRSF_SIZE: if(c > 3 && c <= CRSF_PAYLOAD_SIZE_MAX) { - frame.data[_idx++] = c; + data[_idx++] = c; _state = CRSF_TYPE; } else { reset(); } break; case CRSF_TYPE: - if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS) + if(c == CRSF_FRAMETYPE_RC_CHANNELS_PACKED || c == CRSF_FRAMETYPE_LINK_STATISTICS || c == CRSF_FRAMETYPE_MSP_REQ) { - frame.data[_idx++] = c; + data[_idx++] = c; _state = CRSF_DATA; } else { reset(); } break; case CRSF_DATA: - frame.data[_idx++] = c; - if(_idx > frame.message.size) // _idx is incremented here and operator > accounts as size - 2 + data[_idx++] = c; + if(_idx > msg.size) // _idx is incremented here and operator > accounts as size - 2 { _state = CRSF_CRC; } break; case CRSF_CRC: - frame.data[_idx++] = c; + data[_idx++] = c; reset(); - uint8_t crc = Crsf::crc(frame); + uint8_t crc = msg.crc(); if(c == crc) { - apply(frame); + apply(msg); } break; } @@ -126,20 +125,20 @@ void FAST_CODE_ATTR InputCRSF::reset() _idx = 0; } -void FAST_CODE_ATTR InputCRSF::apply(const CrsfFrame& frame) +void FAST_CODE_ATTR InputCRSF::apply(const CrsfMessage& msg) { - switch (frame.message.type) + switch (msg.type) { case CRSF_FRAMETYPE_RC_CHANNELS_PACKED: - applyChannels(frame); + applyChannels(msg); break; case CRSF_FRAMETYPE_LINK_STATISTICS: - applyLinkStats(frame); + applyLinkStats(msg); break; case CRSF_FRAMETYPE_MSP_REQ: - applyMspReq(frame); + applyMspReq(msg); break; default: @@ -147,28 +146,36 @@ void FAST_CODE_ATTR InputCRSF::apply(const CrsfFrame& frame) } } -void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfFrame& f) +void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfMessage& msg) { - const CrsfLinkStats* frame = reinterpret_cast(f.message.payload); - (void)frame; + const CrsfLinkStats* stats = reinterpret_cast(msg.payload); + (void)stats; // TODO: } -void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfFrame& f) +void FAST_CODE_ATTR InputCRSF::applyChannels(const CrsfMessage& msg) { - const CrsfData* frame = reinterpret_cast(f.message.payload); - Crsf::decodeRcDataShift8(_channels, frame); + const CrsfData* data = reinterpret_cast(msg.payload); + Crsf::decodeRcDataShift8(_channels, data); //Crsf::decodeRcData(_channels, frame); _new_data = true; } -void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfFrame& f) +void FAST_CODE_ATTR InputCRSF::applyMspReq(const CrsfMessage& msg) { - if(_telemetry) + if(!_telemetry) return; + + uint8_t origin; + Msp::MspMessage m; + + Crsf::decodeMsp(msg, m, origin); + + if(m.isCmd() && m.isReady()) { - _telemetry_next = micros() + TELEMETRY_INTERVAL; - _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, f.message.payload, CRSF_PAYLOAD_SIZE_MAX); + _telemetry->processMsp(*_serial, TELEMETRY_PROTOCOL_CRSF, m, origin); } + + _telemetry_next = micros() + TELEMETRY_INTERVAL; } } diff --git a/lib/Espfc/src/Device/InputCRSF.h b/lib/Espfc/src/Device/InputCRSF.h index 289040d..1d145ff 100644 --- a/lib/Espfc/src/Device/InputCRSF.h +++ b/lib/Espfc/src/Device/InputCRSF.h @@ -34,14 +34,14 @@ class InputCRSF: public InputDevice virtual bool needAverage() const override; void print(char c) const; - void parse(Rc::CrsfFrame& frame, int d); + void parse(Rc::CrsfMessage& frame, int d); private: void reset(); - void apply(const Rc::CrsfFrame& frame); - void applyLinkStats(const Rc::CrsfFrame& f); - void applyChannels(const Rc::CrsfFrame& f); - void applyMspReq(const Rc::CrsfFrame& f); + void apply(const Rc::CrsfMessage& msg); + void applyLinkStats(const Rc::CrsfMessage& msg); + void applyChannels(const Rc::CrsfMessage& msg); + void applyMspReq(const Rc::CrsfMessage& msg); static const size_t CHANNELS = 16; static const size_t TELEMETRY_INTERVAL = 20000; @@ -51,7 +51,7 @@ class InputCRSF: public InputDevice CrsfState _state; uint8_t _idx; bool _new_data; - Rc::CrsfFrame _frame; + Rc::CrsfMessage _frame; uint16_t _channels[CHANNELS]; uint32_t _telemetry_next; }; diff --git a/lib/Espfc/src/Rc/Crsf.cpp b/lib/Espfc/src/Rc/Crsf.cpp index 54189e4..e93e041 100644 --- a/lib/Espfc/src/Rc/Crsf.cpp +++ b/lib/Espfc/src/Rc/Crsf.cpp @@ -1,4 +1,4 @@ - +#include #include "Crsf.h" #include "Math/Utils.h" #include "Math/Crc.h" @@ -83,13 +83,86 @@ void FAST_CODE_ATTR Crsf::decodeRcDataShift8(uint16_t* channels, const CrsfData* channels[15] = convert((crsfData[5] >> 5) & 0x07FF); }*/ -void Crsf::encodeRcData(CrsfFrame& frame, const CrsfData& data) +void Crsf::encodeRcData(CrsfMessage& msg, const CrsfData& data) +{ + msg.addr = CRSF_ADDRESS_FLIGHT_CONTROLLER; + msg.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED; + msg.size = sizeof(data) + 2; + std::memcpy(msg.payload, (void*)&data, sizeof(data)); + msg.payload[sizeof(data)] = crc(msg); +} + +int Crsf::encodeMsp(CrsfMessage& msg, const Msp::MspResponse& resp, uint8_t origin) +{ + uint8_t buff[CRSF_PAYLOAD_SIZE_MAX]; + size_t size = resp.serialize(buff, CRSF_PAYLOAD_SIZE_MAX); + + if(size < 4) return 0; // unable to serialize + + uint8_t status = 0; + status |= (1 << 4); // start bit + status |= ((resp.version == Msp::MSP_V1 ? 1 : 2) << 5); + + msg.prepare(Rc::CRSF_FRAMETYPE_MSP_RESP); + msg.writeU8(origin); + msg.writeU8(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER); + msg.writeU8(status); + msg.write(buff + 3, size - 4); // skip sync bytes and crc + msg.finalize(); + + return msg.size; +} + +int Crsf::decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin) { - frame.message.addr = CRSF_ADDRESS_FLIGHT_CONTROLLER; - frame.message.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED; - frame.message.size = sizeof(data) + 2; - std::memcpy(frame.message.payload, (void*)&data, sizeof(data)); - frame.message.payload[sizeof(data)] = crc(frame); + //uint8_t dst = msg.payload[0]; + origin = msg.payload[1]; + uint8_t status = msg.payload[2]; + + //uint8_t sequence = (status & 0x0f); // 00001111 + uint8_t start = (status & 0x10) >> 4; // 00010000 + uint8_t version = (status & 0x60) >> 5; // 01100000 + //uint8_t error = (status & 0x80) >> 7; // 10000000 + + if(start) + { + if(version == 1) + { + const Msp::MspHeaderV1 * hdr = reinterpret_cast(msg.payload + 3); + size_t framePayloadSize = msg.size - 5 - sizeof(Msp::MspHeaderV1); + if(framePayloadSize >= hdr->size) + { + m.expected = hdr->size; + m.received = hdr->size; + m.cmd = hdr->cmd; + m.state = Msp::MSP_STATE_RECEIVED; + m.dir = Msp::MSP_TYPE_CMD; + m.version = Msp::MSP_V1; + std::copy_n(msg.payload + 3 + sizeof(Msp::MspHeaderV1), m.received, m.buffer); + } + } + else if(version == 2) + { + const Msp::MspHeaderV2 * hdr = reinterpret_cast(msg.payload + 3); + size_t framePayloadSize = msg.size - 5 - sizeof(Msp::MspHeaderV2); + if(framePayloadSize >= hdr->size) + { + m.expected = hdr->size; + m.received = hdr->size; + m.cmd = hdr->cmd; + m.state = Msp::MSP_STATE_RECEIVED; + m.dir = Msp::MSP_TYPE_CMD; + m.version = Msp::MSP_V1; + std::copy_n(msg.payload + 3 + sizeof(Msp::MspHeaderV2), m.received, m.buffer); + } + } + } + else + { + // next chunk + } + + return 0; } uint16_t Crsf::convert(int v) @@ -109,12 +182,12 @@ uint16_t Crsf::convert(int v) //return Math::mapi(v, 172, 1811, 988, 2012); } -uint8_t Crsf::crc(const CrsfFrame& frame) +uint8_t Crsf::crc(const CrsfMessage& msg) { // CRC includes type and payload - uint8_t crc = Math::crc8_dvb_s2(0, frame.message.type); - for (int i = 0; i < frame.message.size - 2; i++) { // size includes type and crc - crc = Math::crc8_dvb_s2(crc, frame.message.payload[i]); + uint8_t crc = Math::crc8_dvb_s2(0, msg.type); + for (int i = 0; i < msg.size - 2; i++) { // size includes type and crc + crc = Math::crc8_dvb_s2(crc, msg.payload[i]); } return crc; } diff --git a/lib/Espfc/src/Rc/Crsf.h b/lib/Espfc/src/Rc/Crsf.h index 77eb308..78d7ed1 100644 --- a/lib/Espfc/src/Rc/Crsf.h +++ b/lib/Espfc/src/Rc/Crsf.h @@ -1,6 +1,9 @@ #pragma once #include +#include +#include "Math/Crc.h" +#include "Msp/Msp.h" namespace Espfc { @@ -117,6 +120,19 @@ struct CrsfMessage uint8_t type; // CrsfFrameType uint8_t payload[CRSF_PAYLOAD_SIZE_MAX + 1]; + void prepare(uint8_t t) + { + addr = Rc::CRSF_SYNC_BYTE; + type = t; + size = 0; + } + + void finalize() + { + size += 2; + writeCRC(crc()); + } + void writeU8(uint8_t v) { payload[size++] = v; @@ -136,6 +152,11 @@ struct CrsfMessage writeU8(v >> 24); } + void write(const uint8_t * v, size_t len) + { + while(len--) writeU8(*v++); + } + void writeString(const char * v, bool terminate = false) { while(*v) writeU8(*v++); @@ -146,13 +167,13 @@ struct CrsfMessage { payload[size - 2] = v; } -} __attribute__ ((__packed__)); -union CrsfFrame -{ - CrsfMessage message; - uint8_t data[CRSF_FRAME_SIZE_MAX]; -}; + uint8_t crc() const + { + uint8_t crc = Math::crc8_dvb_s2(0, type); + return Math::crc8_dvb_s2(crc, payload, size - 2); // size includes type and crc + } +} __attribute__ ((__packed__)); class Crsf { @@ -160,9 +181,11 @@ class Crsf static void decodeRcData(uint16_t* channels, const CrsfData* frame); static void decodeRcDataShift8(uint16_t* channels, const CrsfData* frame); //static void decodeRcDataShift32(uint16_t* channels, const CrsfData* frame); - static void encodeRcData(CrsfFrame& frame, const CrsfData& data); + static void encodeRcData(CrsfMessage& frame, const CrsfData& data); + static int encodeMsp(CrsfMessage& msg, const Msp::MspResponse& res, uint8_t origin); + static int decodeMsp(const CrsfMessage& msg, Msp::MspMessage& m, uint8_t& origin); static uint16_t convert(int v); - static uint8_t crc(const CrsfFrame& frame); + static uint8_t crc(const CrsfMessage& frame); }; } diff --git a/lib/Espfc/src/Telemetry/TelemetryCRSF.h b/lib/Espfc/src/Telemetry/TelemetryCRSF.h index 0b4561d..b5dfc97 100644 --- a/lib/Espfc/src/Telemetry/TelemetryCRSF.h +++ b/lib/Espfc/src/Telemetry/TelemetryCRSF.h @@ -5,6 +5,10 @@ #include "Rc/Crsf.h" #include "Math/Utils.h" +// https://github.com/crsf-wg/crsf/wiki/CRSF_FRAMETYPE_MSP_REQ - not correct +// https://github.com/betaflight/betaflight/blob/2525be9a3369fa666d8ce1485ec5ad344326b085/src/main/telemetry/crsf.c#L664 +// https://github.com/betaflight/betaflight/blob/2525be9a3369fa666d8ce1485ec5ad344326b085/src/main/telemetry/msp_shared.c#L46 + namespace Espfc { namespace Telemetry { @@ -30,7 +34,7 @@ class TelemetryCRSF int process(Device::SerialDevice& s) const { - Rc::CrsfFrame f; + Rc::CrsfMessage f; switch(_current) { case CRSF_TELEMETRY_STATE_ATTI: @@ -46,7 +50,8 @@ class TelemetryCRSF case CRSF_TELEMETRY_STATE_FM: flightMode(f); send(f, s); - _current = CRSF_TELEMETRY_STATE_GPS; + //_current = CRSF_TELEMETRY_STATE_GPS; + _current = CRSF_TELEMETRY_STATE_VARIO; break; case CRSF_TELEMETRY_STATE_GPS: //gps(f); @@ -68,15 +73,20 @@ class TelemetryCRSF return 1; } - int sendMsp(Device::SerialDevice& s, Msp::MspResponse r) const + int sendMsp(Device::SerialDevice& s, Msp::MspResponse r, uint8_t origin) const { + Rc::CrsfMessage msg; + + Rc::Crsf::encodeMsp(msg, r, origin); + + send(msg, s); return 1; } - void send(const Rc::CrsfFrame& f, Device::SerialDevice& s) const + void send(const Rc::CrsfMessage& msg, Device::SerialDevice& s) const { - s.write(f.data, f.message.size + 2); + s.write((uint8_t*)&msg, msg.size + 2); } int16_t toAngle(float angle) const @@ -86,83 +96,70 @@ class TelemetryCRSF return lrintf(angle * 1000); } - void attitude(Rc::CrsfFrame& f) const + void attitude(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_ATTITUDE); + msg.prepare(Rc::CRSF_FRAMETYPE_ATTITUDE); int16_t r = toAngle(_model.state.angle.x); int16_t p = toAngle(_model.state.angle.y); int16_t y = toAngle(_model.state.angle.z); - f.message.writeU16(Math::toBigEndian16(r)); - f.message.writeU16(Math::toBigEndian16(p)); - f.message.writeU16(Math::toBigEndian16(y)); + msg.writeU16(Math::toBigEndian16(r)); + msg.writeU16(Math::toBigEndian16(p)); + msg.writeU16(Math::toBigEndian16(y)); - finalize(f); + msg.finalize(); } - void battery(Rc::CrsfFrame& f) const + void battery(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_BATTERY_SENSOR); + msg.prepare(Rc::CRSF_FRAMETYPE_BATTERY_SENSOR); uint16_t voltage = Math::clamp(lrintf(_model.state.battery.voltage * 10.0f), 0l, 32000l); uint16_t current = Math::clamp(lrintf(_model.state.battery.current * 10.0f), 0l, 32000l); uint32_t mahDrawn = 0; uint8_t remainPerc = 100; - f.message.writeU16(Math::toBigEndian16(voltage)); - f.message.writeU16(Math::toBigEndian16(current)); - f.message.writeU8(mahDrawn >> 16); - f.message.writeU8(mahDrawn >> 8); - f.message.writeU8(mahDrawn); - f.message.writeU8(remainPerc); + msg.writeU16(Math::toBigEndian16(voltage)); + msg.writeU16(Math::toBigEndian16(current)); + msg.writeU8(mahDrawn >> 16); + msg.writeU8(mahDrawn >> 8); + msg.writeU8(mahDrawn); + msg.writeU8(remainPerc); - finalize(f); + msg.finalize(); } - void flightMode(Rc::CrsfFrame& f) const + void flightMode(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_FLIGHT_MODE); + msg.prepare(Rc::CRSF_FRAMETYPE_FLIGHT_MODE); - if(_model.armingDisabled()) f.message.writeString("!DIS"); - if(_model.isModeActive(MODE_FAILSAFE)) f.message.writeString("!FS,"); - if(_model.isModeActive(MODE_ARMED)) f.message.writeString("ARM,"); - if(_model.isModeActive(MODE_AIRMODE)) f.message.writeString("AIR,"); - if(_model.isModeActive(MODE_ANGLE)) f.message.writeString("STAB,"); - f.message.writeU8(0); + if(_model.armingDisabled()) msg.writeString("!DIS"); + if(_model.isModeActive(MODE_FAILSAFE)) msg.writeString("!FS,"); + if(_model.isModeActive(MODE_ARMED)) msg.writeString("ARM,"); + if(_model.isModeActive(MODE_AIRMODE)) msg.writeString("AIR,"); + if(_model.isModeActive(MODE_ANGLE)) msg.writeString("STAB,"); + msg.writeU8(0); - finalize(f); + msg.finalize(); } - void vario(Rc::CrsfFrame& f) const + void vario(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_VARIO_SENSOR); + msg.prepare(Rc::CRSF_FRAMETYPE_VARIO_SENSOR); - f.message.writeU16(Math::toBigEndian16(0)); + msg.writeU16(Math::toBigEndian16(0)); - finalize(f); + msg.finalize(); } - void heartbeat(Rc::CrsfFrame& f) const + void heartbeat(Rc::CrsfMessage& msg) const { - prepare(f, Rc::CRSF_FRAMETYPE_HEARTBEAT); - - f.message.writeU16(Math::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); + msg.prepare(Rc::CRSF_FRAMETYPE_HEARTBEAT); - finalize(f); - } + msg.writeU16(Math::toBigEndian16(Rc::CRSF_ADDRESS_FLIGHT_CONTROLLER)); - void prepare(Rc::CrsfFrame& f, uint8_t type) const - { - f.message.addr = Rc::CRSF_SYNC_BYTE; - f.message.type = type; - f.message.size = 0; - } - - void finalize(Rc::CrsfFrame& f) const - { - f.message.size += 2; - f.message.writeCRC(Rc::Crsf::crc(f)); + msg.finalize(); } private: diff --git a/lib/Espfc/src/TelemetryManager.h b/lib/Espfc/src/TelemetryManager.h index e87d3dd..84fe58c 100644 --- a/lib/Espfc/src/TelemetryManager.h +++ b/lib/Espfc/src/TelemetryManager.h @@ -35,14 +35,9 @@ class TelemetryManager return 1; } - int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, const uint8_t * payload, size_t max_len) + int processMsp(Device::SerialDevice& s, TelemetryProtocol protocol, Msp::MspMessage m, uint8_t origin) { - Msp::MspMessage m; Msp::MspResponse r; - for(size_t i = 0; i < max_len; i++) - { - if(!_msp.parse(payload[i], m)) break; - } // not valid msp message, stop processing if(!m.isReady() || !m.isCmd()) return 0; @@ -52,7 +47,7 @@ class TelemetryManager switch(protocol) { case TELEMETRY_PROTOCOL_CRSF: - _crsf.sendMsp(s, r); + _crsf.sendMsp(s, r, origin); break; default: break; diff --git a/test/test_input_crsf/test_input_crsf.cpp b/test/test_input_crsf/test_input_crsf.cpp index 2013220..b50e4ec 100644 --- a/test/test_input_crsf/test_input_crsf.cpp +++ b/test/test_input_crsf/test_input_crsf.cpp @@ -15,8 +15,9 @@ using namespace fakeit; void test_input_crsf_rc_valid() { InputCRSF input; - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); + uint8_t * frame_data = reinterpret_cast(&frame); When(Method(ArduinoFake(), micros)).Return(0); @@ -31,15 +32,16 @@ void test_input_crsf_rc_valid() } for (size_t i; i < sizeof(data); i++) { - TEST_ASSERT_EQUAL_UINT8(data[i], frame.data[i]); + TEST_ASSERT_EQUAL_UINT8(data[i], frame_data[i]); } const uint8_t crc = Crsf::crc(frame); TEST_ASSERT_EQUAL_UINT8(0x23, crc); + TEST_ASSERT_EQUAL_UINT8(0x23, frame.crc()); - TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.message.addr); - TEST_ASSERT_EQUAL_UINT8(0x18, frame.message.size); - TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.message.type); + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x18, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); TEST_ASSERT_EQUAL_UINT16(1500u, input.get(0)); TEST_ASSERT_EQUAL_UINT16(1500u, input.get(1)); @@ -52,7 +54,7 @@ void test_input_crsf_rc_valid() void test_input_crsf_rc_prefix() { InputCRSF input; - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); When(Method(ArduinoFake(), micros)).Return(0); @@ -72,9 +74,9 @@ void test_input_crsf_rc_prefix() const uint8_t crc = Crsf::crc(frame); TEST_ASSERT_EQUAL_UINT8(0x23, crc); - TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.message.addr); - TEST_ASSERT_EQUAL_UINT8(0x18, frame.message.size); - TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.message.type); + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x18, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); TEST_ASSERT_EQUAL_UINT16(1500, input.get(0)); TEST_ASSERT_EQUAL_UINT16(1500, input.get(1)); @@ -82,7 +84,7 @@ void test_input_crsf_rc_prefix() void test_crsf_encode_rc() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -110,44 +112,46 @@ void test_crsf_encode_rc() 0x7C, 0xE0, 0x03, 0x1F, 0xF8, 0xC0, 0x07, 0x3E, 0xF0, 0x81, 0x0F, 0x7C, 0xDB }; - TEST_ASSERT_EQUAL_UINT8(expected[0], frame.data[0]); // addr - TEST_ASSERT_EQUAL_UINT8(expected[1], frame.data[1]); // size - TEST_ASSERT_EQUAL_UINT8(expected[2], frame.data[2]); // type - TEST_ASSERT_EQUAL_UINT8(expected[3], frame.data[3]); - TEST_ASSERT_EQUAL_UINT8(expected[4], frame.data[4]); - TEST_ASSERT_EQUAL_UINT8(expected[5], frame.data[5]); - TEST_ASSERT_EQUAL_UINT8(expected[6], frame.data[6]); - TEST_ASSERT_EQUAL_UINT8(expected[7], frame.data[7]); - TEST_ASSERT_EQUAL_UINT8(expected[8], frame.data[8]); - TEST_ASSERT_EQUAL_UINT8(expected[9], frame.data[9]); - TEST_ASSERT_EQUAL_UINT8(expected[10], frame.data[10]); - TEST_ASSERT_EQUAL_UINT8(expected[11], frame.data[11]); - TEST_ASSERT_EQUAL_UINT8(expected[12], frame.data[12]); - TEST_ASSERT_EQUAL_UINT8(expected[13], frame.data[13]); - TEST_ASSERT_EQUAL_UINT8(expected[14], frame.data[14]); - TEST_ASSERT_EQUAL_UINT8(expected[15], frame.data[15]); - TEST_ASSERT_EQUAL_UINT8(expected[16], frame.data[16]); - TEST_ASSERT_EQUAL_UINT8(expected[17], frame.data[17]); - TEST_ASSERT_EQUAL_UINT8(expected[18], frame.data[18]); - TEST_ASSERT_EQUAL_UINT8(expected[19], frame.data[19]); - TEST_ASSERT_EQUAL_UINT8(expected[20], frame.data[20]); - TEST_ASSERT_EQUAL_UINT8(expected[21], frame.data[21]); - TEST_ASSERT_EQUAL_UINT8(expected[22], frame.data[22]); - TEST_ASSERT_EQUAL_UINT8(expected[23], frame.data[23]); - TEST_ASSERT_EQUAL_UINT8(expected[24], frame.data[24]); - TEST_ASSERT_EQUAL_UINT8(expected[25], frame.data[25]); // crc + uint8_t * frame_data = reinterpret_cast(&frame); + + TEST_ASSERT_EQUAL_UINT8(expected[0], frame_data[0]); // addr + TEST_ASSERT_EQUAL_UINT8(expected[1], frame_data[1]); // size + TEST_ASSERT_EQUAL_UINT8(expected[2], frame_data[2]); // type + TEST_ASSERT_EQUAL_UINT8(expected[3], frame_data[3]); + TEST_ASSERT_EQUAL_UINT8(expected[4], frame_data[4]); + TEST_ASSERT_EQUAL_UINT8(expected[5], frame_data[5]); + TEST_ASSERT_EQUAL_UINT8(expected[6], frame_data[6]); + TEST_ASSERT_EQUAL_UINT8(expected[7], frame_data[7]); + TEST_ASSERT_EQUAL_UINT8(expected[8], frame_data[8]); + TEST_ASSERT_EQUAL_UINT8(expected[9], frame_data[9]); + TEST_ASSERT_EQUAL_UINT8(expected[10], frame_data[10]); + TEST_ASSERT_EQUAL_UINT8(expected[11], frame_data[11]); + TEST_ASSERT_EQUAL_UINT8(expected[12], frame_data[12]); + TEST_ASSERT_EQUAL_UINT8(expected[13], frame_data[13]); + TEST_ASSERT_EQUAL_UINT8(expected[14], frame_data[14]); + TEST_ASSERT_EQUAL_UINT8(expected[15], frame_data[15]); + TEST_ASSERT_EQUAL_UINT8(expected[16], frame_data[16]); + TEST_ASSERT_EQUAL_UINT8(expected[17], frame_data[17]); + TEST_ASSERT_EQUAL_UINT8(expected[18], frame_data[18]); + TEST_ASSERT_EQUAL_UINT8(expected[19], frame_data[19]); + TEST_ASSERT_EQUAL_UINT8(expected[20], frame_data[20]); + TEST_ASSERT_EQUAL_UINT8(expected[21], frame_data[21]); + TEST_ASSERT_EQUAL_UINT8(expected[22], frame_data[22]); + TEST_ASSERT_EQUAL_UINT8(expected[23], frame_data[23]); + TEST_ASSERT_EQUAL_UINT8(expected[24], frame_data[24]); + TEST_ASSERT_EQUAL_UINT8(expected[25], frame_data[25]); // crc const uint8_t crc = Crsf::crc(frame); TEST_ASSERT_EQUAL_UINT8(0xdb, crc); - TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.message.addr); - TEST_ASSERT_EQUAL_UINT8(0x18, frame.message.size); - TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.message.type); + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x18, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, frame.type); } void test_crsf_decode_rc_struct() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -172,7 +176,7 @@ void test_crsf_decode_rc_struct() uint16_t channels[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - Crsf::decodeRcData(channels, (const CrsfData*)frame.message.payload); + Crsf::decodeRcData(channels, (const CrsfData*)frame.payload); TEST_ASSERT_EQUAL_UINT16(1500, channels[0]); TEST_ASSERT_EQUAL_UINT16(1500, channels[1]); @@ -197,7 +201,7 @@ void test_crsf_decode_rc_struct() void test_crsf_decode_rc_shift8() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -222,7 +226,7 @@ void test_crsf_decode_rc_shift8() uint16_t channels[16] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; - Crsf::decodeRcDataShift8(channels, (const CrsfData*)frame.message.payload); + Crsf::decodeRcDataShift8(channels, (const CrsfData*)frame.payload); TEST_ASSERT_EQUAL_UINT16(1500, channels[0]); TEST_ASSERT_EQUAL_UINT16(1500, channels[1]); @@ -247,7 +251,7 @@ void test_crsf_decode_rc_shift8() /*void test_crsf_decode_rc_shift32() { - CrsfFrame frame; + CrsfMessage frame; memset(&frame, 0, sizeof(frame)); CrsfData data; @@ -295,6 +299,119 @@ void test_crsf_decode_rc_shift8() TEST_ASSERT_EQUAL_UINT16(1500, channels[15]); }*/ +void test_crsf_encode_msp_v1() +{ + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + + Msp::MspResponse resp; + resp.version = Msp::MSP_V1; + resp.cmd = MSP_API_VERSION; + resp.result = 0; + resp.writeU8(1); + resp.writeU8(2); + resp.writeU8(3); + + Crsf::encodeMsp(frame, resp, CRSF_ADDRESS_RADIO_TRANSMITTER); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(10, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_RESP, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC + TEST_ASSERT_EQUAL_UINT8(0x30, frame.payload[2]); // status + + // ext msp v1 header + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[3]); // size + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[4]); // type // api_version(1) + + // ext msp payload + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[5]); // param1 + TEST_ASSERT_EQUAL_UINT8(2, frame.payload[6]); // param2 + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[7]); // param3 + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0x6D, frame.crc()); +} + +void test_crsf_encode_msp_v2() +{ + CrsfMessage frame; + memset(&frame, 0, sizeof(frame)); + + Msp::MspResponse resp; + resp.version = Msp::MSP_V2; + resp.cmd = MSP_API_VERSION; + resp.result = 0; + resp.writeU8(1); + resp.writeU8(2); + resp.writeU8(3); + + Crsf::encodeMsp(frame, resp, CRSF_ADDRESS_RADIO_TRANSMITTER); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(13, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_RESP, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[0]); // radio-transmitter addr + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[1]); // FC addr + TEST_ASSERT_EQUAL_UINT8(0x50, frame.payload[2]); // status flags + + // ext msp v2 header + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[3]); // flags + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[4]); // type: api_version(1) (lo) + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[5]); // type: api_version(1) (hi) + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[6]); // size (lo) + TEST_ASSERT_EQUAL_UINT8(0, frame.payload[7]); // size (hi) + + // ext msp payload + TEST_ASSERT_EQUAL_UINT8(1, frame.payload[8]); // param1 + TEST_ASSERT_EQUAL_UINT8(2, frame.payload[9]); // param2 + TEST_ASSERT_EQUAL_UINT8(3, frame.payload[10]); // param3 + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0xF8, frame.crc()); +} + +void test_crsf_decode_msp_v1() +{ + const uint8_t data[] = { + 0xc8, 0x08, 0x7a, 0xc8, 0xea, 0x32, 0x00, 0x70, 0x70, 0x4b + }; + CrsfMessage frame; + std::copy_n(data, sizeof(data), (uint8_t*)&frame); + + Msp::MspMessage m; + uint8_t origin = 0; + + Crsf::decodeMsp(frame, m, origin); + + // crsf headers + TEST_ASSERT_EQUAL_UINT8(CRSF_ADDRESS_FLIGHT_CONTROLLER, frame.addr); + TEST_ASSERT_EQUAL_UINT8(0x08, frame.size); + TEST_ASSERT_EQUAL_UINT8(CRSF_FRAMETYPE_MSP_REQ, frame.type); + + // crsf ext headers + TEST_ASSERT_EQUAL_UINT8(0xC8, frame.payload[0]); // FC addr + TEST_ASSERT_EQUAL_UINT8(0xEA, frame.payload[1]); // radio-transmitter addr (origin) + TEST_ASSERT_EQUAL_UINT8(0x32, frame.payload[2]); // status flags + + // ext msp v2 header + TEST_ASSERT_EQUAL_UINT8(0x00, frame.payload[3]); // size + TEST_ASSERT_EQUAL_UINT8(0x70, frame.payload[4]); // type: msp_pid(0x70) + + // crsf crc + TEST_ASSERT_EQUAL_UINT8(0x4B, frame.crc()); + + // origin + TEST_ASSERT_EQUAL_UINT8(0xEA, origin); +} + int main(int argc, char **argv) { UNITY_BEGIN(); @@ -304,7 +421,8 @@ int main(int argc, char **argv) RUN_TEST(test_crsf_decode_rc_struct); RUN_TEST(test_crsf_decode_rc_shift8); //RUN_TEST(test_crsf_decode_rc_shift32); - UNITY_END(); - - return 0; + RUN_TEST(test_crsf_encode_msp_v1); + RUN_TEST(test_crsf_encode_msp_v2); + RUN_TEST(test_crsf_decode_msp_v1); + return UNITY_END(); } \ No newline at end of file