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

AP_Frsky_Telem: frontend/backend split

zr-v5.1
Peter Barker 4 years ago committed by Peter Barker
parent
fb467da473
commit
05d7950261
8 changed files with 354 additions and 203 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 25
      libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
  2. 66
      libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
  3. 23
      libraries/AP_Frsky_Telem/AP_Frsky_D.h
  4. 38
      libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
  5. 30
      libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
  6. 96
      libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
  7. 137
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
  8. 142
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

25
libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
Unescape View File

@ -0,0 +1,25 @@
#include "AP_Frsky_Backend.h"
extern const AP_HAL::HAL& hal;
bool AP_Frsky_Backend::init()
{
// if SPort Passthrough is using external data then it will
// override this to do nothing:
return init_serial_port();
}
bool AP_Frsky_Backend::init_serial_port()
{
if (!hal.scheduler->thread_create(
FUNCTOR_BIND_MEMBER(&AP_Frsky_Backend::loop, void),
"FrSky",
1024,
AP_HAL::Scheduler::PRIORITY_RCIN,
1)) {
return false;
}
// we don't want flow control for either protocol
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
return true;
}

66
libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
Unescape View File

@ -0,0 +1,66 @@
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
class AP_Frsky_Backend {
public:
AP_Frsky_Backend(AP_HAL::UARTDriver *port) :
_port(port) { }
virtual ~AP_Frsky_Backend() {}
virtual bool init();
virtual void send() = 0;
// SPort is at 57600, D overrides this
virtual uint32_t initial_baud() const { return 57600; }
// get next telemetry data for external consumers of SPort data
virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) {
return false;
}
virtual void queue_text_message(MAV_SEVERITY severity, const char *text) { }
protected:
AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver
virtual bool init_serial_port();
// methods related to the nuts-and-bolts of sending data
void send_byte(uint8_t value);
void send_uint16(uint16_t id, uint16_t data);
void calc_nav_alt(void);
void calc_gps_position(void);
float get_vspeed_ms(void);
// methods to convert flight controller data to FrSky D or SPort format
float format_gps(float dec);
struct
{
int32_t vario_vspd;
char lat_ns, lon_ew;
uint16_t latdddmm;
uint16_t latmmmm;
uint16_t londddmm;
uint16_t lonmmmm;
uint16_t alt_gps_meters;
uint16_t alt_gps_cm;
uint16_t alt_nav_meters;
uint16_t alt_nav_cm;
int16_t speed_in_meter;
uint16_t speed_in_centimeter;
uint16_t yaw;
} _SPort_data;
private:
void loop(void);
};

23
libraries/AP_Frsky_Telem/AP_Frsky_D.h
Unescape View File

@ -0,0 +1,23 @@
#pragma once
#include "AP_Frsky_Backend.h"
class AP_Frsky_D : public AP_Frsky_Backend {
public:
using AP_Frsky_Backend::AP_Frsky_Backend;
protected:
void send() override;
uint32_t initial_baud() const override { return 9600; }
private:
struct {
uint32_t last_200ms_frame;
uint32_t last_1000ms_frame;
} _D;
};

38
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
Unescape View File

@ -0,0 +1,38 @@
#pragma once
#include "AP_Frsky_Backend.h"
class AP_Frsky_SPort : public AP_Frsky_Backend {
public:
using AP_Frsky_Backend::AP_Frsky_Backend;
void send() override;
protected:
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
struct PACKED
{
bool send_latitude; // sizeof(bool) = 4 ?
uint32_t gps_lng_sample;
uint8_t new_byte;
} _passthrough;
uint32_t calc_gps_latlng(bool *send_latitude);
private:
struct
{
bool sport_status;
bool gps_refresh;
bool vario_refresh;
uint8_t fas_call;
uint8_t gps_call;
uint8_t vario_call;
uint8_t various_call;
} _SPort;
};

30
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
Unescape View File

@ -0,0 +1,30 @@
#include "AP_Frsky_SPort_Passthrough.h"
bool AP_Frsky_SPort_Passthrough::init()
{
if (!AP_RCTelemetry::init()) {
return false;
}
return AP_Frsky_SPort::init();
}
bool AP_Frsky_SPort_Passthrough::init_serial_port()
{
if (_use_external_data) {
return true;
}
return AP_Frsky_SPort::init_serial_port();
}
void AP_Frsky_SPort_Passthrough::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data)
{
if (_use_external_data) {
external_data.frame = frame;
external_data.appid = appid;
external_data.data = data;
external_data.pending = true;
return;
}
return AP_Frsky_SPort::send_sport_frame(frame, appid, data);
}

