From 71921f3aa4b8026e4544995820c169f8c4d873dd Mon Sep 17 00:00:00 2001 From: floaledm Date: Tue, 3 May 2016 10:05:09 -0500 Subject: [PATCH] Plane: removed frsky_telemetry_send scheduled task --- ArduPlane/ArduPlane.cpp | 3 --- ArduPlane/Plane.h | 1 - ArduPlane/system.cpp | 14 ++------------ 3 files changed, 2 insertions(+), 16 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index ee83d9b4ca..6f7fce342b 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -79,9 +79,6 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(update_logging1, 10, 300), SCHED_TASK(update_logging2, 10, 300), SCHED_TASK(parachute_check, 10, 200), -#if FRSKY_TELEM_ENABLED == ENABLED - SCHED_TASK(frsky_telemetry_send, 5, 100), -#endif SCHED_TASK(terrain_update, 10, 200), SCHED_TASK(update_is_flying_5Hz, 5, 100), SCHED_TASK(dataflash_periodic, 50, 400), diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6aaa929893..574c710083 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1003,7 +1003,6 @@ private: void print_comma(void); void servo_write(uint8_t ch, uint16_t pwm); bool should_log(uint32_t mask); - void frsky_telemetry_send(void); int8_t throttle_percentage(void); void change_arm_state(void); bool disarm_motors(void); diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index 9e08dad4e3..344022af4c 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -159,7 +159,8 @@ void Plane::init_ardupilot() // setup frsky #if FRSKY_TELEM_ENABLED == ENABLED - frsky_telemetry.init(serial_manager); + // setup frsky, and pass a number of parameters to the library + frsky_telemetry.init(serial_manager, (uint8_t *)&control_mode); #endif mavlink_system.sysid = g.sysid_this_mav; @@ -782,17 +783,6 @@ bool Plane::should_log(uint32_t mask) #endif } -/* - send FrSky telemetry. Should be called at 5Hz by scheduler - */ -#if FRSKY_TELEM_ENABLED == ENABLED -void Plane::frsky_telemetry_send(void) -{ - frsky_telemetry.send_frames((uint8_t)control_mode); -} -#endif - - /* return throttle percentage from 0 to 100 for normal use and -100 to 100 when using reverse thrust */