Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RCProtocol: handle 0x10 and 0x00 frames, check scheduler delay and fix incoming crc 

Respond to 0x10 when the rx is controlling bandwidth, else respond only to 0x00
but in this case never exceed a max number of consecutive frames.
Always respond to polling and if necessary send a null frame.
moved delay check after get_telem_data() to account for scheduler time
align incoming to outgoing crc
This commit is contained in:
yaapu 2020-02-17 14:25:00 -08:00 committed by Andrew Tridgell
parent 0c5618c9e7
commit b4bdc76a53
1 changed files with 94 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

142
libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp
View File

@ -42,6 +42,12 @@ extern const AP_HAL::HAL& hal;
#define FPORT_TYPE_CONTROL 0
#define FPORT_TYPE_DOWNLINK 1
#define FPORT_PRIM_NULL 0x00
#define FPORT_PRIM_DATA 0x10
#define FPORT_PRIM_READ 0x30
#define FPORT_PRIM_WRITE 0x31
#define MAX_FPORT_CONSECUTIVE_FRAMES 2
struct PACKED FPort_Frame {
uint8_t header; // 0x7E
@ -80,6 +86,18 @@ struct PACKED FPort_Frame {
};
};
struct {
bool available = false;
uint32_t data;
uint16_t appid;
uint8_t frame;
} telem_data;
// receiver sends 0x10 when ready to receive telemetry frames (R-XSR)
bool rx_driven_frame_rate = false;
// if the receiver is not controlling frame rate apply a constraint on consecutive frames
uint8_t consecutive_telemetry_frame_count;
static_assert(sizeof(FPort_Frame) == FPORT_CONTROL_FRAME_SIZE, "FPort_Frame incorrect size");
// constructor
@ -130,10 +148,30 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame)
void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
{
#if !APM_BUILD_TYPE(APM_BUILD_iofirmware)
if (frame.downlink.prim != 0x10 && frame.downlink.prim != 0x00) {
// only respond to frame types 0x00 or 0x10
return;
switch (frame.downlink.prim) {
case FPORT_PRIM_DATA:
// we've seen at least one 0x10 frame
rx_driven_frame_rate = true;
break;
case FPORT_PRIM_NULL:
if (rx_driven_frame_rate) {
return;
}
// with 0x00 and no rx control we have a constraint
// on max consecutive frames
if (consecutive_telemetry_frame_count >= MAX_FPORT_CONSECUTIVE_FRAMES) {
consecutive_telemetry_frame_count = 0;
return;
} else {
consecutive_telemetry_frame_count++;
}
break;
case FPORT_PRIM_READ:
case FPORT_PRIM_WRITE:
// do not respond to 0x30 and 0x31
return;
}
/*
if we are getting FPORT over a UART then we can ask the FrSky
telem library for some passthrough data to send back, enabling
@ -146,6 +184,21 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
return;
}
/*
get SPort data from FRSky_Telem or send a null frame.
We save the data to a variable so in case we're late we'll
send it in the next call, this prevents corruption of
status text messages
*/
if (!telem_data.available) {
if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
// nothing to send, send a null frame
telem_data.frame = 0x00;
telem_data.appid = 0x00;
telem_data.data = 0x00;
}
telem_data.available = true;
}
/*
check that we haven't been too slow in responding to the new
UART data. If we respond too late then we will corrupt the next
@ -158,54 +211,45 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
// we've been too slow in responding
return;
}
uint8_t buf[10];
/*
get the SPort data from FRSky_Telem and send it as an uplink
packet
*/
uint8_t frametype;
uint16_t appid;
uint32_t data;
if (AP_Frsky_Telem::get_telem_data(frametype, appid, data)) {
uint8_t buf[10];
buf[0] = 0x08;
buf[1] = 0x81;
buf[2] = telem_data.frame;
buf[3] = telem_data.appid & 0xFF;
buf[4] = telem_data.appid >> 8;
memcpy(&buf[5], &telem_data.data, 4);
buf[0] = 0x08;
buf[1] = 0x81;
buf[2] = frametype;
buf[3] = appid & 0xFF;
buf[4] = appid >> 8;
memcpy(&buf[5], &data, 4);
uint16_t sum = 0;
for (uint8_t i=0; i<sizeof(buf)-1; i++) {
sum += buf[i];
sum += sum >> 8;
sum &= 0xFF;
}
sum = 0xff - ((sum & 0xff) + (sum >> 8));
buf[9] = sum;
// perform byte stuffing per FPort spec
uint8_t len = 0;
uint8_t buf2[sizeof(buf)*2+1];
if (rc().fport_pad()) {
// this padding helps on some uarts that have hw pullups
buf2[len++] = 0xff;
}
for (uint8_t i=0; i<sizeof(buf); i++) {
uint8_t c = buf[i];
if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
buf2[len++] = FRAME_DLE;
buf2[len++] = c ^ FRAME_XOR;
} else {
buf2[len++] = c;
}
}
uart->write(buf2, len);
uint16_t sum = 0;
for (uint8_t i=0; i<sizeof(buf)-1; i++) {
sum += buf[i];
sum += sum >> 8;
sum &= 0xFF;
}
sum = 0xff - ((sum & 0xff) + (sum >> 8));
buf[9] = (uint8_t)sum;
// perform byte stuffing per FPort spec
uint8_t len = 0;
uint8_t buf2[sizeof(buf)*2+1];
if (rc().fport_pad()) {
// this padding helps on some uarts that have hw pullups
buf2[len++] = 0xff;
}
for (uint8_t i=0; i<sizeof(buf); i++) {
uint8_t c = buf[i];
if (c == FRAME_DLE || buf[i] == FRAME_HEAD) {
buf2[len++] = FRAME_DLE;
buf2[len++] = c ^ FRAME_XOR;
} else {
buf2[len++] = c;
}
}
uart->write(buf2, len);
// get fresh telem_data in the next call
telem_data.available = false;
#endif
}
@ -306,6 +350,8 @@ bool AP_RCProtocol_FPort::check_checksum(void)
uint16_t sum = 0;
for (uint8_t i=0; i<len; i++) {
sum += b[i];
sum += sum >> 8;
sum &= 0xFF;
}
sum = (sum & 0xff) + (sum >> 8);
return sum == 0xff;