96
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
Unescape View File

@ -0,0 +1,96 @@
#pragma once
#include "AP_Frsky_SPort.h"
#include <AP_RCTelemetry/AP_RCTelemetry.h>
// for fair scheduler
#define TIME_SLOT_MAX 11
class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry {
public:
AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) :
AP_Frsky_SPort(port),
AP_RCTelemetry(TIME_SLOT_MAX),
_use_external_data(use_external_data)
{ }
bool init() override;
bool init_serial_port() override;
// passthrough WFQ scheduler
bool is_packet_ready(uint8_t idx, bool queue_empty) override;
void process_packet(uint8_t idx) override;
void adjust_packet_weight(bool queue_empty) override;
// setup ready for passthrough operation
void setup_wfq_scheduler(void) override;
bool get_next_msg_chunk(void) override;
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override;
void queue_text_message(MAV_SEVERITY severity, const char *text) override {
AP_RCTelemetry::queue_message(severity, text);
}
protected:
void send() override;
private:
enum PassthroughParam : uint8_t {
FRAME_TYPE = 1,
BATT_FS_VOLTAGE = 2,
BATT_FS_CAPACITY = 3,
BATT_CAPACITY_1 = 4,
BATT_CAPACITY_2 = 5
};
enum PassthroughPacketType : uint8_t {
TEXT = 0, // 0x5000 status text (dynamic)
ATTITUDE = 1, // 0x5006 Attitude and range (dynamic)
GPS_LAT = 2, // 0x800 GPS lat
GPS_LON = 3, // 0x800 GPS lon
VEL_YAW = 4, // 0x5005 Vel and Yaw
AP_STATUS = 5, // 0x5001 AP status
GPS_STATUS = 6, // 0x5002 GPS status
HOME = 7, // 0x5004 Home
BATT_2 = 8, // 0x5008 Battery 2 status
BATT_1 = 9, // 0x5008 Battery 1 status
PARAM = 10 // 0x5007 parameters
};
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
uint32_t calc_param(void);
uint32_t calc_batt(uint8_t instance);
uint32_t calc_ap_status(void);
uint32_t calc_home(void);
uint32_t calc_velandyaw(void);
uint32_t calc_attiandrng(void);
// 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;
struct
{
uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent
uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
uint8_t char_index; // index of which character to get in the message
} _msg_chunk;
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
uint8_t _paramID;
uint32_t calc_gps_status(void);
uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power);
};

137
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Unescape View File

