aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RCProtocol: added support for passthrough telemetry over crossfire

Browse Source
This commit is contained in:
yaapu 2020-12-10 10:28:41 +01:00 committed by Peter Barker
parent 4933544489
commit 5ef20aec27
6 changed files with 61 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp
View File

@ -187,12 +187,6 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
if (_frame_ofs == _frame.length + CSRF_HEADER_LEN) {
log_data(AP_RCProtocol::CRSF, timestamp_us, (const uint8_t*)&_frame, _frame_ofs - CSRF_HEADER_LEN);
if ((timestamp_us - _last_frame_time_us) <= CRSF_INTER_FRAME_TIME_US_150HZ + CRSF_MAX_FRAME_TIME_US) {
_fast_telem = true;
} else {
_fast_telem = false;
}
// we consumed the partial frame, reset
_frame_ofs = 0;
@ -209,9 +203,9 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte)
_last_frame_time_us = timestamp_us;
// decode here
if (decode_csrf_packet()) {
add_input(MAX_CHANNELS, _channels, false, _current_rssi);
add_input(MAX_CHANNELS, _channels, false, _link_status.rssi);
}
}
}
}
void AP_RCProtocol_CRSF::update(void)
@ -328,19 +322,6 @@ bool AP_RCProtocol_CRSF::process_telemetry(bool check_constraint)
return false;
#endif
}
/*
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
incoming control frame
*/
uint64_t tend = uart->receive_time_constraint_us(1);
uint64_t now = AP_HAL::micros64();
uint64_t tdelay = now - tend;
if (tdelay > CRSF_MAX_FRAME_TIME_US && check_constraint) {
// we've been too slow in responding
return false;
}
write_frame(&_telemetry_frame);
// get fresh telem_data in the next call
telem_available = false;
@ -361,13 +342,15 @@ void AP_RCProtocol_CRSF::process_link_stats_frame(const void* data)
}
// AP rssi: -1 for unknown, 0 for no link, 255 for maximum link
if (rssi_dbm < 50) {
_current_rssi = 255;
_link_status.rssi = 255;
} else if (rssi_dbm > 120) {
_current_rssi = 0;
_link_status.rssi = 0;
} else {
// this is an approximation recommended by Remo from TBS
_current_rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
_link_status.rssi = int16_t(roundf((1.0f - (rssi_dbm - 50.0f) / 70.0f) * 255.0f));
}
_link_status.rf_mode = static_cast<RFMode>(MIN(link->rf_mode, 3U));
}
// process a byte provided by a uart
@ -397,4 +380,3 @@ namespace AP {
return AP_RCProtocol_CRSF::get_singleton();
}
};

32
libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h
View File

@ -55,6 +55,9 @@ public:
CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
CRSF_FRAMETYPE_COMMAND = 0x32,
// Custom Telemetry Frames 0x7F,0x80
CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY = 0x7F, // as suggested by Remo Masina for fw < 4.06
CRSF_FRAMETYPE_AP_CUSTOM_TELEM = 0x80, // reserved for ArduPilot by TBS, requires fw >= 4.06
};
// Command IDs for CRSF_FRAMETYPE_COMMAND
@ -116,6 +119,13 @@ public:
CRSF_COMMAND_RX_BIND = 0x01,
};
// SubType IDs for CRSF_FRAMETYPE_CUSTOM_TELEM
enum CustomTelemSubTypeID : uint8_t {
CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH = 0xF0,
CRSF_AP_CUSTOM_TELEM_STATUS_TEXT = 0xF1,
CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH = 0xF2,
};
enum DeviceAddress {
CRSF_ADDRESS_BROADCAST = 0x00,
CRSF_ADDRESS_USB = 0x10,
@ -161,6 +171,24 @@ public:
int8_t downlink_dnr; // ( db )
} PACKED;
enum class RFMode : uint8_t {
CRSF_RF_MODE_4HZ = 0,
CRSF_RF_MODE_50HZ,
CRSF_RF_MODE_150HZ,
CRSF_RF_MODE_UNKNOWN,
};
struct LinkStatus {
int16_t rssi = -1;
RFMode rf_mode;
};
// this will be used by AP_CRSF_Telem to access link status data
// from within AP_RCProtocol_CRSF thread so no need for cross-thread synch
const volatile LinkStatus& get_link_status() const {
return _link_status;
}
private:
struct Frame _frame;
struct Frame _telemetry_frame;
@ -187,8 +215,8 @@ private:
uint32_t _last_rx_time_us;
uint32_t _start_frame_time_us;
bool telem_available;
bool _fast_telem; // is 150Hz telemetry active
int16_t _current_rssi = -1;
volatile struct LinkStatus _link_status;
AP_HAL::UARTDriver *_uart;

