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

AP_Frsky_Telem: added support for sending frsky telemetry data from scripting

This commit is contained in:
yaapu 2020-12-20 20:57:08 +01:00 committed by Peter Barker
parent 0f363809f5
commit 24382e25fc
4 changed files with 168 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp
View File

@ -9,6 +9,8 @@
#include <string.h>
AP_Frsky_SPort *AP_Frsky_SPort::singleton;
/*
* send telemetry data
* for FrSky SPort protocol (X-receivers)
@ -125,6 +127,16 @@ void AP_Frsky_SPort::send(void)
_SPort.various_call = 0;
}
break;
default:
{
// respond to custom user data polling
WITH_SEMAPHORE(_sport_push_buffer.sem);
if (_sport_push_buffer.pending && readbyte == _sport_push_buffer.packet.sensor) {
send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data);
_sport_push_buffer.pending = false;
}
}
break;
}
_SPort.sport_status = false;
}
@ -343,3 +355,93 @@ uint8_t AP_Frsky_SPort::calc_sensor_id(const uint8_t physical_id)
result += (BIT(physical_id, 0) ^ BIT(physical_id, 2) ^ BIT(physical_id, 4)) << 7;
return result;
}
/*
* prepare value for transmission through FrSky link
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t power)
{
uint16_t res = 0;
uint32_t abs_number = abs(number);
if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power
if (abs_number < 100) {
res = abs_number<<1;
} else if (abs_number < 1270) {
res = ((uint8_t)roundf(abs_number * 0.1f)<<1)|0x1;
} else { // transmit max possible value (0x7F x 10^1 = 1270)
res = 0xFF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<8;
}
} else if ((digits == 2) && (power == 2)) { // number encoded on 9 bits: 7 bits for digits + 2 for 10^power
if (abs_number < 100) {
res = abs_number<<2;
} else if (abs_number < 1000) {
res = ((uint8_t)roundf(abs_number * 0.1f)<<2)|0x1;
} else if (abs_number < 10000) {
res = ((uint8_t)roundf(abs_number * 0.01f)<<2)|0x2;
} else if (abs_number < 127000) {
res = ((uint8_t)roundf(abs_number * 0.001f)<<2)|0x3;
} else { // transmit max possible value (0x7F x 10^3 = 127000)
res = 0x1FF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<9;
}
} else if ((digits == 3) && (power == 1)) { // number encoded on 11 bits: 10 bits for digits + 1 for 10^power
if (abs_number < 1000) {
res = abs_number<<1;
} else if (abs_number < 10240) {
res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1;
} else { // transmit max possible value (0x3FF x 10^1 = 10240)
res = 0x7FF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<11;
}
} else if ((digits == 3) && (power == 2)) { // number encoded on 12 bits: 10 bits for digits + 2 for 10^power
if (abs_number < 1000) {
res = abs_number<<2;
} else if (abs_number < 10000) {
res = ((uint16_t)roundf(abs_number * 0.1f)<<2)|0x1;
} else if (abs_number < 100000) {
res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2;
} else if (abs_number < 1024000) {
res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3;
} else { // transmit max possible value (0x3FF x 10^3 = 127000)
res = 0xFFF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<12;
}
}
return res;
}
/*
* Push user data down the telemetry link by responding to sensor polling (sport)
* or by using dedicated slots in the scheduler (fport)
* for SPort and FPort protocols (X-receivers)
*/
bool AP_Frsky_SPort::sport_telemetry_push(uint8_t sensor, uint8_t frame, uint16_t appid, int32_t data)
{
WITH_SEMAPHORE(_sport_push_buffer.sem);
if (_sport_push_buffer.pending) {
return false;
}
_sport_push_buffer.packet.sensor = sensor;
_sport_push_buffer.packet.frame = frame;
_sport_push_buffer.packet.appid = appid;
_sport_push_buffer.packet.data = static_cast<uint32_t>(data);
_sport_push_buffer.pending = true;
return true;
}
namespace AP {
AP_Frsky_SPort *frsky_sport() {
return AP_Frsky_SPort::get_singleton();
}
};

