diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index d9300e7919..a75c2b7c38 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -79,6 +79,7 @@ #include #include +#include #include #include @@ -460,6 +461,12 @@ static int32_t altitude_error_cm; //////////////////////////////////////////////////////////////////////////////// static AP_BattMonitor battery; +//////////////////////////////////////////////////////////////////////////////// +// FrSky telemetry support +#if FRSKY_TELEM_ENABLED == ENABLED +static AP_Frsky_Telem frsky_telemetry(ahrs, battery); +#endif + //////////////////////////////////////////////////////////////////////////////// // Airspeed Sensors //////////////////////////////////////////////////////////////////////////////// @@ -766,6 +773,10 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { { compass_save, 3000, 2500 }, { update_logging1, 5, 1700 }, { update_logging2, 5, 1700 }, +#if FRSKY_TELEM_ENABLED == ENABLED + { telemetry_send, 10, 100 }, +#endif + }; // setup the var_info table diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index b262f13869..f86fc84e9d 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -138,6 +138,7 @@ public: k_param_serial0_baud_old, // deprecated k_param_gcs2, // stream rates for uartD k_param_serial2_baud_old, // deprecated + k_param_serial2_protocol, // 120: Fly-by-wire control // @@ -308,6 +309,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; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 413b90f64b..3a70859944 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -59,7 +59,17 @@ 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: AUTOTUNE_LEVEL // @DisplayName: Autotune level diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 03b9ea1b4a..fb46b66f0f 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -77,6 +77,9 @@ #ifndef CAMERA # define CAMERA DISABLED #endif + #ifndef FRSKY_TELEM_ENABLED + # define FRSKY_TELEM_ENABLED DISABLED + #endif #endif ////////////////////////////////////////////////////////////////////////////// @@ -123,6 +126,18 @@ # 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 + ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 021b461123..1067712d04 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -211,4 +211,10 @@ enum { ATT_CONTROL_APMCONTROL = 1 }; +enum Telem2Protocol { + SERIAL2_MAVLINK = 1, + SERIAL2_FRSKY_DPORT = 2, + SERIAL2_FRSKY_SPORT = 3 // not supported yet +}; + #endif // _DEFINES_H diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 80898aef1f..dbd79c66d5 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -119,7 +119,12 @@ static void init_ardupilot() gcs[1].setup_uart(hal.uartC, map_baudrate(g.serial1_baud), 128, SERIAL1_BUFSIZE); #if MAVLINK_COMM_NUM_BUFFERS > 2 - gcs[2].setup_uart(hal.uartD, map_baudrate(g.serial2_baud), 128, SERIAL2_BUFSIZE); + 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, SERIAL2_BUFSIZE); + } #endif mavlink_system.sysid = g.sysid_this_mav; @@ -618,3 +623,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 +}