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

AP_Frsky_Telem: support FPort

Browse Source
This commit is contained in:
Andrew Tridgell 2020-05-11 15:56:45 +10:00
parent 01a603be43
commit 12292b4283
2 changed files with 282 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

362
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -31,14 +31,57 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
#include <AP_Common/Location.h> #include <AP_Common/Location.h>
#include <AP_GPS/AP_GPS.h> #include <AP_GPS/AP_GPS.h>
#include <AP_Logger/AP_Logger.h> #include <AP_Baro/AP_Baro.h>
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_Frsky_Telem::AP_Frsky_Telem(void) AP_Frsky_Telem *AP_Frsky_Telem::singleton;
AP_Frsky_Telem::AP_Frsky_Telem(bool _external_data) :
use_external_data(_external_data)
{ {
singleton = this;
}
AP_Frsky_Telem::~AP_Frsky_Telem(void)
{
singleton = nullptr;
}
/*
setup ready for passthrough telem
*/
void AP_Frsky_Telem::setup_passthrough(void)
{
#if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
// 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);
}
#endif
// initialize packet weights for the WFQ scheduler
// priority[i] = 1/_passthrough.packet_weight[i]
// rate[i] = LinkRate * ( priority[i] / (sum(priority[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
} }
/* /*
@ -55,31 +98,7 @@ bool AP_Frsky_Telem::init()
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers) _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))) { } 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) _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) setup_passthrough();
// 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
} }
if (_port != nullptr) { if (_port != nullptr) {
@ -117,10 +136,12 @@ void AP_Frsky_Telem::update_avg_packet_rate()
* WFQ scheduler * WFQ scheduler
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(uint8_t prev_byte) void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(void)
{ {
update_avg_packet_rate();
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
uint8_t max_delay_idx = TIME_SLOT_MAX; uint8_t max_delay_idx = 0;
float max_delay = 0; float max_delay = 0;
float delay = 0; float delay = 0;
@ -177,8 +198,6 @@ void AP_Frsky_Telem::passthrough_wfq_adaptive_scheduler(uint8_t prev_byte)
_passthrough.packet_timer[max_delay_idx] = AP_HAL::millis(); _passthrough.packet_timer[max_delay_idx] = AP_HAL::millis();
// send packet // send packet
switch (max_delay_idx) { switch (max_delay_idx) {
case TIME_SLOT_MAX: // nothing to send
break;
case 0: // 0x5000 status text case 0: // 0x5000 status text
if (get_next_msg_chunk()) { if (get_next_msg_chunk()) {
send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk); send_uint32(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
@ -249,8 +268,7 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
} }
if (prev_byte == START_STOP_SPORT) { if (prev_byte == START_STOP_SPORT) {
if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request if (_passthrough.new_byte == SENSOR_ID_28) { // byte 0x7E is the header of each poll request
update_avg_packet_rate(); passthrough_wfq_adaptive_scheduler();
passthrough_wfq_adaptive_scheduler(prev_byte);
} }
} }
} }
@ -261,8 +279,6 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
*/ */
void AP_Frsky_Telem::send_SPort(void) void AP_Frsky_Telem::send_SPort(void)
{ {
const AP_AHRS &_ahrs = AP::ahrs();
int16_t numc; int16_t numc;
numc = _port->available(); numc = _port->available();
@ -276,6 +292,19 @@ void AP_Frsky_Telem::send_SPort(void)
return; return;
} }
if (numc == 0) {
// no serial data to process do bg tasks
if (_SPort.vario_refresh) {
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
_SPort.vario_refresh = false;
}
if (_SPort.gps_refresh) {
calc_gps_position(); // gps data is not recalculated until all of it has been sent
_SPort.gps_refresh = false;
}
return;
}
for (int16_t i = 0; i < numc; i++) { for (int16_t i = 0; i < numc; i++) {
int16_t readbyte = _port->read(); int16_t readbyte = _port->read();
if (_SPort.sport_status == false) { if (_SPort.sport_status == false) {
@ -285,7 +314,24 @@ void AP_Frsky_Telem::send_SPort(void)
} else { } else {
const AP_BattMonitor &_battery = AP::battery(); const AP_BattMonitor &_battery = AP::battery();
switch(readbyte) { switch(readbyte) {
case SENSOR_ID_FAS: case SENSOR_ID_VARIO: // Sensor ID 0
switch (_SPort.vario_call) {
case 0:
send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_BP, _SPort_data.alt_nav_meters); // send altitude integer part
break;
case 1:
send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_AP, _SPort_data.alt_nav_cm); // send altitude decimal part
break;
case 2:
send_uint32(SPORT_DATA_FRAME, DATA_ID_VARIO, _SPort_data.vario_vspd); // send vspeed m/s
_SPort.vario_refresh = true;
break;
}
if (++_SPort.vario_call > 2) {
_SPort.vario_call = 0;
}
break;
case SENSOR_ID_FAS: // Sensor ID 2
switch (_SPort.fas_call) { switch (_SPort.fas_call) {
case 0: case 0:
send_uint32(SPORT_DATA_FRAME, DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining send_uint32(SPORT_DATA_FRAME, DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
@ -304,60 +350,40 @@ void AP_Frsky_Telem::send_SPort(void)
} }
break; break;
} }
if (_SPort.fas_call++ > 2) _SPort.fas_call = 0; if (++_SPort.fas_call > 2) {
_SPort.fas_call = 0;
}
break; break;
case SENSOR_ID_GPS: case SENSOR_ID_GPS: // Sensor ID 3
switch (_SPort.gps_call) { switch (_SPort.gps_call) {
case 0: case 0:
calc_gps_position(); // gps data is not recalculated until all of it has been sent send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
break; break;
case 1: case 1:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
break; break;
case 2: case 2:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer part
break; break;
case 3: case 3:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_AP, _SPort_data.speed_in_centimeter); // send gps speed decimal part
break; break;
case 4: case 4:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_BP, _SPort_data.alt_gps_meters); // send gps altitude integer part
break; break;
case 5: case 5:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_AP, _SPort_data.alt_gps_cm); // send gps altitude decimals
break; break;
case 6: case 6:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, _SPort_data.yaw); // send heading in degree based on AHRS and not GPS
break; _SPort.gps_refresh = true;
case 7:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
break;
case 8:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
break;
case 9:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals
break;
case 10:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
break; break;
} }
if (_SPort.gps_call++ > 10) _SPort.gps_call = 0; if (++_SPort.gps_call > 6) {
break; _SPort.gps_call = 0;
case SENSOR_ID_VARIO:
switch (_SPort.vario_call) {
case 0 :
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part
break;
case 1:
send_uint32(SPORT_DATA_FRAME, DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part
break;
} }
if (_SPort.vario_call++ > 1) _SPort.vario_call = 0;
break; break;
case SENSOR_ID_SP2UR: case SENSOR_ID_SP2UR: // Sensor ID 6
switch (_SPort.various_call) { switch (_SPort.various_call) {
case 0 : case 0 :
send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t) send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
@ -366,7 +392,9 @@ void AP_Frsky_Telem::send_SPort(void)
send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
break; break;
} }
if (_SPort.various_call++ > 1) _SPort.various_call = 0; if (++_SPort.various_call > 1) {
_SPort.various_call = 0;
}
break; break;
} }
_SPort.sport_status = false; _SPort.sport_status = false;
@ -382,9 +410,7 @@ void AP_Frsky_Telem::send_SPort(void)
*/ */
void AP_Frsky_Telem::send_D(void) void AP_Frsky_Telem::send_D(void)
{ {
const AP_AHRS &_ahrs = AP::ahrs();
const AP_BattMonitor &_battery = AP::battery(); const AP_BattMonitor &_battery = AP::battery();
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
// send frame1 every 200ms // send frame1 every 200ms
if (now - _D.last_200ms_frame >= 200) { if (now - _D.last_200ms_frame >= 200) {
@ -399,25 +425,26 @@ void AP_Frsky_Telem::send_D(void)
} }
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
calc_nav_alt(); calc_nav_alt();
send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part send_uint16(DATA_ID_BARO_ALT_BP, _SPort_data.alt_nav_meters); // send nav altitude integer part
send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part send_uint16(DATA_ID_BARO_ALT_AP, _SPort_data.alt_nav_cm); // send nav altitude decimal part
} }
// send frame2 every second // send frame2 every second
if (now - _D.last_1000ms_frame >= 1000) { if (now - _D.last_1000ms_frame >= 1000) {
_D.last_1000ms_frame = now; _D.last_1000ms_frame = now;
AP_AHRS &_ahrs = AP::ahrs();
send_uint16(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS send_uint16(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
calc_gps_position(); calc_gps_position();
if (AP::gps().status() >= 3) { if (AP::gps().status() >= 3) {
send_uint16(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part send_uint16(DATA_ID_GPS_LAT_BP, _SPort_data.latdddmm); // send gps lattitude degree and minute integer part
send_uint16(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part send_uint16(DATA_ID_GPS_LAT_AP, _SPort_data.latmmmm); // send gps lattitude minutes decimal part
send_uint16(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information send_uint16(DATA_ID_GPS_LAT_NS, _SPort_data.lat_ns); // send gps North / South information
send_uint16(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part send_uint16(DATA_ID_GPS_LONG_BP, _SPort_data.londddmm); // send gps longitude degree and minute integer part
send_uint16(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part send_uint16(DATA_ID_GPS_LONG_AP, _SPort_data.lonmmmm); // send gps longitude minutes decimal part
send_uint16(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information send_uint16(DATA_ID_GPS_LONG_EW, _SPort_data.lon_ew); // send gps East / West information
send_uint16(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part send_uint16(DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer part
send_uint16(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part send_uint16(DATA_ID_GPS_SPEED_AP, _SPort_data.speed_in_centimeter); // send gps speed decimal part
send_uint16(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part send_uint16(DATA_ID_GPS_ALT_BP, _SPort_data.alt_gps_meters); // send gps altitude integer part
send_uint16(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimal part send_uint16(DATA_ID_GPS_ALT_AP, _SPort_data.alt_gps_cm); // send gps altitude decimal part
} }
} }
} }
@ -503,6 +530,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) 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 send_byte(frame); // frame type
uint8_t *bytes = (uint8_t*)&id; uint8_t *bytes = (uint8_t*)&id;
send_byte(bytes[0]); // LSB send_byte(bytes[0]); // LSB
@ -665,13 +699,18 @@ void AP_Frsky_Telem::check_sensor_status_flags(void)
*/ */
void AP_Frsky_Telem::check_ekf_status(void) void AP_Frsky_Telem::check_ekf_status(void)
{ {
const AP_AHRS &_ahrs = AP::ahrs();
// get variances // get variances
bool get_variance;
float velVar, posVar, hgtVar, tasVar; float velVar, posVar, hgtVar, tasVar;
Vector3f magVar; Vector3f magVar;
Vector2f offset; Vector2f offset;
if (_ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset)) { {
AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
get_variance = _ahrs.get_variances(velVar, posVar, hgtVar, magVar, tasVar, offset);
}
if (get_variance) {
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed if ((now - check_ekf_status_timer) >= 10000) { // prevent repeating any ekf_status message unless 10 seconds have passed
// multiple errors can be reported at a time. Same setup as Mission Planner. // multiple errors can be reported at a time. Same setup as Mission Planner.
@ -849,14 +888,21 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
*/ */
uint32_t AP_Frsky_Telem::calc_home(void) uint32_t AP_Frsky_Telem::calc_home(void)
{ {
const AP_AHRS &_ahrs = AP::ahrs();
uint32_t home = 0; uint32_t home = 0;
Location loc; Location loc;
Location home_loc;
bool get_position;
float _relative_home_altitude = 0; float _relative_home_altitude = 0;
if (_ahrs.get_position(loc)) {
{
AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
get_position = _ahrs.get_position(loc);
home_loc = _ahrs.get_home();
}
if (get_position) {
// check home_loc is valid // check home_loc is valid
const Location &home_loc = _ahrs.get_home();
if (home_loc.lat != 0 || home_loc.lng != 0) { if (home_loc.lat != 0 || home_loc.lng != 0) {
// distance between vehicle and home_loc in meters // distance between vehicle and home_loc in meters
home = prep_number(roundf(home_loc.get_distance(loc)), 3, 2); home = prep_number(roundf(home_loc.get_distance(loc)), 3, 2);
@ -867,7 +913,7 @@ uint32_t AP_Frsky_Telem::calc_home(void)
_relative_home_altitude = loc.alt; _relative_home_altitude = loc.alt;
if (!loc.relative_alt) { if (!loc.relative_alt) {
// loc.alt has home altitude added, remove it // loc.alt has home altitude added, remove it
_relative_home_altitude -= _ahrs.get_home().alt; _relative_home_altitude -= home_loc.alt;
} }
} }
// altitude above home in decimeters // altitude above home in decimeters
@ -881,17 +927,11 @@ uint32_t AP_Frsky_Telem::calc_home(void)
*/ */
uint32_t AP_Frsky_Telem::calc_velandyaw(void) uint32_t AP_Frsky_Telem::calc_velandyaw(void)
{ {
AP_AHRS &_ahrs = AP::ahrs(); float vspd = get_vspeed_ms();
uint32_t velandyaw;
Vector3f velNED {};
// if we can't get velocity then we use zero for vertical velocity
if (!_ahrs.get_velocity_NED(velNED)) {
velNED.zero();
}
// vertical velocity in dm/s // vertical velocity in dm/s
velandyaw = prep_number(roundf(-velNED.z * 10), 2, 1); uint32_t velandyaw = prep_number(roundf(vspd * 10), 2, 1);
AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
// horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed) // horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed)
const AP_Airspeed *aspeed = _ahrs.get_airspeed(); const AP_Airspeed *aspeed = _ahrs.get_airspeed();
if (aspeed && aspeed->enabled()) { if (aspeed && aspeed->enabled()) {
@ -910,11 +950,10 @@ uint32_t AP_Frsky_Telem::calc_velandyaw(void)
*/ */
uint32_t AP_Frsky_Telem::calc_attiandrng(void) uint32_t AP_Frsky_Telem::calc_attiandrng(void)
{ {
const AP_AHRS &_ahrs = AP::ahrs();
const RangeFinder *_rng = RangeFinder::get_singleton(); const RangeFinder *_rng = RangeFinder::get_singleton();
uint32_t attiandrng; uint32_t attiandrng;
AP_AHRS &_ahrs = AP::ahrs();
// roll from [-18000;18000] centidegrees to unsigned .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits) // roll from [-18000;18000] centidegrees to unsigned .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
attiandrng = ((uint16_t)roundf((_ahrs.roll_sensor + 18000) * 0.05f) & ATTIANDRNG_ROLL_LIMIT); attiandrng = ((uint16_t)roundf((_ahrs.roll_sensor + 18000) * 0.05f) & ATTIANDRNG_ROLL_LIMIT);
// pitch from [-9000;9000] centidegrees to unsigned .2 degree increments [0;900] (just in case, limit to 1023 (0x3FF) since the value is stored on 10 bits) // pitch from [-9000;9000] centidegrees to unsigned .2 degree increments [0;900] (just in case, limit to 1023 (0x3FF) since the value is stored on 10 bits)
@ -989,16 +1028,40 @@ uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t pow
return res; return res;
} }
/*
* get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
* for FrSky D and SPort protocols
*/
float AP_Frsky_Telem::get_vspeed_ms(void)
{
{// release semaphore as soon as possible
AP_AHRS &_ahrs = AP::ahrs();
Vector3f v;
WITH_SEMAPHORE(_ahrs.get_semaphore());
if (_ahrs.get_velocity_NED(v)) {
return -v.z;
}
}
auto &_baro = AP::baro();
WITH_SEMAPHORE(_baro.get_semaphore());
return _baro.get_climb_rate();
}
/* /*
* 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_Telem::calc_nav_alt(void)
{ {
const AP_AHRS &_ahrs = AP::ahrs(); _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s
Location loc; Location loc;
float current_height = 0; // in centimeters above home float current_height = 0; // in centimeters above home
AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
if (_ahrs.get_position(loc)) { if (_ahrs.get_position(loc)) {
current_height = loc.alt*0.01f; current_height = loc.alt*0.01f;
if (!loc.relative_alt) { if (!loc.relative_alt) {
@ -1007,8 +1070,8 @@ void AP_Frsky_Telem::calc_nav_alt(void)
} }
} }
_gps.alt_nav_meters = (int16_t)current_height; _SPort_data.alt_nav_meters = (int16_t)current_height;
_gps.alt_nav_cm = (current_height - _gps.alt_nav_meters) * 100; _SPort_data.alt_nav_cm = (current_height - _SPort_data.alt_nav_meters) * 100;
} }
/* /*
@ -1035,33 +1098,36 @@ void AP_Frsky_Telem::calc_gps_position(void)
if (AP::gps().status() >= 3) { if (AP::gps().status() >= 3) {
const Location &loc = AP::gps().location(); //get gps instance 0 const Location &loc = AP::gps().location(); //get gps instance 0
lat = format_gps(fabsf(loc.lat/10000000.0f)); lat = format_gps(fabsf(loc.lat/10000000.0f));
_gps.latdddmm = lat; _SPort_data.latdddmm = lat;
_gps.latmmmm = (lat - _gps.latdddmm) * 10000; _SPort_data.latmmmm = (lat - _SPort_data.latdddmm) * 10000;
_gps.lat_ns = (loc.lat < 0) ? 'S' : 'N'; _SPort_data.lat_ns = (loc.lat < 0) ? 'S' : 'N';
lon = format_gps(fabsf(loc.lng/10000000.0f)); lon = format_gps(fabsf(loc.lng/10000000.0f));
_gps.londddmm = lon; _SPort_data.londddmm = lon;
_gps.lonmmmm = (lon - _gps.londddmm) * 10000; _SPort_data.lonmmmm = (lon - _SPort_data.londddmm) * 10000;
_gps.lon_ew = (loc.lng < 0) ? 'W' : 'E'; _SPort_data.lon_ew = (loc.lng < 0) ? 'W' : 'E';
alt = loc.alt * 0.01f; alt = loc.alt * 0.01f;
_gps.alt_gps_meters = (int16_t)alt; _SPort_data.alt_gps_meters = (int16_t)alt;
_gps.alt_gps_cm = (alt - _gps.alt_gps_meters) * 100; _SPort_data.alt_gps_cm = (alt - _SPort_data.alt_gps_meters) * 100;
speed = AP::gps().ground_speed(); speed = AP::gps().ground_speed();
_gps.speed_in_meter = speed; _SPort_data.speed_in_meter = speed;
_gps.speed_in_centimeter = (speed - _gps.speed_in_meter) * 100; _SPort_data.speed_in_centimeter = (speed - _SPort_data.speed_in_meter) * 100;
} else { } else {
_gps.latdddmm = 0; _SPort_data.latdddmm = 0;
_gps.latmmmm = 0; _SPort_data.latmmmm = 0;
_gps.lat_ns = 0; _SPort_data.lat_ns = 0;
_gps.londddmm = 0; _SPort_data.londddmm = 0;
_gps.lonmmmm = 0; _SPort_data.lonmmmm = 0;
_gps.alt_gps_meters = 0; _SPort_data.alt_gps_meters = 0;
_gps.alt_gps_cm = 0; _SPort_data.alt_gps_cm = 0;
_gps.speed_in_meter = 0; _SPort_data.speed_in_meter = 0;
_gps.speed_in_centimeter = 0; _SPort_data.speed_in_centimeter = 0;
} }
AP_AHRS &_ahrs = AP::ahrs();
_SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS
} }
uint32_t AP_Frsky_Telem::sensor_status_flags() const uint32_t AP_Frsky_Telem::sensor_status_flags() const
@ -1073,3 +1139,45 @@ uint32_t AP_Frsky_Telem::sensor_status_flags() const
return ~health & enabled & present; 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)
{
passthrough_wfq_adaptive_scheduler();
if (!external_data.pending) {
return false;
}
frame = external_data.frame;
appid = external_data.appid;
data = external_data.data;
external_data.pending = false;
return true;
}
/*
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 (!singleton && !hal.util->get_soft_armed()) {
// if telem data is requested when we are disarmed and don't
// yet have a AP_Frsky_Telem object then try to allocate one
new AP_Frsky_Telem(true);
// initialize the passthrough scheduler
if (singleton) {
singleton->setup_passthrough();
}
}
if (!singleton) {
return false;
}
return singleton->_get_telem_data(frame, appid, data);
}
namespace AP {
AP_Frsky_Telem *frsky_telem() {
return AP_Frsky_Telem::get_singleton();
}
};

45
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -42,6 +42,7 @@ for FrSky D protocol (D-receivers)
#define DATA_ID_GPS_LONG_EW 0x22 #define DATA_ID_GPS_LONG_EW 0x22
#define DATA_ID_GPS_LAT_NS 0x23 #define DATA_ID_GPS_LAT_NS 0x23
#define DATA_ID_CURRENT 0x28 #define DATA_ID_CURRENT 0x28
#define DATA_ID_VARIO 0x30
#define DATA_ID_VFAS 0x39 #define DATA_ID_VFAS 0x39
#define START_STOP_D 0x5E #define START_STOP_D 0x5E
@ -112,7 +113,9 @@ for FrSky SPort Passthrough
class AP_Frsky_Telem { class AP_Frsky_Telem {
public: public:
AP_Frsky_Telem(); AP_Frsky_Telem(bool external_data=false);
~AP_Frsky_Telem();
/* Do not allow copies */ /* Do not allow copies */
AP_Frsky_Telem(const AP_Frsky_Telem &other) = delete; AP_Frsky_Telem(const AP_Frsky_Telem &other) = delete;
@ -130,6 +133,13 @@ public:
// functioning correctly // functioning correctly
uint32_t sensor_status_flags() const; 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
static bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data);
private: private:
AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver 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 AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
@ -148,6 +158,7 @@ private:
struct struct
{ {
int32_t vario_vspd;
char lat_ns, lon_ew; char lat_ns, lon_ew;
uint16_t latdddmm; uint16_t latdddmm;
uint16_t latmmmm; uint16_t latmmmm;
@ -159,7 +170,8 @@ private:
uint16_t alt_nav_cm; uint16_t alt_nav_cm;
int16_t speed_in_meter; int16_t speed_in_meter;
uint16_t speed_in_centimeter; uint16_t speed_in_centimeter;
} _gps; uint16_t yaw;
} _SPort_data;
struct PACKED struct PACKED
{ {
@ -177,7 +189,7 @@ private:
struct struct
{ {
const uint32_t packet_min_period[TIME_SLOT_MAX] = { const uint32_t packet_min_period[TIME_SLOT_MAX] = {
0, //0x5000 text, no rate limiter 28, //0x5000 text, 25Hz
38, //0x5006 attitude 20Hz 38, //0x5006 attitude 20Hz
280, //0x800 GPS 3Hz 280, //0x800 GPS 3Hz
280, //0x800 GPS 3Hz 280, //0x800 GPS 3Hz
@ -194,6 +206,8 @@ private:
struct struct
{ {
bool sport_status; bool sport_status;
bool gps_refresh;
bool vario_refresh;
uint8_t fas_call; uint8_t fas_call;
uint8_t gps_call; uint8_t gps_call;
uint8_t vario_call; uint8_t vario_call;
@ -213,9 +227,10 @@ private:
uint8_t char_index; // index of which character to get in the message uint8_t char_index; // index of which character to get in the message
} _msg_chunk; } _msg_chunk;
float get_vspeed_ms(void);
// passthrough WFQ scheduler // passthrough WFQ scheduler
void update_avg_packet_rate(); void update_avg_packet_rate();
void passthrough_wfq_adaptive_scheduler(uint8_t prev_byte); void passthrough_wfq_adaptive_scheduler();
// main transmission function when protocol is FrSky SPort Passthrough (OpenTX) // main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
void send_SPort_Passthrough(void); void send_SPort_Passthrough(void);
// main transmission function when protocol is FrSky SPort // main transmission function when protocol is FrSky SPort
@ -249,4 +264,26 @@ private:
void calc_nav_alt(void); void calc_nav_alt(void);
float format_gps(float dec); float format_gps(float dec);
void calc_gps_position(void); void calc_gps_position(void);
// setup ready for passthrough operation
void setup_passthrough(void);
// 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);
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();
}; };