From b606c0df64e87b8eacf717c0fa6d3f8d9b0cf27e Mon Sep 17 00:00:00 2001 From: yaapu Date: Wed, 18 Sep 2019 23:19:24 +0200 Subject: [PATCH] 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 --- libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp | 202 ++++++++++++-------- libraries/AP_Frsky_Telem/AP_Frsky_Telem.h | 7 +- 2 files changed, 131 insertions(+), 78 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 62685e2785..90ba25892c 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -31,7 +31,7 @@ #include #include #include -#include +#include #include #include @@ -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 diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 253d21aac9..bfb75a8d39 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -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();