@ -34,12 +34,15 @@
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
#include <math.h> #include <math.h>
#include "AP_Frsky_D.h"
#include "AP_Frsky_SPort.h"
#include "AP_Frsky_SPort_Passthrough.h"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_Frsky_Telem *AP_Frsky_Telem::singleton; AP_Frsky_Telem *AP_Frsky_Telem::singleton;
AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) : AP_RCTelemetry(TIME_SLOT_MAX), AP_Frsky_Telem::AP_Frsky_Telem()
use_external_data(_external_data)
{ {
singleton = this; singleton = this;
} }
@ -52,7 +55,7 @@ AP_Frsky_Telem::~AP_Frsky_Telem(void)
/* /*
setup ready for passthrough telem setup ready for passthrough telem
*/ */
void AP_Frsky_Telem::setup_wfq_scheduler(void) void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
{ {
// initialize packet weights for the WFQ scheduler // initialize packet weights for the WFQ scheduler
// priority[i] = 1/_scheduler.packet_weight[i] // priority[i] = 1/_scheduler.packet_weight[i]
@ -73,39 +76,34 @@ void AP_Frsky_Telem::setup_wfq_scheduler(void)
/* /*
* init - perform required initialisation * init - perform required initialisation
*/ */
bool AP_Frsky_Telem::init() bool AP_Frsky_Telem::init(bool use_external_data)
{ {
if (use_external_data) {
return AP_RCTelemetry::init();
}
const AP_SerialManager &serial_manager = AP::serialmanager(); const AP_SerialManager &serial_manager = AP::serialmanager();
// check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports) // check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports)
if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) { AP_HAL::UARTDriver *port;
_protocol = AP_SerialManager::SerialProtocol_FrSky_D; // FrSky D protocol (D-receivers) if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) {
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) { _backend = new AP_Frsky_D(port);
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers) } else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) { _backend = new AP_Frsky_SPort(port);
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough; // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers) } else if (use_external_data || (port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
AP_RCTelemetry::init(); _backend = new AP_Frsky_SPort_Passthrough(port, use_external_data);
} }
if (_port != nullptr) { if (_backend == nullptr) {
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::loop, void), return false;
"FrSky", }
1024, AP_HAL::Scheduler::PRIORITY_RCIN, 1)) {
return false; if (!_backend->init()) {
} delete _backend;
// we don't want flow control for either protocol _backend = nullptr;
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE); return false;
return true;
} }
return false; return true;
} }
void AP_Frsky_Telem::adjust_packet_weight(bool queue_empty) void AP_Frsky_SPort_Passthrough::adjust_packet_weight(bool queue_empty)
{ {
if (!queue_empty) { if (!queue_empty) {
_scheduler.packet_weight[TEXT] = 45; // messages _scheduler.packet_weight[TEXT] = 45; // messages
@ -117,7 +115,7 @@ void AP_Frsky_Telem::adjust_packet_weight(bool queue_empty)
} }
// WFQ scheduler // WFQ scheduler
bool AP_Frsky_Telem::is_packet_ready(uint8_t idx, bool queue_empty) bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
{ {
bool packet_ready = false; bool packet_ready = false;
switch (idx) { switch (idx) {
@ -142,7 +140,7 @@ bool AP_Frsky_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
* WFQ scheduler * WFQ scheduler
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
void AP_Frsky_Telem::process_packet(uint8_t idx) void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
{ {
// send packet // send packet
switch (idx) { switch (idx) {
@ -194,7 +192,7 @@ void AP_Frsky_Telem::process_packet(uint8_t idx)
* send telemetry data * send telemetry data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
void AP_Frsky_Telem::send_SPort_Passthrough(void) void AP_Frsky_SPort_Passthrough::send(void)
{ {
int16_t numc; int16_t numc;
numc = _port->available(); numc = _port->available();
@ -225,7 +223,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
* send telemetry data * send telemetry data
* for FrSky SPort protocol (X-receivers) * for FrSky SPort protocol (X-receivers)
*/ */
void AP_Frsky_Telem::send_SPort(void) void AP_Frsky_SPort::send(void)
{ {
int16_t numc; int16_t numc;
numc = _port->available(); numc = _port->available();
@ -356,7 +354,7 @@ void AP_Frsky_Telem::send_SPort(void)
* a second frame (frame2) is sent every second (1000ms) with gps position data, and ahrs.yaw_sensor heading (instead of GPS heading) * a second frame (frame2) is sent every second (1000ms) with gps position data, and ahrs.yaw_sensor heading (instead of GPS heading)
* for FrSky D protocol (D-receivers) * for FrSky D protocol (D-receivers)
*/ */
void AP_Frsky_Telem::send_D(void) void AP_Frsky_D::send(void)
{ {
const AP_BattMonitor &_battery = AP::battery(); const AP_BattMonitor &_battery = AP::battery();
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
@ -400,32 +398,21 @@ void AP_Frsky_Telem::send_D(void)
/* /*
thread to loop handling bytes thread to loop handling bytes
*/ */
void AP_Frsky_Telem::loop(void) void AP_Frsky_Backend::loop(void)
{ {
// initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from) // initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from)
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers) _port->begin(initial_baud(), 0, 0);
_port->begin(AP_SERIALMANAGER_FRSKY_D_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
} else { // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
_port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
}
_port->set_unbuffered_writes(true);
while (true) { while (true) {
hal.scheduler->delay(1); hal.scheduler->delay(1);
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers) send();
send_D();
} else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort) { // FrSky SPort protocol (X-receivers)
send_SPort();
} else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough) { // FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
send_SPort_Passthrough();
}
} }
} }
/* /*
send 1 byte and do byte stuffing send 1 byte and do byte stuffing
*/ */
void AP_Frsky_Telem::send_byte(uint8_t byte) void AP_Frsky_Backend::send_byte(uint8_t byte)
{ {
if (byte == START_STOP_D) { if (byte == START_STOP_D) {
_port->write(0x5D); _port->write(0x5D);
@ -441,16 +428,8 @@ void AP_Frsky_Telem::send_byte(uint8_t byte)
/* /*
* send an 8 bytes SPort frame of FrSky data - for FrSky SPort protocol (X-receivers) * send an 8 bytes SPort frame of FrSky data - for FrSky SPort protocol (X-receivers)
*/ */
void AP_Frsky_Telem::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data) void AP_Frsky_SPort::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data)
{ {
if (use_external_data) {
external_data.frame = frame;
external_data.appid = appid;
external_data.data = data;
external_data.pending = true;
return;
}
uint8_t buf[8]; uint8_t buf[8];
buf[0] = frame; buf[0] = frame;
@ -503,7 +482,7 @@ void AP_Frsky_Telem::send_sport_frame(uint8_t frame, uint16_t appid, uint32_t d
/* /*
* send one uint16 frame of FrSky data - for FrSky D protocol (D-receivers) * send one uint16 frame of FrSky data - for FrSky D protocol (D-receivers)
*/ */
void AP_Frsky_Telem::send_uint16(uint16_t id, uint16_t data) void AP_Frsky_Backend::send_uint16(uint16_t id, uint16_t data)
{ {
_port->write(START_STOP_D); // send a 0x5E start byte _port->write(START_STOP_D); // send a 0x5E start byte
uint8_t *bytes = (uint8_t*)&id; uint8_t *bytes = (uint8_t*)&id;
@ -517,7 +496,7 @@ void AP_Frsky_Telem::send_uint16(uint16_t id, uint16_t data)
* grabs one "chunk" (4 bytes) of the queued message to be transmitted * grabs one "chunk" (4 bytes) of the queued message to be transmitted
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
bool AP_Frsky_Telem::get_next_msg_chunk(void) bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void)
{ {
if (!_statustext.available) { if (!_statustext.available) {
WITH_SEMAPHORE(_statustext.sem); WITH_SEMAPHORE(_statustext.sem);
@ -578,7 +557,7 @@ bool AP_Frsky_Telem::get_next_msg_chunk(void)
* prepare parameter data * prepare parameter data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_param(void) uint32_t AP_Frsky_SPort_Passthrough::calc_param(void)
{ {
const AP_BattMonitor &_battery = AP::battery(); const AP_BattMonitor &_battery = AP::battery();
@ -615,7 +594,7 @@ uint32_t AP_Frsky_Telem::calc_param(void)
* prepare gps latitude/longitude data * prepare gps latitude/longitude data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_gps_latlng(bool *send_latitude) uint32_t AP_Frsky_SPort::calc_gps_latlng(bool *send_latitude)
{ {
uint32_t latlng; uint32_t latlng;
const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw) const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
@ -643,7 +622,7 @@ uint32_t AP_Frsky_Telem::calc_gps_latlng(bool *send_latitude)
* prepare gps status data * prepare gps status data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_gps_status(void) uint32_t AP_Frsky_SPort_Passthrough::calc_gps_status(void)
{ {
const AP_GPS &gps = AP::gps(); const AP_GPS &gps = AP::gps();
@ -667,7 +646,7 @@ uint32_t AP_Frsky_Telem::calc_gps_status(void)
* prepare battery data * prepare battery data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance) uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance)
{ {
const AP_BattMonitor &_battery = AP::battery(); const AP_BattMonitor &_battery = AP::battery();
@ -693,7 +672,7 @@ uint32_t AP_Frsky_Telem::calc_batt(uint8_t instance)
* prepare various autopilot status data * prepare various autopilot status data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_ap_status(void) uint32_t AP_Frsky_SPort_Passthrough::calc_ap_status(void)
{ {
uint32_t ap_status; uint32_t ap_status;
@ -724,7 +703,7 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
* prepare home position related data * prepare home position related data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_home(void) uint32_t AP_Frsky_SPort_Passthrough::calc_home(void)
{ {
uint32_t home = 0; uint32_t home = 0;
Location loc; Location loc;
@ -763,7 +742,7 @@ uint32_t AP_Frsky_Telem::calc_home(void)
* prepare velocity and yaw data * prepare velocity and yaw data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_velandyaw(void) uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void)
{ {
float vspd = get_vspeed_ms(); float vspd = get_vspeed_ms();
// vertical velocity in dm/s // vertical velocity in dm/s
@ -786,7 +765,7 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
* prepare attitude (roll, pitch) and range data * prepare attitude (roll, pitch) and range data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::calc_attiandrng(void) uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
{ {
const RangeFinder *_rng = RangeFinder::get_singleton(); const RangeFinder *_rng = RangeFinder::get_singleton();
@ -805,7 +784,7 @@ uint32_t AP_Frsky_Telem::calc_attiandrng(void)
* prepare value for transmission through FrSky link * prepare value for transmission through FrSky link
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t power) uint16_t AP_Frsky_SPort_Passthrough::prep_number(int32_t number, uint8_t digits, uint8_t power)
{ {
uint16_t res = 0; uint16_t res = 0;
uint32_t abs_number = abs(number); uint32_t abs_number = abs(number);
@ -870,7 +849,7 @@ uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t pow
* get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
* for FrSky D and SPort protocols * for FrSky D and SPort protocols
*/ */
float AP_Frsky_Telem::get_vspeed_ms(void) float AP_Frsky_Backend::get_vspeed_ms(void)
{ {
{// release semaphore as soon as possible {// release semaphore as soon as possible
@ -891,7 +870,7 @@ float AP_Frsky_Telem::get_vspeed_ms(void)
* prepare altitude between vehicle and home location data * prepare altitude between vehicle and home location data
* for FrSky D and SPort protocols * for FrSky D and SPort protocols
*/ */
void AP_Frsky_Telem::calc_nav_alt(void) void AP_Frsky_Backend::calc_nav_alt(void)
{ {
_SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s
@ -916,7 +895,7 @@ void AP_Frsky_Telem::calc_nav_alt(void)
* format the decimal latitude/longitude to the required degrees/minutes * format the decimal latitude/longitude to the required degrees/minutes
* for FrSky D and SPort protocols * for FrSky D and SPort protocols
*/ */
float AP_Frsky_Telem::format_gps(float dec) float AP_Frsky_Backend::format_gps(float dec)
{ {
uint8_t dm_deg = (uint8_t) dec; uint8_t dm_deg = (uint8_t) dec;
return (dm_deg * 100.0f) + (dec - dm_deg) * 60; return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
@ -926,7 +905,7 @@ float AP_Frsky_Telem::format_gps(float dec)
* prepare gps data * prepare gps data
* for FrSky D and SPort protocols * for FrSky D and SPort protocols
*/ */
void AP_Frsky_Telem::calc_gps_position(void) void AP_Frsky_Backend::calc_gps_position(void)
{ {
float lat; float lat;
float lon; float lon;
@ -971,7 +950,7 @@ void AP_Frsky_Telem::calc_gps_position(void)
/* /*
fetch Sport data for an external transport, such as FPort 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) bool AP_Frsky_SPort_Passthrough::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
{ {
run_wfq_scheduler(); run_wfq_scheduler();
if (!external_data.pending) { if (!external_data.pending) {
@ -984,6 +963,14 @@ bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &
return true; return true;
} }
bool AP_Frsky_Telem::_get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
{
if (_backend == nullptr) {
return false;
}
return _backend->get_telem_data(frame, appid, data);
}
/* /*
fetch Sport data for an external transport, such as FPort fetch Sport data for an external transport, such as FPort
*/ */
@ -992,10 +979,10 @@ bool AP_Frsky_Telem::get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &d
if (!singleton && !hal.util->get_soft_armed()) { if (!singleton && !hal.util->get_soft_armed()) {
// if telem data is requested when we are disarmed and don't // if telem data is requested when we are disarmed and don't
// yet have a AP_Frsky_Telem object then try to allocate one // yet have a AP_Frsky_Telem object then try to allocate one
new AP_Frsky_Telem(true); new AP_Frsky_Telem();
// initialize the passthrough scheduler // initialize the passthrough scheduler
if (singleton) { if (singleton) {
singleton->init(); singleton->init(true);
} }
} }
if (!singleton) { if (!singleton) {

142
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
Unescape View File

@ -18,7 +18,8 @@
#include <AP_Notify/AP_Notify.h> #include <AP_Notify/AP_Notify.h>
#include <AP_SerialManager/AP_SerialManager.h> #include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL/utility/RingBuffer.h> #include <AP_HAL/utility/RingBuffer.h>
#include <AP_RCTelemetry/AP_RCTelemetry.h>
#include "AP_Frsky_Backend.h"
#define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent) #define FRSKY_TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent)
@ -111,12 +112,10 @@ for FrSky SPort Passthrough
#define ATTIANDRNG_PITCH_LIMIT 0x3FF #define ATTIANDRNG_PITCH_LIMIT 0x3FF
#define ATTIANDRNG_PITCH_OFFSET 11 #define ATTIANDRNG_PITCH_OFFSET 11
#define ATTIANDRNG_RNGFND_OFFSET 21 #define ATTIANDRNG_RNGFND_OFFSET 21
// for fair scheduler
#define TIME_SLOT_MAX 11
class AP_Frsky_Telem : public AP_RCTelemetry { class AP_Frsky_Telem {
public: public:
AP_Frsky_Telem(bool external_data=false); AP_Frsky_Telem();
~AP_Frsky_Telem(); ~AP_Frsky_Telem();
@ -125,7 +124,7 @@ public:
AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete; AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete;
// init - perform required initialisation // init - perform required initialisation
virtual bool init() override; bool init(bool use_external_data=false);
static AP_Frsky_Telem *get_singleton(void) { static AP_Frsky_Telem *get_singleton(void) {
return singleton; return singleton;
@ -134,135 +133,22 @@ public:
// get next telemetry data for external consumers of SPort data // get next telemetry data for external consumers of SPort data
static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data); static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
private: void queue_message(MAV_SEVERITY severity, const char *text) {
AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver if (_backend == nullptr) {
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter return;
uint16_t _crc; }
return _backend->queue_text_message(severity, text);
uint8_t _paramID; }
enum PassthroughPacketType : uint8_t {
TEXT = 0, // 0x5000 status text (dynamic)
ATTITUDE = 1, // 0x5006 Attitude and range (dynamic)
GPS_LAT = 2, // 0x800 GPS lat
GPS_LON = 3, // 0x800 GPS lon
VEL_YAW = 4, // 0x5005 Vel and Yaw
AP_STATUS = 5, // 0x5001 AP status
GPS_STATUS = 6, // 0x5002 GPS status
HOME = 7, // 0x5004 Home
BATT_2 = 8, // 0x5008 Battery 2 status
BATT_1 = 9, // 0x5008 Battery 1 status
PARAM = 10 // 0x5007 parameters
};
enum PassthroughParam : uint8_t {
FRAME_TYPE = 1,
BATT_FS_VOLTAGE = 2,
BATT_FS_CAPACITY = 3,
BATT_CAPACITY_1 = 4,
BATT_CAPACITY_2 = 5
};
struct
{
int32_t vario_vspd;
char lat_ns, lon_ew;
uint16_t latdddmm;
uint16_t latmmmm;
uint16_t londddmm;
uint16_t lonmmmm;
uint16_t alt_gps_meters;
uint16_t alt_gps_cm;
uint16_t alt_nav_meters;
uint16_t alt_nav_cm;
int16_t speed_in_meter;
uint16_t speed_in_centimeter;
uint16_t yaw;
} _SPort_data;
struct PACKED
{
bool send_latitude; // sizeof(bool) = 4 ?
uint32_t gps_lng_sample;
uint8_t new_byte;
} _passthrough;
struct
{
bool sport_status;
bool gps_refresh;
bool vario_refresh;
uint8_t fas_call;
uint8_t gps_call;
uint8_t vario_call;
uint8_t various_call;
} _SPort;
struct
{
uint32_t last_200ms_frame;
uint32_t last_1000ms_frame;
} _D;
struct
{
uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent
uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
uint8_t char_index; // index of which character to get in the message
} _msg_chunk;
float get_vspeed_ms(void);
// passthrough WFQ scheduler
bool is_packet_ready(uint8_t idx, bool queue_empty) override;
void process_packet(uint8_t idx) override;
void adjust_packet_weight(bool queue_empty) override;
// setup ready for passthrough operation
void setup_wfq_scheduler(void) override;
// main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
void send_SPort_Passthrough(void);
// main transmission function when protocol is FrSky SPort
void send_SPort(void);
// main transmission function when protocol is FrSky D
void send_D(void);
// tick - main call to send updates to transmitter (called by scheduler at 1kHz)
void loop(void);
// methods related to the nuts-and-bolts of sending data
void send_byte(uint8_t value);
void send_uint16(uint16_t id, uint16_t data);
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format private:
bool get_next_msg_chunk(void) override;
uint32_t calc_param(void);
uint32_t calc_gps_latlng(bool *send_latitude);
uint32_t calc_gps_status(void);
uint32_t calc_batt(uint8_t instance);
uint32_t calc_ap_status(void);
uint32_t calc_home(void);
uint32_t calc_velandyaw(void);
uint32_t calc_attiandrng(void);
uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power);
// methods to convert flight controller data to FrSky D or SPort format AP_Frsky_Backend *_backend;
void calc_nav_alt(void);
float format_gps(float dec);
void calc_gps_position(void);
// get next telemetry data for external consumers of SPort data (internal function) // get next telemetry data for external consumers of SPort data (internal function)
bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data); bool _get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
static AP_Frsky_Telem *singleton; 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 { namespace AP {

Write Preview
Loading…
Cancel
Save