From a1f64d606ab408ef7463a05f033ce6e128d72aa3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 3 Jun 2013 16:12:41 +1000 Subject: [PATCH] Plane: fixed remaining places where primary channel ordering was assumed should now use rcmap for all inputs --- ArduPlane/Attitude.pde | 12 ++++++------ ArduPlane/defines.h | 6 ------ ArduPlane/failsafe.pde | 16 ++++++++-------- ArduPlane/radio.pde | 32 +++++++++++++------------------- 4 files changed, 27 insertions(+), 39 deletions(-) diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 23f8d7c8d8..17e6418c03 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -524,8 +524,8 @@ static void set_servos(void) channel_roll->radio_out = channel_roll->radio_in; channel_pitch->radio_out = channel_pitch->radio_in; } else { - channel_roll->radio_out = hal.rcin->read(CH_ROLL); - channel_pitch->radio_out = hal.rcin->read(CH_PITCH); + channel_roll->radio_out = channel_roll->read(); + channel_pitch->radio_out = channel_pitch->read(); } channel_throttle->radio_out = channel_throttle->radio_in; channel_rudder->radio_out = channel_rudder->radio_in; @@ -700,10 +700,10 @@ static void set_servos(void) // send values to the PWM timers for output // ---------------------------------------- - hal.rcout->write(CH_1, channel_roll->radio_out); // send to Servos - hal.rcout->write(CH_2, channel_pitch->radio_out); // send to Servos - hal.rcout->write(CH_3, channel_throttle->radio_out); // send to Servos - hal.rcout->write(CH_4, channel_rudder->radio_out); // send to Servos + channel_roll->output(); + channel_pitch->output(); + channel_throttle->output(); + channel_rudder->output(); // Route configurable aux. functions to their respective servos g.rc_5.output_ch(CH_5); g.rc_6.output_ch(CH_6); diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 434431db30..fbc802134b 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -48,12 +48,6 @@ #define GPS_PROTOCOL_MTK19 6 #define GPS_PROTOCOL_AUTO 7 -#define CH_ROLL CH_1 -#define CH_PITCH CH_2 -#define CH_THROTTLE CH_3 -#define CH_RUDDER CH_4 -#define CH_YAW CH_4 - // HIL enumerations. Note that HIL_MODE_ATTITUDE and HIL_MODE_SENSORS // are now the same thing, and are sensors based. The old define is // kept to allow old APM_Config.h headers to keep working diff --git a/ArduPlane/failsafe.pde b/ArduPlane/failsafe.pde index 3f04c2c026..c601f60556 100644 --- a/ArduPlane/failsafe.pde +++ b/ArduPlane/failsafe.pde @@ -39,21 +39,21 @@ void failsafe_check(uint32_t tnow) // pass RC inputs to outputs every 20ms last_timestamp = tnow; hal.rcin->clear_overrides(); - channel_roll->radio_out = hal.rcin->read(CH_1); - channel_pitch->radio_out = hal.rcin->read(CH_2); - channel_throttle->radio_out = hal.rcin->read(CH_3); - channel_rudder->radio_out = hal.rcin->read(CH_4); + channel_roll->radio_out = channel_roll->read(); + channel_pitch->radio_out = channel_pitch->read(); + channel_throttle->radio_out = channel_throttle->read(); + channel_rudder->radio_out = channel_rudder->read(); if (g.vtail_output != MIXING_DISABLED) { channel_output_mixer(g.vtail_output, channel_pitch->radio_out, channel_rudder->radio_out); } else if (g.elevon_output != MIXING_DISABLED) { channel_output_mixer(g.elevon_output, channel_pitch->radio_out, channel_roll->radio_out); } if (!demoing_servos) { - servo_write(CH_1, channel_roll->radio_out); + channel_roll->output(); } - servo_write(CH_2, channel_pitch->radio_out); - servo_write(CH_3, channel_throttle->radio_out); - servo_write(CH_4, channel_rudder->radio_out); + channel_pitch->output(); + channel_throttle->output(); + channel_rudder->output(); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true); RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true); diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde index b93e74a75b..1d8bb4835f 100644 --- a/ArduPlane/radio.pde +++ b/ArduPlane/radio.pde @@ -36,22 +36,16 @@ static void init_rc_in() static void init_rc_out() { - hal.rcout->enable_ch(CH_1); - hal.rcout->enable_ch(CH_2); - hal.rcout->enable_ch(CH_3); - hal.rcout->enable_ch(CH_4); + channel_roll->enable_out(); + channel_pitch->enable_out(); + channel_throttle->enable_out(); + channel_rudder->enable_out(); enable_aux_servos(); // Initialization of servo outputs - servo_write(CH_1, channel_roll->radio_trim); - servo_write(CH_2, channel_pitch->radio_trim); - servo_write(CH_3, channel_throttle->radio_min); - servo_write(CH_4, channel_rudder->radio_trim); - - servo_write(CH_5, g.rc_5.radio_trim); - servo_write(CH_6, g.rc_6.radio_trim); - servo_write(CH_7, g.rc_7.radio_trim); - servo_write(CH_8, g.rc_8.radio_trim); + for (uint8_t i=0; i<8; i++) { + RC_Channel::rc_channel(i)->output_trim(); + } #if CONFIG_HAL_BOARD == HAL_BOARD_APM2 || CONFIG_HAL_BOARD == HAL_BOARD_PX4 servo_write(CH_9, g.rc_9.radio_trim); @@ -65,8 +59,8 @@ static void init_rc_out() static void read_radio() { - elevon.ch1_temp = hal.rcin->read(CH_ROLL); - elevon.ch2_temp = hal.rcin->read(CH_PITCH); + elevon.ch1_temp = channel_roll->read(); + elevon.ch2_temp = channel_pitch->read(); uint16_t pwm_roll, pwm_pitch; if (g.mix_mode == 0) { @@ -82,13 +76,13 @@ static void read_radio() // 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(hal.rcin->read(CH_3)); - channel_rudder->set_pwm_no_deadzone(hal.rcin->read(CH_4)); + 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_throttle->set_pwm(hal.rcin->read(CH_3)); - channel_rudder->set_pwm(hal.rcin->read(CH_4)); + channel_throttle->set_pwm(channel_throttle->read()); + channel_rudder->set_pwm(channel_rudder->read()); } g.rc_5.set_pwm(hal.rcin->read(CH_5));