30
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
View File

@ -7,9 +7,23 @@ class AP_Frsky_SPort : public AP_Frsky_Backend
public:
using AP_Frsky_Backend::AP_Frsky_Backend;
AP_Frsky_SPort(AP_HAL::UARTDriver *port) : AP_Frsky_Backend(port) {
singleton = this;
}
/* Do not allow copies */
AP_Frsky_SPort(const AP_Frsky_SPort &other) = delete;
AP_Frsky_SPort &operator=(const AP_Frsky_SPort&) = delete;
void send() override;
// send an sport packet by responding to the specified polled sensor
bool sport_telemetry_push(const uint8_t sensor, const uint8_t frame, const uint16_t appid, const int32_t data);
// utility method to pack numbers in a compact size
uint16_t prep_number(int32_t const number, const uint8_t digits, const uint8_t power);
static AP_Frsky_SPort *get_singleton(void) {
return singleton;
}
protected:
@ -22,9 +36,14 @@ protected:
} _passthrough;
uint32_t calc_gps_latlng(bool &send_latitude);
static uint8_t calc_sensor_id(const uint8_t physical_id);
struct {
sport_packet_t packet;
bool pending = false;
HAL_Semaphore sem;
} _sport_push_buffer;
private:
struct {
@ -36,4 +55,11 @@ private:
uint8_t vario_call;
uint8_t various_call;
} _SPort;
static AP_Frsky_SPort *singleton;
};
namespace AP {
AP_Frsky_SPort *frsky_sport();
};

105
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -110,6 +110,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status
set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status
set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
set_scheduler_entry(UDATA, 5000, 200); // user data
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
set_scheduler_entry(MAV, 35, 25); // mavlite
// initialize sport sensor IDs
@ -160,6 +161,12 @@ void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty)
_scheduler.packet_weight[ATTITUDE] = 45; // attitude
}
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
// when using fport raise user data priority if any packets are pending
if (_use_external_data && _sport_push_buffer.pending) {
_scheduler.packet_weight[UDATA] = 250;
} else {
_scheduler.packet_weight[UDATA] = 5000; // user data
}
}
// WFQ scheduler
@ -181,10 +188,15 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
case BATT_2:
packet_ready = AP::battery().num_instances() > 1;
break;
case UDATA:
// when using fport user data is sent by scheduler
// when using sport user data is sent responding to custom polling
packet_ready = _use_external_data && _sport_push_buffer.pending;
break;
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
case MAV:
packet_ready = !_SPort_bidir.tx_packet_queue.is_empty();
break;
case MAV:
packet_ready = !_SPort_bidir.tx_packet_queue.is_empty();
break;
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
default:
packet_ready = true;
@ -242,6 +254,15 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
case PARAM: // 0x5007 parameters
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break;
case UDATA: // user data
{
WITH_SEMAPHORE(_sport_push_buffer.sem);
if (_use_external_data && _sport_push_buffer.pending) {
send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data);
_sport_push_buffer.pending = false;
}
}
break;
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
case MAV: // mavlite
process_tx_queue();
@ -276,8 +297,17 @@ void AP_Frsky_SPort_Passthrough::send(void)
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
}
// check if we should respond to this polling byte
if (prev_byte == FRAME_HEAD && is_passthrough_byte(_passthrough.new_byte)) {
run_wfq_scheduler();
if (prev_byte == FRAME_HEAD) {
if (is_passthrough_byte(_passthrough.new_byte)) {
run_wfq_scheduler();
} else {
// respond to custom user data polling
WITH_SEMAPHORE(_sport_push_buffer.sem);
if (_sport_push_buffer.pending && _passthrough.new_byte == _sport_push_buffer.packet.sensor) {
send_sport_frame(_sport_push_buffer.packet.frame, _sport_push_buffer.packet.appid, _sport_push_buffer.packet.data);
_sport_push_buffer.pending = false;
}
}
}
}
@ -597,71 +627,6 @@ bool AP_Frsky_SPort_Passthrough::get_telem_data(sport_packet_t* packet_array, ui
return idx > 0;
}
/*
* prepare value for transmission through FrSky link
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits, uint8_t power)
{
uint16_t res = 0;
uint32_t abs_number = abs(number);
if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power
if (abs_number < 100) {
res = abs_number<<1;
} else if (abs_number < 1270) {
res = ((uint8_t)roundf(abs_number * 0.1f)<<1)|0x1;
} else { // transmit max possible value (0x7F x 10^1 = 1270)
res = 0xFF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<8;
}
} else if ((digits == 2) && (power == 2)) { // number encoded on 9 bits: 7 bits for digits + 2 for 10^power
if (abs_number < 100) {
res = abs_number<<2;
} else if (abs_number < 1000) {
res = ((uint8_t)roundf(abs_number * 0.1f)<<2)|0x1;
} else if (abs_number < 10000) {
res = ((uint8_t)roundf(abs_number * 0.01f)<<2)|0x2;
} else if (abs_number < 127000) {
res = ((uint8_t)roundf(abs_number * 0.001f)<<2)|0x3;
} else { // transmit max possible value (0x7F x 10^3 = 127000)
res = 0x1FF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<9;
}
} else if ((digits == 3) && (power == 1)) { // number encoded on 11 bits: 10 bits for digits + 1 for 10^power
if (abs_number < 1000) {
res = abs_number<<1;
} else if (abs_number < 10240) {
res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1;
} else { // transmit max possible value (0x3FF x 10^1 = 10240)
res = 0x7FF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<11;
}
} else if ((digits == 3) && (power == 2)) { // number encoded on 12 bits: 10 bits for digits + 2 for 10^power
if (abs_number < 1000) {
res = abs_number<<2;
} else if (abs_number < 10000) {
res = ((uint16_t)roundf(abs_number * 0.1f)<<2)|0x1;
} else if (abs_number < 100000) {
res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2;
} else if (abs_number < 1024000) {
res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3;
} else { // transmit max possible value (0x3FF x 10^3 = 127000)
res = 0xFFF;
}
if (number < 0) { // if number is negative, add sign bit in front
res |= 0x1<<12;
}
}
return res;
}
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
/*
allow external transports (e.g. FPort), to supply telemetry data

9
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -13,10 +13,7 @@
#include "AP_Frsky_MAVlite_MAVliteToSPort.h"
#include "AP_Frsky_MAVliteMsgHandler.h"
#define FRSKY_WFQ_TIME_SLOT_MAX 12U
#define SPORT_TX_PACKET_DUPLICATES 1 // number of duplicates packets we send (fport only)
#else
#define FRSKY_WFQ_TIME_SLOT_MAX 11U
#endif
class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry
@ -25,7 +22,7 @@ public:
AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data, AP_Frsky_Parameters *&frsky_parameters) :
AP_Frsky_SPort(port),
AP_RCTelemetry(FRSKY_WFQ_TIME_SLOT_MAX),
AP_RCTelemetry(WFQ_LAST_ITEM),
_use_external_data(use_external_data),
_frsky_parameters(frsky_parameters)
{
@ -70,8 +67,9 @@ public:
BATT_2 = 8, // 0x5008 Battery 2 status
BATT_1 = 9, // 0x5008 Battery 1 status
PARAM = 10, // 0x5007 parameters
UDATA = 11, // user data
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
MAV = 11, // mavlite
MAV = 12, // mavlite
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
WFQ_LAST_ITEM // must be last
};
@ -151,7 +149,6 @@ private:
uint8_t _paramID;
uint32_t calc_gps_status(void);
uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power);
static AP_Frsky_SPort_Passthrough *singleton;
};