mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: added VSpd to telemetry protocol 4
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
This commit is contained in:
yaapu
Andrew Tridgell
parent
12f31e328f
commit
b606c0df64
|
|
@ -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>
|
||||
|
||||
|
|
@ -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();
|
||||
|
||||
|
|
@ -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) {
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
@ -419,8 +439,8 @@ 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) {
|
||||
|
|
@ -428,16 +448,16 @@ void AP_Frsky_Telem::send_D(void)
|
|||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1016,16 +1036,40 @@ 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) {
|
||||
|
|
@ -1033,9 +1077,9 @@ void AP_Frsky_Telem::calc_nav_alt(void)
|
|||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -157,6 +158,7 @@ private:
|
|||
|
||||
struct
|
||||
{
|
||||
int32_t vario_vspd;
|
||||
char lat_ns, lon_ew;
|
||||
uint16_t latdddmm;
|
||||
uint16_t latmmmm;
|
||||
|
|
@ -168,7 +170,8 @@ private:
|
|||
uint16_t alt_nav_cm;
|
||||
int16_t speed_in_meter;
|
||||
uint16_t speed_in_centimeter;
|
||||
} _gps;
|
||||
uint16_t yaw;
|
||||
} _SPort_data;
|
||||
|
||||
struct PACKED
|
||||
{
|
||||
|
|
@ -207,6 +210,7 @@ private:
|
|||
uint8_t gps_call;
|
||||
uint8_t vario_call;
|
||||
uint8_t various_call;
|
||||
uint8_t next_sensor_id;
|
||||
} _SPort;
|
||||
|
||||
struct
|
||||
|
|
@ -222,6 +226,7 @@ private:
|
|||
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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue