zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_FrSky_Telem: allow for external library to send SPort data 

this provides the API needed for AP_RCProtocol_FPort to send
passthrough data over FPORT
zr-v5.1
Andrew Tridgell 5 years ago
parent
e9dded98ce
commit
49f81fbd18
2 changed files with 113 additions and 25 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 110
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
  2. 28
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

110
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Unescape View File

@ -37,9 +37,49 @@ @@ -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() @@ -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);
}
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();
}
// 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
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) @@ -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 @@ -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
Unescape View File

@ -114,6 +114,8 @@ class AP_Frsky_Telem { @@ -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: @@ -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: @@ -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();
};

Write Preview
Loading…
Cancel
Save