From 7b7f5e242be9d25393e1a4d6dab90d4a936f12c5 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 10 Apr 2018 15:14:57 -0700 Subject: [PATCH] Plane: Simplify channel reading --- ArduPlane/Plane.h | 11 ----------- ArduPlane/radio.cpp | 18 ++++-------------- 2 files changed, 4 insertions(+), 25 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 19f6c620f3..2f302d2cd1 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -309,17 +309,6 @@ private: // This is used to enable the PX4IO override for testing bool px4io_override_enabled; - struct { - // These are trim values used for elevon control - // For elevons radio_in[CH_ROLL] and radio_in[CH_PITCH] are - // equivalent aileron and elevator, not left and right elevon - uint16_t trim1; - uint16_t trim2; - // These are used in the calculation of elevon1_trim and elevon2_trim - uint16_t ch1_temp; - uint16_t ch2_temp; - } elevon { 1500, 1500, 1500, 1500 }; - // Failsafe struct { // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index 96b2c03bf2..0061e65291 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -183,25 +183,15 @@ void Plane::read_radio() failsafe.last_valid_rc_ms = millis(); - elevon.ch1_temp = channel_roll->read(); - elevon.ch2_temp = channel_pitch->read(); - uint16_t pwm_roll, pwm_pitch; - - pwm_roll = elevon.ch1_temp; - pwm_pitch = elevon.ch2_temp; - RC_Channels::set_pwm_all(); if (control_mode == TRAINING) { // in training mode we don't want to use a deadzone, as we // want manual pass through when not exceeding attitude limits - channel_roll->set_pwm_no_deadzone(pwm_roll); - channel_pitch->set_pwm_no_deadzone(pwm_pitch); - channel_throttle->set_pwm_no_deadzone(channel_throttle->read()); - channel_rudder->set_pwm_no_deadzone(channel_rudder->read()); - } else { - channel_roll->set_pwm(pwm_roll); - channel_pitch->set_pwm(pwm_pitch); + channel_roll->recompute_pwm_no_deadzone(); + channel_pitch->recompute_pwm_no_deadzone(); + channel_throttle->recompute_pwm_no_deadzone(); + channel_rudder->recompute_pwm_no_deadzone(); } control_failsafe();