Skip to content

Commit

Permalink
msp over crsf
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Aug 4, 2024
1 parent c66db69 commit 2319744
Show file tree
Hide file tree
Showing 7 changed files with 373 additions and 160 deletions.
69 changes: 38 additions & 31 deletions lib/Espfc/src/Device/InputCRSF.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
#include <algorithm>
#include "InputCRSF.h"
#include "Utils/MemoryHelper.h"

Expand All @@ -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;
}

Expand Down Expand Up @@ -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<uint8_t*>(&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;
}
Expand All @@ -126,49 +125,57 @@ 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:
break;
}
}

void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfFrame& f)
void FAST_CODE_ATTR InputCRSF::applyLinkStats(const CrsfMessage& msg)
{
const CrsfLinkStats* frame = reinterpret_cast<const CrsfLinkStats*>(f.message.payload);
(void)frame;
const CrsfLinkStats* stats = reinterpret_cast<const CrsfLinkStats*>(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<const CrsfData*>(f.message.payload);
Crsf::decodeRcDataShift8(_channels, frame);
const CrsfData* data = reinterpret_cast<const CrsfData*>(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;
}

}
Expand Down
12 changes: 6 additions & 6 deletions lib/Espfc/src/Device/InputCRSF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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;
};
Expand Down
95 changes: 84 additions & 11 deletions lib/Espfc/src/Rc/Crsf.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@

#include <Arduino.h>
#include "Crsf.h"
#include "Math/Utils.h"
#include "Math/Crc.h"
Expand Down Expand Up @@ -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<const Msp::MspHeaderV1*>(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<const Msp::MspHeaderV2*>(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)
Expand All @@ -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;
}
Expand Down
39 changes: 31 additions & 8 deletions lib/Espfc/src/Rc/Crsf.h
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,9 @@
#pragma once

#include <cstdint>
#include <cstddef>
#include "Math/Crc.h"
#include "Msp/Msp.h"

namespace Espfc {

Expand Down Expand Up @@ -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;
Expand All @@ -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++);
Expand All @@ -146,23 +167,25 @@ 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
{
public:
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);
};

}
Expand Down
Loading

0 comments on commit 2319744

Please sign in to comment.