From d5868e3c7d5c850bd03cb7f6e477e589f2b0cc61 Mon Sep 17 00:00:00 2001 From: yaapu Date: Fri, 29 Jan 2021 09:50:59 +0100 Subject: [PATCH] AP_Frsky_Telem: added telemetry support for RPM sensors 1 and 2 for SPort, FPort/FPort2 and passthrough over crossfire --- libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp | 19 +++++++++ libraries/AP_Frsky_Telem/AP_Frsky_Backend.h | 3 ++ libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 19 +++++++++ libraries/AP_Frsky_Telem/AP_Frsky_SPort.h | 1 + .../AP_Frsky_SPort_Passthrough.cpp | 39 +++++++++++++++++++ .../AP_Frsky_SPort_Passthrough.h | 6 ++- 6 files changed, 85 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp index 080e88850a..63c2b0afd7 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp @@ -3,6 +3,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -140,3 +141,21 @@ void AP_Frsky_Backend::calc_gps_position(void) _SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS } +/* + * prepare rpm data + * for FrSky D and SPort protocols + */ +bool AP_Frsky_Backend::calc_rpm(const uint8_t instance, int32_t &value) const +{ + const AP_RPM* rpm = AP::rpm(); + if (rpm == nullptr) { + return false; + } + + float rpm_value; + if (!rpm->get_rpm(instance, rpm_value)) { + return false; + } + value = static_cast(roundf(rpm_value)); + return true; +} diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h index 7a46a07f6d..b247fd5c88 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h @@ -58,6 +58,7 @@ protected: void calc_nav_alt(void); void calc_gps_position(void); + bool calc_rpm(const uint8_t instance, int32_t &value) const; float get_vspeed_ms(void); @@ -108,6 +109,7 @@ protected: static const uint8_t BYTESTUFF_D = 0x5D; // FrSky data IDs; + static const uint16_t RPM_LAST_ID = 0x050F; static const uint16_t GPS_LONG_LATI_FIRST_ID = 0x0800; static const uint16_t DIY_FIRST_ID = 0x5000; @@ -121,6 +123,7 @@ protected: static const uint8_t SENSOR_ID_VARIO = 0x00; // Sensor ID 0 static const uint8_t SENSOR_ID_FAS = 0x22; // Sensor ID 2 static const uint8_t SENSOR_ID_GPS = 0x83; // Sensor ID 3 + static const uint8_t SENSOR_ID_RPM = 0xE4; // Sensor ID 4 static const uint8_t SENSOR_ID_SP2UR = 0xC6; // Sensor ID 6 static const uint8_t SENSOR_ID_27 = 0x1B; // Sensor ID 27 diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 31889f1b99..0449d6deab 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -4,11 +4,14 @@ #include #include #include +#include #include "AP_Frsky_SPortParser.h" #include +extern const AP_HAL::HAL& hal; + AP_Frsky_SPort *AP_Frsky_SPort::singleton; /* @@ -114,6 +117,22 @@ void AP_Frsky_SPort::send(void) _SPort.gps_call = 0; } break; + case SENSOR_ID_RPM: // Sensor ID 4 + { + const AP_RPM* rpm = AP::rpm(); + if (rpm == nullptr) { + break; + } + int32_t value; + if (calc_rpm(_SPort.rpm_call, value)) { + // use high numbered frsky sensor ids to leave low numbered free for externally attached physical frsky sensors + send_sport_frame(SPORT_DATA_FRAME, 1+RPM_LAST_ID-(rpm->num_sensors()-_SPort.rpm_call), value); + } + if (++_SPort.rpm_call > (rpm->num_sensors()-1)) { + _SPort.rpm_call = 0; + } + } + break; case SENSOR_ID_SP2UR: // Sensor ID 6 switch (_SPort.various_call) { case 0 : diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h index ddc72474ec..f847794e66 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h @@ -54,6 +54,7 @@ private: uint8_t gps_call; uint8_t vario_call; uint8_t various_call; + uint8_t rpm_call; } _SPort; static AP_Frsky_SPort *singleton; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 95d1b58143..ab8b6680cc 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL @@ -110,6 +111,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void) set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters + set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2 set_scheduler_entry(UDATA, 5000, 200); // user data #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL set_scheduler_entry(MAV, 35, 25); // mavlite @@ -188,6 +190,16 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) case BATT_2: packet_ready = AP::battery().num_instances() > 1; break; + case RPM: + { + packet_ready = false; + const AP_RPM *rpm = AP::rpm(); + if (rpm == nullptr) { + break; + } + packet_ready = rpm->num_sensors() > 0; + } + break; case UDATA: // when using fport user data is sent by scheduler // when using sport user data is sent responding to custom polling @@ -254,6 +266,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) case PARAM: // 0x5007 parameters send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); break; + case RPM: // 0x500A rpm sensors 1 and 2 + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm()); + break; case UDATA: // user data { WITH_SEMAPHORE(_sport_push_buffer.sem); @@ -577,6 +592,30 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void) return attiandrng; } +/* + * prepare rpm for sensors 1 and 2 + * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) + */ +uint32_t AP_Frsky_SPort_Passthrough::calc_rpm(void) +{ + const AP_RPM *ap_rpm = AP::rpm(); + if (ap_rpm == nullptr) { + return 0; + } + uint32_t value = 0; + // we send: rpm_value*0.1 as 16 bits signed + float rpm; + // bits 0-15 for rpm 0 + if (ap_rpm->get_rpm(0,rpm)) { + value |= (int16_t)roundf(rpm * 0.1); + } + // bits 16-31 for rpm 1 + if (ap_rpm->get_rpm(1,rpm)) { + value |= (int16_t)roundf(rpm * 0.1) << 16; + } + return value; +} + /* fetch Sport data for an external transport, such as FPort or crossfire Note: we need to create a packet array with unique packet types diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index ce0f9c2014..22bed7f815 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -67,9 +67,10 @@ public: BATT_2 = 8, // 0x5008 Battery 2 status BATT_1 = 9, // 0x5008 Battery 1 status PARAM = 10, // 0x5007 parameters - UDATA = 11, // user data + RPM = 11, // 0x500A rpm sensors 1 and 2 + UDATA = 12, // user data #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL - MAV = 12, // mavlite + MAV = 13, // mavlite #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL WFQ_LAST_ITEM // must be last }; @@ -97,6 +98,7 @@ private: uint32_t calc_home(void); uint32_t calc_velandyaw(void); uint32_t calc_attiandrng(void); + uint32_t calc_rpm(void); // use_external_data is set when this library will // be providing data to another transport, such as FPort