From 8f091e70c8181bb830278fd0ba07ecd0d9da3ae2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 15 Apr 2013 14:53:56 +1000 Subject: [PATCH] Plane: move elevon variables into a structure --- ArduPlane/ArduPlane.pde | 23 ++++++++++++++++------- ArduPlane/Attitude.pde | 4 ++-- ArduPlane/radio.pde | 20 ++++++++++---------- 3 files changed, 28 insertions(+), 19 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 68b5b38336..d0554a96cc 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -295,13 +295,22 @@ enum FlightMode control_mode = INITIALISING; uint8_t oldSwitchPosition; // This is used to enable the inverted flight feature bool inverted_flight = false; -// 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 -static uint16_t elevon1_trim = 1500; -static uint16_t elevon2_trim = 1500; -// These are used in the calculation of elevon1_trim and elevon2_trim -static uint16_t ch1_temp = 1500; -static uint16_t ch2_temp = 1500; + +static 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 = { + .trim1 = 1500, + .trim2 = 1500, + .ch1_temp = 1500, + .ch2_temp = 1500 +}; // A flag if GCS joystick control is in use static bool rc_override_active = false; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index de289d1d5b..03d7fc6331 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -610,8 +610,8 @@ static void set_servos(void) } // directly set the radio_out values for elevon mode - g.channel_roll.radio_out = elevon1_trim + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); - g.channel_pitch.radio_out = elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); + g.channel_roll.radio_out = elevon.trim1 + (BOOL_TO_SIGN(g.reverse_ch1_elevon) * (ch1 * 500.0/ SERVO_MAX)); + g.channel_pitch.radio_out = elevon.trim2 + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX)); } #if OBC_FAILSAFE == ENABLED diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index d268b9a641..c301446013 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -71,16 +71,16 @@ static void init_rc_out() static void read_radio() { - ch1_temp = hal.rcin->read(CH_ROLL); - ch2_temp = hal.rcin->read(CH_PITCH); + elevon.ch1_temp = hal.rcin->read(CH_ROLL); + elevon.ch2_temp = hal.rcin->read(CH_PITCH); uint16_t pwm_roll, pwm_pitch; if (g.mix_mode == 0) { - pwm_roll = ch1_temp; - pwm_pitch = ch2_temp; + pwm_roll = elevon.ch1_temp; + pwm_pitch = elevon.ch2_temp; }else{ - pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(ch1_temp - elevon1_trim)) / 2 + 1500; - pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(ch1_temp - elevon1_trim)) / 2 + 1500; + pwm_roll = BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; + pwm_pitch = (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int16_t(elevon.ch2_temp - elevon.trim2) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int16_t(elevon.ch1_temp - elevon.trim1)) / 2 + 1500; } if (control_mode == TRAINING) { @@ -202,11 +202,11 @@ static void trim_control_surfaces() RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron_with_input); RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_elevator_with_input); } else{ - if (ch1_temp != 0) { - elevon1_trim = ch1_temp; + if (elevon.ch1_temp != 0) { + elevon.trim1 = elevon.ch1_temp; } - if (ch2_temp != 0) { - elevon2_trim = ch2_temp; + if (elevon.ch2_temp != 0) { + elevon.trim2 = elevon.ch2_temp; } //Recompute values here using new values for elevon1_trim and elevon2_trim //We cannot use radio_in[CH_ROLL] and radio_in[CH_PITCH] values from read_radio() because the elevon trim values have changed