Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_FrSky_Telem: allow for external library to send SPort data

Browse Source 
this provides the API needed for AP_RCProtocol_FPort to send
passthrough data over FPORT
This commit is contained in:
Andrew Tridgell 2019-12-03 14:08:12 +11:00
parent e9dded98ce
commit 49f81fbd18
2 changed files with 114 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

112
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -37,9 +37,49 @@
extern const AP_HAL::HAL& hal;
AP_Frsky_Telem *AP_Frsky_Telem::singleton;
AP_Frsky_Telem::AP_Frsky_Telem(void) :
_statustext_queue(FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY)
{
singleton = this;
}
AP_Frsky_Telem::~AP_Frsky_Telem(void)
{
singleton = nullptr;
}
/*
setup ready for passthrough telem
*/
void AP_Frsky_Telem::setup_passthrough(void)
{
// make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
// add firmware and frame info to message queue
const char* _frame_string = gcs().frame_string();
if (_frame_string == nullptr) {
queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string);
} else {
char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string);
queue_message(MAV_SEVERITY_INFO, firmware_buf);
}
// initialize packet weights for the WFQ scheduler
// weight[i] = 1/_passthrough.packet_period[i]
// rate[i] = LinkRate * ( weight[i] / (sum(weight[1-n])) )
_passthrough.packet_weight[0] = 35; // 0x5000 status text (dynamic)
_passthrough.packet_weight[1] = 50; // 0x5006 Attitude and range (dynamic)
_passthrough.packet_weight[2] = 550; // 0x800 GPS lat (600 with 1 sensor)
_passthrough.packet_weight[3] = 550; // 0x800 GPS lon (600 with 1 sensor)
_passthrough.packet_weight[4] = 400; // 0x5005 Vel and Yaw
_passthrough.packet_weight[5] = 700; // 0x5001 AP status
_passthrough.packet_weight[6] = 700; // 0x5002 GPS Status
_passthrough.packet_weight[7] = 400; // 0x5004 Home
_passthrough.packet_weight[8] = 1300; // 0x5008 Battery 2 status
_passthrough.packet_weight[9] = 1300; // 0x5003 Battery 1 status
_passthrough.packet_weight[10] = 1700; // 0x5007 parameters
}
/*
@ -56,33 +96,20 @@ bool AP_Frsky_Telem::init()
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers)
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough; // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
// make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
// add firmware and frame info to message queue
const char* _frame_string = gcs().frame_string();
if (_frame_string == nullptr) {
queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string);
} else {
char firmware_buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", AP::fwversion().fw_string, _frame_string);
queue_message(MAV_SEVERITY_INFO, firmware_buf);
}
// initialize packet weights for the WFQ scheduler
// weight[i] = 1/_passthrough.packet_period[i]
// rate[i] = LinkRate * ( weight[i] / (sum(weight[1-n])) )
_passthrough.packet_weight[0] = 35; // 0x5000 status text (dynamic)
_passthrough.packet_weight[1] = 50; // 0x5006 Attitude and range (dynamic)
_passthrough.packet_weight[2] = 550; // 0x800 GPS lat (600 with 1 sensor)
_passthrough.packet_weight[3] = 550; // 0x800 GPS lon (600 with 1 sensor)
_passthrough.packet_weight[4] = 400; // 0x5005 Vel and Yaw
_passthrough.packet_weight[5] = 700; // 0x5001 AP status
_passthrough.packet_weight[6] = 700; // 0x5002 GPS Status
_passthrough.packet_weight[7] = 400; // 0x5004 Home
_passthrough.packet_weight[8] = 1300; // 0x5008 Battery 2 status
_passthrough.packet_weight[9] = 1300; // 0x5003 Battery 1 status
_passthrough.packet_weight[10] = 1700; // 0x5007 parameters
setup_passthrough();
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_RCIN, 0))) {
_protocol = AP_SerialManager::SerialProtocol_RCIN; // FrSky SPort over FPort
setup_passthrough();
}
if (_protocol == AP_SerialManager::SerialProtocol_RCIN) {
use_external_data = true;
setup_passthrough();
// we don't setup the thread, as output is driven by the AP_RCProtocol library
// calling get_telem_data();
return true;
}
if (_port != nullptr) {
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::loop, void),
"FrSky",
@ -499,6 +526,13 @@ void AP_Frsky_Telem::send_byte(uint8_t byte)
*/
void AP_Frsky_Telem::send_uint32(uint8_t frame, uint16_t id, uint32_t data)
{
if (use_external_data) {
external_data.frame = frame;
external_data.appid = id;
external_data.data = data;
external_data.pending = true;
return;
}
send_byte(frame); // frame type
uint8_t *bytes = (uint8_t*)&id;
send_byte(bytes[0]); // LSB
@ -1063,3 +1097,29 @@ uint32_t AP_Frsky_Telem::sensor_status_flags() const
return ~health & enabled & present;
}
/*
fetch Sport data for an external transport, such as FPort
*/
bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
{
if (!use_external_data) {
// not initialised for external data
return false;
}
passthrough_wfq_adaptive_scheduler(0);
if (!external_data.pending) {
return false;
}
frame = external_data.frame;
appid = external_data.appid;
data = external_data.data;
external_data.pending = false;
return true;
}
namespace AP {
AP_Frsky_Telem *frsky_telem() {
return AP_Frsky_Telem::get_singleton();
}
};

28
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -114,6 +114,8 @@ class AP_Frsky_Telem {
public:
AP_Frsky_Telem();
~AP_Frsky_Telem();
/* Do not allow copies */
AP_Frsky_Telem(const AP_Frsky_Telem &other) = delete;
AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete;
@ -130,6 +132,13 @@ public:
// functioning correctly
uint32_t sensor_status_flags() const;
static AP_Frsky_Telem *get_singleton(void) {
return singleton;
}
// get next telemetry data for external consumers of SPort data
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
private:
AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
@ -244,4 +253,23 @@ private:
void calc_nav_alt(void);
float format_gps(float dec);
void calc_gps_position(void);
// setup ready for passthrough operation
void setup_passthrough(void);
static AP_Frsky_Telem *singleton;
// use_external_data is set when this library will
// be providing data to another transport, such as FPort
bool use_external_data;
struct {
uint8_t frame;
uint16_t appid;
uint32_t data;
bool pending;
} external_data;
};
namespace AP {
AP_Frsky_Telem *frsky_telem();
};