17
libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp
View File

@ -148,11 +148,12 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
status text messages
*/
if (!telem_data.available) {
if (!AP_Frsky_Telem::get_telem_data(telem_data.frame, telem_data.appid, telem_data.data)) {
uint8_t packet_count;
if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
// nothing to send, send a null frame
telem_data.frame = 0x00;
telem_data.appid = 0x00;
telem_data.data = 0x00;
telem_data.packet.frame = 0x00;
telem_data.packet.appid = 0x00;
telem_data.packet.data = 0x00;
}
telem_data.available = true;
}
@ -172,10 +173,10 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
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[2] = telem_data.packet.frame;
buf[3] = telem_data.packet.appid & 0xFF;
buf[4] = telem_data.packet.appid >> 8;
memcpy(&buf[5], &telem_data.packet.data, 4);
buf[9] = crc_sum8(&buf[0], 9);
// perform byte stuffing per FPort spec

6
libraries/AP_RCProtocol/AP_RCProtocol_FPort.h
View File

@ -17,6 +17,8 @@
#pragma once
#include <AP_Frsky_Telem/AP_Frsky_SPort.h>
#include "AP_RCProtocol.h"
#include "SoftSerial.h"
@ -50,9 +52,7 @@ private:
struct {
bool available = false;
uint32_t data;
uint16_t appid;
uint8_t frame;
AP_Frsky_SPort::sport_packet_t packet;
} telem_data;
// receiver sends 0x10 when ready to receive telemetry frames (R-XSR)

17
libraries/AP_RCProtocol/AP_RCProtocol_FPort2.cpp
View File

@ -137,10 +137,11 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
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)) {
telem_data.frame = 0x00;
telem_data.appid = 0x00;
telem_data.data = 0x00;
uint8_t packet_count;
if (!AP_Frsky_Telem::get_telem_data(&telem_data.packet, packet_count, 1)) {
telem_data.packet.frame = 0x00;
telem_data.packet.appid = 0x00;
telem_data.packet.data = 0x00;
}
telem_data.available = true;
}
@ -169,10 +170,10 @@ void AP_RCProtocol_FPort2::decode_downlink(const FPort2_Frame &frame)
buf[1] = frame.type;
// do not consume telemetry data for invalid downlink frames, i.e. incoming prim == 0x00
if (frame.downlink.prim != FPORT2_PRIM_NULL) {
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[2] = telem_data.packet.frame;
buf[3] = telem_data.packet.appid & 0xFF;
buf[4] = telem_data.packet.appid >> 8;
memcpy(&buf[5], &telem_data.packet.data, 4);
// get fresh telem_data in the next call
telem_data.available = false;
}

6
libraries/AP_RCProtocol/AP_RCProtocol_FPort2.h
View File

@ -17,6 +17,8 @@
#pragma once
#include <AP_Frsky_Telem/AP_Frsky_SPort.h>
#include "AP_RCProtocol.h"
#include "SoftSerial.h"
@ -53,8 +55,6 @@ private:
struct {
bool available;
uint32_t data;
uint16_t appid;
uint8_t frame;
AP_Frsky_SPort::sport_packet_t packet;
} telem_data;
};