Plane: Add SERIAL2_PROTOCOL for GCS and FRSky telemtry

This allows selection of protocol type on telem2. The default is MAVLink, but can be selected as FrSky protocol

APMrover2/APMrover2.pde

@@ -97,6 +97,7 @@
 #include <APM_Control.h>
 #include <AP_L1_Control.h>
 #include <AP_BoardConfig.h>
+#include <AP_Frsky_Telem.h>
 
 #include <AP_HAL_AVR.h>
 #include <AP_HAL_AVR_SITL.h>
@@ -434,6 +435,13 @@ static bool ch7_flag;
 ////////////////////////////////////////////////////////////////////////////////
 static AP_BattMonitor battery;
 
+////////////////////////////////////////////////////////////////////////////////
+// Battery Sensors
+////////////////////////////////////////////////////////////////////////////////
+#if FRSKY_TELEM_ENABLED == ENABLED
+static AP_Frsky_Telem frsky_telemetry(ahrs, battery);
+#endif
+
 ////////////////////////////////////////////////////////////////////////////////
 // Navigation control variables
 ////////////////////////////////////////////////////////////////////////////////
@@ -537,7 +545,10 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { gcs_failsafe_check,     5,    600 },
     { compass_accumulate,     1,    900 },
     { update_notify,          1,    300 },
-    { one_second_loop,       50,   3000 }
+    { one_second_loop,       50,   3000 },
+#if FRSKY_TELEM_ENABLED == ENABLED
+    { telemetry_send,        10,    100 }	
+#endif
 };
 
 

APMrover2/Parameters.h

@@ -68,6 +68,7 @@ public:
         k_param_skip_gyro_cal,
         k_param_gcs2,       // stream rates for uartD
         k_param_serial2_baud_old,
+        k_param_serial2_protocol,
 
         //
         // 130: Sensor parameters
@@ -213,6 +214,7 @@ public:
     AP_Int16    serial1_baud;
 #if MAVLINK_COMM_NUM_BUFFERS > 2
     AP_Int16    serial2_baud;
+    AP_Int8     serial2_protocol;
 #endif
     AP_Int8     telem_delay;
     AP_Int8     skip_gyro_cal;

APMrover2/Parameters.pde

@@ -77,7 +77,18 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @Values: 1:1200,2:2400,4:4800,9:9600,19:19200,38:38400,57:57600,111:111100,115:115200,500:500000,921:921600,1500:1500000
     // @User: Standard
     GSCALAR(serial2_baud,           "SERIAL2_BAUD",   SERIAL2_BAUD/1000),
-#endif
+
+#if FRSKY_TELEM_ENABLED == ENABLED
+    // @Param: SERIAL2_PROTOCOL
+    // @DisplayName: SERIAL2 protocol selection
+    // @Description: Control what protocol telemetry 2 port should be used for
+    // @Values: 1:GCS Mavlink,2:Frsky D-PORT
+    // @User: Standard
+    GSCALAR(serial2_protocol,        "SERIAL2_PROTOCOL", SERIAL2_MAVLINK),
+#endif // FRSKY_TELEM_ENABLED
+
+#endif // MAVLINK_COMM_NUM_BUFFERS
+
 
     // @Param: TELEM_DELAY
     // @DisplayName: Telemetry startup delay 

APMrover2/config.h

@@ -122,6 +122,19 @@
 # define SERIAL2_BAUD			 57600
 #endif
 
+//////////////////////////////////////////////////////////////////////////////
+// FrSky telemetry support
+//
+
+#ifndef FRSKY_TELEM_ENABLED
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1 || CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # define FRSKY_TELEM_ENABLED DISABLED
+#else
+ # define FRSKY_TELEM_ENABLED ENABLED
+#endif
+#endif
+
+
 #ifndef CH7_OPTION
 # define CH7_OPTION		          CH7_SAVE_WP
 #endif

APMrover2/defines.h

@@ -138,4 +138,10 @@ enum mode {
 // mark a function as not to be inlined
 #define NOINLINE __attribute__((noinline))
 
+enum Telem2Protocol {
+    SERIAL2_MAVLINK     = 1,
+    SERIAL2_FRSKY_DPORT = 2,
+    SERIAL2_FRSKY_SPORT = 3 // not supported yet
+};
+
 #endif // _DEFINES_H

APMrover2/system.pde

@@ -138,7 +138,12 @@ static void init_ardupilot()
     gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, 128);
 
 #if MAVLINK_COMM_NUM_BUFFERS > 2
-    gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, 128);
+    if (g.serial2_protocol == SERIAL2_FRSKY_DPORT || 
+        g.serial2_protocol == SERIAL2_FRSKY_SPORT) {
+        frsky_telemetry.init(hal.uartD, g.serial2_protocol);
+    } else {
+        gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, 128);
+    }
 #endif
 
 	mavlink_system.sysid = g.sysid_this_mav;
@@ -511,3 +516,14 @@ static bool should_log(uint32_t mask)
     }
     return ret;
 }
+
+/*
+  send FrSky telemetry. Should be called at 5Hz by scheduler
+ */
+static void telemetry_send(void)
+{
+#if FRSKY_TELEM_ENABLED == ENABLED
+    frsky_telemetry.send_frames((uint8_t)control_mode, 
+                                (AP_Frsky_Telem::FrSkyProtocol)g.serial2_protocol.get());
+#endif
+}