From e3860595d75e8b2bbd4fc03a4d855cc31ad7238b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 28 Nov 2012 08:58:11 +1100 Subject: [PATCH] Rover: cleaned up a bunch more plane cruft removed elevon and flap support --- APMrover2/APMrover2.pde | 19 ++------- APMrover2/Parameters.h | 25 ++++-------- APMrover2/Parameters.pde | 1 - APMrover2/config.h | 15 ------- APMrover2/control_modes.pde | 32 --------------- APMrover2/radio.pde | 80 +++++-------------------------------- APMrover2/system.pde | 3 -- APMrover2/test.pde | 45 --------------------- 8 files changed, 23 insertions(+), 197 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 5475de06a4..8c232d3255 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -363,13 +363,6 @@ byte control_mode = INITIALISING; // Used to maintain the state of the previous control switch position // This is set to -1 when we need to re-read the switch byte oldSwitchPosition; -// 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; // These are values received from the GCS if the user is using GCS joystick // control and are substituted for the values coming from the RC radio static int16_t rc_override[8] = {0,0,0,0,0,0,0,0}; @@ -904,10 +897,6 @@ static void slow_loop() // ------------------------------- read_control_switch(); - // Read Control Surfaces/Mix switches - // ---------------------------------- - update_servo_switches(); - update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); #if MOUNT == ENABLED @@ -1045,10 +1034,10 @@ static void update_navigation() // update_loiter(); calc_nav_roll(); calc_bearing_error(); - if(verify_RTL()) - { g.channel_throttle.servo_out = g.throttle_min.get(); - set_mode(MANUAL); - } + if(verify_RTL()) { + g.channel_throttle.servo_out = g.throttle_min.get(); + set_mode(MANUAL); + } break; } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index a0a78a01b0..7d2589c072 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -37,14 +37,14 @@ public: k_param_auto_trim, k_param_switch_enable, k_param_log_bitmask, - k_param_mix_mode, - k_param_reverse_elevons, - k_param_reverse_ch1_elevon, - k_param_reverse_ch2_elevon, - k_param_flap_1_percent, - k_param_flap_1_speed, - k_param_flap_2_percent, - k_param_flap_2_speed, + k_param_mix_mode, // unused + k_param_reverse_elevons, // unused + k_param_reverse_ch1_elevon, // unused + k_param_reverse_ch2_elevon, // unused + k_param_flap_1_percent, // unused + k_param_flap_1_speed, // unused + k_param_flap_2_percent, // unused + k_param_flap_2_speed, // unused k_param_num_resets, k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_reset_switch_chan, @@ -303,11 +303,6 @@ public: // Misc // AP_Int8 auto_trim; - AP_Int8 switch_enable; - AP_Int8 mix_mode; - AP_Int8 reverse_elevons; - AP_Int8 reverse_ch1_elevon; - AP_Int8 reverse_ch2_elevon; AP_Int16 num_resets; AP_Int16 log_bitmask; AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change @@ -334,10 +329,6 @@ public: // 3 = HRLV #endif #endif - AP_Int8 flap_1_percent; - AP_Int8 flap_1_speed; - AP_Int8 flap_2_percent; - AP_Int8 flap_2_speed; // ************ ThermoPilot parameters ************************ // - JLN update diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 5fd544e4a5..2fb1a49dd5 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -58,7 +58,6 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(pitch_limit_min, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE), GSCALAR(auto_trim, "TRIM_AUTO", AUTO_TRIM), - GSCALAR(switch_enable, "SWITCH_ENABLE", REVERSE_SWITCH), GSCALAR(num_resets, "SYS_NUM_RESETS", 0), GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK), GSCALAR(log_last_filenumber, "LOG_LASTFILE", 0), diff --git a/APMrover2/config.h b/APMrover2/config.h index 178f15a0f7..fccb9ff771 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -345,14 +345,6 @@ # define MANUAL_LEVEL DISABLED #endif -////////////////////////////////////////////////////////////////////////////// -// THROTTLE_REVERSE -// -#ifndef THROTTLE_REVERSE -# define THROTTLE_REVERSE DISABLED -#endif - - ////////////////////////////////////////////////////////////////////////////// // ENABLE_STICK_MIXING // @@ -382,13 +374,6 @@ # define GROUND_START_DELAY 0 #endif -////////////////////////////////////////////////////////////////////////////// -// ENABLE REVERSE_SWITCH -// -#ifndef REVERSE_SWITCH -# define REVERSE_SWITCH ENABLED -#endif - ////////////////////////////////////////////////////////////////////////////// // MOUNT (ANTENNA OR CAMERA) // diff --git a/APMrover2/control_modes.pde b/APMrover2/control_modes.pde index 9e54f522a8..efb6a07b9a 100644 --- a/APMrover2/control_modes.pde +++ b/APMrover2/control_modes.pde @@ -121,35 +121,3 @@ if (g.ch7_option == CH7_SAVE_WP){ // set to 1 } } -static void update_servo_switches() -{ -#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 - -#if HIL_MODE != HIL_MODE_ATTITUDE // JLN update - if (!g.switch_enable) { - // switches are disabled in EEPROM (see SWITCH_ENABLE option) - // this means the EEPROM control of all channel reversal is enabled - return; - } - // up is reverse - // up is elevon - g.mix_mode = (PINL & 128) ? 1 : 0; // 1 for elevon mode - if (g.mix_mode == 0) { - g.channel_roll.set_reverse((PINE & 128) ? true : false); - g.channel_pitch.set_reverse((PINE & 64) ? true : false); - g.channel_rudder.set_reverse((PINL & 64) ? true : false); - } else { - g.reverse_elevons = (PINE & 128) ? true : false; - g.reverse_ch1_elevon = (PINE & 64) ? true : false; - g.reverse_ch2_elevon = (PINL & 64) ? true : false; - } -#else - g.mix_mode = 0; // 1 for elevon mode // setup for the the Predator MQ9 of AeroSim - JLN update - g.channel_roll.set_reverse(false); // roll reversed - g.channel_pitch.set_reverse(false); // pitch normal - g.channel_rudder.set_reverse(false); // yaw normal -#endif -#endif -} - - diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde index 23685576dc..a2a2bb97ef 100644 --- a/APMrover2/radio.pde +++ b/APMrover2/radio.pde @@ -7,9 +7,6 @@ static byte failsafeCounter = 0; // we wait a second to take over the throttle static void init_rc_in() { - // set rc reversing - update_servo_switches(); - // set rc channel ranges g.channel_roll.set_angle(SERVO_MAX); g.channel_pitch.set_angle(SERVO_MAX); @@ -47,13 +44,13 @@ static void init_rc_out() #if HIL_MODE != HIL_MODE_ATTITUDE APM_RC.OutputCh(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs APM_RC.OutputCh(CH_2, g.channel_pitch.radio_trim); - APM_RC.OutputCh(CH_3, g.channel_throttle.radio_min); + APM_RC.OutputCh(CH_3, g.channel_throttle.radio_trim); APM_RC.OutputCh(CH_4, g.channel_rudder.radio_trim); APM_RC.OutputCh(CH_5, g.rc_5.radio_trim); APM_RC.OutputCh(CH_6, g.rc_6.radio_trim); APM_RC.OutputCh(CH_7, g.rc_7.radio_trim); - APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); + APM_RC.OutputCh(CH_8, g.rc_8.radio_trim); #else APM_RC.OutputCh(CH_1, 1500); // Initialization of servo outputs APM_RC.OutputCh(CH_2, 1500); @@ -63,23 +60,15 @@ static void init_rc_out() APM_RC.OutputCh(CH_5, 1500); APM_RC.OutputCh(CH_6, 1500); APM_RC.OutputCh(CH_7, 1500); - APM_RC.OutputCh(CH_8, 2000); + APM_RC.OutputCh(CH_8, 2000); #endif } static void read_radio() { - ch1_temp = APM_RC.InputCh(CH_ROLL); - ch2_temp = APM_RC.InputCh(CH_PITCH); - - if(g.mix_mode == 0){ - g.channel_roll.set_pwm(ch1_temp); - g.channel_pitch.set_pwm(ch2_temp); - }else{ - g.channel_roll.set_pwm(BOOL_TO_SIGN(g.reverse_elevons) * (BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) - BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); - g.channel_pitch.set_pwm((BOOL_TO_SIGN(g.reverse_ch2_elevon) * int(ch2_temp - elevon2_trim) + BOOL_TO_SIGN(g.reverse_ch1_elevon) * int(ch1_temp - elevon1_trim)) / 2 + 1500); - } + g.channel_roll.set_pwm(APM_RC.InputCh(CH_ROLL)); + g.channel_pitch.set_pwm(APM_RC.InputCh(CH_PITCH)); g.channel_throttle.set_pwm(APM_RC.InputCh(CH_3)); g.channel_rudder.set_pwm(APM_RC.InputCh(CH_4)); @@ -88,11 +77,6 @@ static void read_radio() g.rc_7.set_pwm(APM_RC.InputCh(CH_7)); g.rc_8.set_pwm(APM_RC.InputCh(CH_8)); - // TO DO - go through and patch throttle reverse for RC_Channel library compatibility - #if THROTTLE_REVERSE == 1 - g.channel_throttle.radio_in = g.channel_throttle.radio_max + g.channel_throttle.radio_min - g.channel_throttle.radio_in; - #endif - control_failsafe(g.channel_throttle.radio_in); g.channel_throttle.servo_out = g.channel_throttle.control_in; @@ -162,33 +146,21 @@ static void trim_control_surfaces() read_radio(); // Store control surface trim values // --------------------------------- - if(g.mix_mode == 0){ - if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) { + if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) { g.channel_roll.radio_trim = g.channel_roll.radio_max + g.channel_roll.radio_min - g.channel_roll.radio_in; g.channel_pitch.radio_trim = g.channel_pitch.radio_max + g.channel_pitch.radio_min - g.channel_pitch.radio_in; g.channel_rudder.radio_trim = g.channel_rudder.radio_max + g.channel_rudder.radio_min - g.channel_rudder.radio_in; RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron); - } else { + } else { g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active g.channel_pitch.radio_trim = 1500; - g.channel_rudder.radio_trim = 1500; - g.channel_throttle.radio_trim = 1000; - } - - }else{ - elevon1_trim = ch1_temp; - elevon2_trim = 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 - uint16_t center = 1500; - g.channel_roll.radio_trim = center; - g.channel_pitch.radio_trim = center; - } + g.channel_rudder.radio_trim = 1500; + g.channel_throttle.radio_trim = 1000; + } // save to eeprom g.channel_roll.save_eeprom(); g.channel_pitch.save_eeprom(); - //g.channel_throttle.save_eeprom(); g.channel_rudder.save_eeprom(); } @@ -197,35 +169,5 @@ static void trim_radio() for (int y = 0; y < 30; y++) { read_radio(); } - - // Store the trim values - // --------------------- - if(g.mix_mode == 0){ - if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) { - g.channel_roll.radio_trim = g.channel_roll.radio_in; - g.channel_pitch.radio_trim = g.channel_pitch.radio_in; - //g.channel_throttle.radio_trim = g.channel_throttle.radio_in; - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron); - } else { - g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active - g.channel_pitch.radio_trim = 1500; - g.channel_rudder.radio_trim = 1500; - g.channel_throttle.radio_trim = 1000; - } - - } else { - elevon1_trim = ch1_temp; - elevon2_trim = ch2_temp; - uint16_t center = 1500; - g.channel_roll.radio_trim = center; - g.channel_pitch.radio_trim = center; - g.channel_rudder.radio_trim = g.channel_rudder.radio_in; - } - - // save to eeprom - g.channel_roll.save_eeprom(); - g.channel_pitch.save_eeprom(); - //g.channel_throttle.save_eeprom(); - g.channel_rudder.save_eeprom(); + trim_control_surfaces(); } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index e4deba5cd9..eea0397ba4 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -297,9 +297,6 @@ static void init_ardupilot() #endif #endif // CLI_ENABLED - // read in the flight switches - update_servo_switches(); - startup_ground(); #if LITE == DISABLED diff --git a/APMrover2/test.pde b/APMrover2/test.pde index 2cf1ab26f9..1e845fbfd6 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -24,9 +24,6 @@ static int8_t test_xbee(uint8_t argc, const Menu::arg *argv); static int8_t test_eedump(uint8_t argc, const Menu::arg *argv); static int8_t test_rawgps(uint8_t argc, const Menu::arg *argv); static int8_t test_modeswitch(uint8_t argc, const Menu::arg *argv); -#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 -static int8_t test_dipswitches(uint8_t argc, const Menu::arg *argv); -#endif static int8_t test_logging(uint8_t argc, const Menu::arg *argv); // Creates a constant array of structs representing menu options @@ -44,9 +41,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"xbee", test_xbee}, {"eedump", test_eedump}, {"modeswitch", test_modeswitch}, -#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 - {"dipswitches", test_dipswitches}, -#endif // Tests below here are for hardware sensors only present // when real sensors are attached or they are emulated @@ -165,11 +159,6 @@ test_radio(uint8_t argc, const Menu::arg *argv) print_hit_enter(); delay(1000); - #if THROTTLE_REVERSE == 1 - cliSerial->printf_P(PSTR("Throttle is reversed in config: \n")); - delay(1000); - #endif - // read the radio to set trims // --------------------------- trim_radio(); @@ -177,7 +166,6 @@ test_radio(uint8_t argc, const Menu::arg *argv) while(1){ delay(20); read_radio(); - update_servo_switches(); g.channel_roll.calc_pwm(); g.channel_pitch.calc_pwm(); @@ -417,39 +405,6 @@ test_logging(uint8_t argc, const Menu::arg *argv) return 0; } -#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 -static int8_t -test_dipswitches(uint8_t argc, const Menu::arg *argv) -{ - print_hit_enter(); - delay(1000); - - if (!g.switch_enable) { - cliSerial->println_P(PSTR("dip switches disabled, using EEPROM")); - } - - while(1){ - delay(100); - update_servo_switches(); - - if (g.mix_mode == 0) { - cliSerial->printf_P(PSTR("Mix:standard \trev roll:%d, rev pitch:%d, rev rudder:%d\n"), - (int)g.channel_roll.get_reverse(), - (int)g.channel_pitch.get_reverse(), - (int)g.channel_rudder.get_reverse()); - } else { - cliSerial->printf_P(PSTR("Mix:elevons \trev elev:%d, rev ch1:%d, rev ch2:%d\n"), - (int)g.reverse_elevons, - (int)g.reverse_ch1_elevon, - (int)g.reverse_ch2_elevon); - } - if(cliSerial->available() > 0){ - return (0); - } - } -} -#endif // CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 - //------------------------------------------------------------------------------------------- // tests in this section are for real sensors or sensors that have been simulated