Skip to content

Commit

Permalink
AP_Frsky_Telem: added VSpd to telemetry protocol 4
Browse files Browse the repository at this point in the history 
The Frsky vario "virtual" sensor was reporting altitude but not vertical speed.
This patch adds VSpd as a new sensor when protocol 4 is selected.
GPS frsky sensor is migrated to 2 byte sensor ID and needs rediscovery if using the previous 1 byte version
  • Loading branch information
@yaapu
yaapu authored and yaapu committed Feb 8, 2020
1 parent 12f31e3 commit 111781b
Show file tree
Hide file tree
Showing 2 changed files with 131 additions and 78 deletions.
202 changes: 125 additions & 77 deletions libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
#include <GCS_MAVLink/GCS.h>
#include <AP_Common/Location.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_Baro/AP_Baro.h>
#include <stdio.h>
#include <math.h>

Expand Down Expand Up @@ -281,8 +281,6 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
*/
void AP_Frsky_Telem::send_SPort(void)
{
const AP_AHRS &_ahrs = AP::ahrs();

int16_t numc;
numc = _port->available();

Expand All @@ -296,6 +294,23 @@ void AP_Frsky_Telem::send_SPort(void)
return;
}

if (numc == 0) {
// no serial data to process do bg tasks
switch (_SPort.next_sensor_id) {
case SENSOR_ID_VARIO:
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
break;
case SENSOR_ID_FAS:
break;
case SENSOR_ID_GPS:
calc_gps_position(); // gps data is not recalculated until all of it has been sent
break;
case SENSOR_ID_SP2UR:
break;
}
return;
}

for (int16_t i = 0; i < numc; i++) {
int16_t readbyte = _port->read();
if (_SPort.sport_status == false) {
Expand All @@ -305,7 +320,25 @@ void AP_Frsky_Telem::send_SPort(void)
} else {
const AP_BattMonitor &_battery = AP::battery();
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
// update vfas data in next idle serial port loop
_SPort.next_sensor_id = SENSOR_ID_FAS;
break;
}
if (++_SPort.vario_call > 2) {
_SPort.vario_call = 0;
}
break;
case SENSOR_ID_FAS: // Sensor ID 2
switch (_SPort.fas_call) {
case 0:
send_uint32(SPORT_DATA_FRAME, DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
Expand All @@ -322,71 +355,58 @@ void AP_Frsky_Telem::send_SPort(void)
send_uint32(SPORT_DATA_FRAME, DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
break;
}
// update gps data in next idle serial port loop
_SPort.next_sensor_id = SENSOR_ID_GPS;
break;
}
if (_SPort.fas_call++ > 2) _SPort.fas_call = 0;
if (++_SPort.fas_call > 2) {
_SPort.fas_call = 0;
}
break;
case SENSOR_ID_GPS:
case SENSOR_ID_GPS: // Sensor ID 3
switch (_SPort.gps_call) {
case 0:
calc_gps_position(); // gps data is not recalculated until all of it has been sent
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
send_uint32(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
break;
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;
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;
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;
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;
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;
case 6:
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
break;
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
send_uint32(SPORT_DATA_FRAME, DATA_ID_GPS_COURS_BP, _SPort_data.yaw); // send heading in degree based on AHRS and not GPS
// update SPUR data in next idle serial port loop
_SPort.next_sensor_id = SENSOR_ID_SP2UR;
break;
}
if (_SPort.gps_call++ > 10) _SPort.gps_call = 0;
if (++_SPort.gps_call > 6) {
_SPort.gps_call = 0;
}
break;
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;
case SENSOR_ID_SP2UR:
case SENSOR_ID_SP2UR: // Sensor ID 6
switch (_SPort.various_call) {
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)
break;
case 1:
send_uint32(SPORT_DATA_FRAME, DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
// update vario data in next idle serial port loop
_SPort.next_sensor_id = SENSOR_ID_VARIO;
break;
}
if (_SPort.various_call++ > 1) _SPort.various_call = 0;
if (++_SPort.various_call > 1) {
_SPort.various_call = 0;
}
break;
}
_SPort.sport_status = false;
Expand Down Expand Up @@ -419,25 +439,25 @@ void AP_Frsky_Telem::send_D(void)
}
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
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_AP, _gps.alt_nav_cm); // send nav altitude decimal 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, _SPort_data.alt_nav_cm); // send nav altitude decimal part
}
// send frame2 every second
if (now - _D.last_1000ms_frame >= 1000) {
_D.last_1000ms_frame = now;
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();
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_AP, _gps.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_LONG_BP, _gps.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_EW, _gps.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_AP, _gps.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_AP, _gps.alt_gps_cm); // send gps altitude decimal 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, _SPort_data.latmmmm); // send gps lattitude minutes decimal part
send_uint16(DATA_ID_GPS_LAT_NS, _SPort_data.lat_ns); // send gps North / South information
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, _SPort_data.lonmmmm); // send gps longitude minutes decimal part
send_uint16(DATA_ID_GPS_LONG_EW, _SPort_data.lon_ew); // send gps East / West information
send_uint16(DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer 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, _SPort_data.alt_gps_meters); // send gps altitude integer part
send_uint16(DATA_ID_GPS_ALT_AP, _SPort_data.alt_gps_cm); // send gps altitude decimal part
}
}
}
Expand Down Expand Up @@ -1016,26 +1036,50 @@ uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t pow
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
* for FrSky D and SPort protocols
*/
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;
float current_height = 0; // in centimeters above home

AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
if (_ahrs.get_position(loc)) {
current_height = loc.alt*0.01f;
if (!loc.relative_alt) {
// loc.alt has home altitude added, remove it
current_height -= _ahrs.get_home().alt*0.01f;
}
}
_gps.alt_nav_meters = (int16_t)current_height;
_gps.alt_nav_cm = (current_height - _gps.alt_nav_meters) * 100;

_SPort_data.alt_nav_meters = (int16_t)current_height;
_SPort_data.alt_nav_cm = (current_height - _SPort_data.alt_nav_meters) * 100;
}

/*
Expand All @@ -1062,33 +1106,37 @@ void AP_Frsky_Telem::calc_gps_position(void)
if (AP::gps().status() >= 3) {
const Location &loc = AP::gps().location(); //get gps instance 0
lat = format_gps(fabsf(loc.lat/10000000.0f));
_gps.latdddmm = lat;
_gps.latmmmm = (lat - _gps.latdddmm) * 10000;
_gps.lat_ns = (loc.lat < 0) ? 'S' : 'N';
_SPort_data.latdddmm = lat;
_SPort_data.latmmmm = (lat - _SPort_data.latdddmm) * 10000;
_SPort_data.lat_ns = (loc.lat < 0) ? 'S' : 'N';

lon = format_gps(fabsf(loc.lng/10000000.0f));
_gps.londddmm = lon;
_gps.lonmmmm = (lon - _gps.londddmm) * 10000;
_gps.lon_ew = (loc.lng < 0) ? 'W' : 'E';
_SPort_data.londddmm = lon;
_SPort_data.lonmmmm = (lon - _SPort_data.londddmm) * 10000;
_SPort_data.lon_ew = (loc.lng < 0) ? 'W' : 'E';

alt = loc.alt * 0.01f;
_gps.alt_gps_meters = (int16_t)alt;
_gps.alt_gps_cm = (alt - _gps.alt_gps_meters) * 100;
_SPort_data.alt_gps_meters = (int16_t)alt;
_SPort_data.alt_gps_cm = (alt - _SPort_data.alt_gps_meters) * 100;

speed = AP::gps().ground_speed();
_gps.speed_in_meter = speed;
_gps.speed_in_centimeter = (speed - _gps.speed_in_meter) * 100;
_SPort_data.speed_in_meter = speed;
_SPort_data.speed_in_centimeter = (speed - _SPort_data.speed_in_meter) * 100;
} else {
_gps.latdddmm = 0;
_gps.latmmmm = 0;
_gps.lat_ns = 0;
_gps.londddmm = 0;
_gps.lonmmmm = 0;
_gps.alt_gps_meters = 0;
_gps.alt_gps_cm = 0;
_gps.speed_in_meter = 0;
_gps.speed_in_centimeter = 0;
_SPort_data.latdddmm = 0;
_SPort_data.latmmmm = 0;
_SPort_data.lat_ns = 0;
_SPort_data.londddmm = 0;
_SPort_data.lonmmmm = 0;
_SPort_data.alt_gps_meters = 0;
_SPort_data.alt_gps_cm = 0;
_SPort_data.speed_in_meter = 0;
_SPort_data.speed_in_centimeter = 0;
}

AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
_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
Expand Down
7 changes: 6 additions & 1 deletion libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ for FrSky D protocol (D-receivers)
#define DATA_ID_GPS_LONG_EW 0x22
#define DATA_ID_GPS_LAT_NS 0x23
#define DATA_ID_CURRENT 0x28
#define DATA_ID_VARIO 0x30
#define DATA_ID_VFAS 0x39

#define START_STOP_D 0x5E
Expand Down Expand Up @@ -157,6 +158,7 @@ class AP_Frsky_Telem {

struct
{
int32_t vario_vspd;
char lat_ns, lon_ew;
uint16_t latdddmm;
uint16_t latmmmm;
Expand All @@ -168,7 +170,8 @@ class AP_Frsky_Telem {
uint16_t alt_nav_cm;
int16_t speed_in_meter;
uint16_t speed_in_centimeter;
} _gps;
uint16_t yaw;
} _SPort_data;

struct PACKED
{
Expand Down Expand Up @@ -207,6 +210,7 @@ class AP_Frsky_Telem {
uint8_t gps_call;
uint8_t vario_call;
uint8_t various_call;
uint8_t next_sensor_id;
} _SPort;

struct
Expand All @@ -222,6 +226,7 @@ class AP_Frsky_Telem {
uint8_t char_index; // index of which character to get in the message
} _msg_chunk;

float get_vspeed_ms(void);
// passthrough WFQ scheduler
void update_avg_packet_rate();
void passthrough_wfq_adaptive_scheduler();
Expand Down

0 comments on commit 111781b

Please sign in to comment.