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

@@ -3,6 +3,7 @@
 #include <AP_Baro/AP_Baro.h>
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_GPS/AP_GPS.h>
+#include <AP_RPM/AP_RPM.h>
 
 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<int32_t>(roundf(rpm_value));
+    return true;
+}

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
 

libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp

@@ -4,11 +4,14 @@
 #include <AP_BattMonitor/AP_BattMonitor.h>
 #include <AP_GPS/AP_GPS.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_RPM/AP_RPM.h>
 
 #include "AP_Frsky_SPortParser.h"
 
 #include <string.h>
 
+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 :

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;

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -7,6 +7,7 @@
 #include <AP_InertialSensor/AP_InertialSensor.h>
 #include <AP_Notify/AP_Notify.h>
 #include <AP_RangeFinder/AP_RangeFinder.h>
+#include <AP_RPM/AP_RPM.h>
 #include <GCS_MAVLink/GCS.h>
 
 #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

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