diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index 7b1fbdeb5f..a9e8b19fd7 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -1,3 +1,28 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- +// Example config file. Use APM_Config.h.reference and the wiki to find additional defines to setup your plane. +// Once you upload the code, run the factory "reset" to save all config values to EEPROM. +// After reset, use the setup mode to set your radio limits for CH1-4, and to set your flight modes. + +// GPS is auto-selected + +#define GCS_PROTOCOL GCS_PROTOCOL_NONE + +//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_UP_PINS_BACK + +# define SERIAL0_BAUD 38400 + +//# define STABILIZE_ROLL_P 0.4 +//# define STABILIZE_PITCH_P 0.4 +//# define STABILIZE_DAMPENER 0.1 + + +//#define ACRO_RATE_TRIGGER 4200 +// if you want full ACRO mode, set value to 0 +// if you want safe ACRO mode, set value to 100 +// if you want mostly stabilize with flips, set value to 4200 + + + +// Logging +//#define LOG_CURRENT ENABLED + -#include "configs/tridge.h" diff --git a/ArduCopterMega/config.h b/ArduCopterMega/config.h index f308697808..d669500c6e 100644 --- a/ArduCopterMega/config.h +++ b/ArduCopterMega/config.h @@ -558,16 +558,3 @@ #ifndef ALLOW_RC_OVERRIDE # define ALLOW_RC_OVERRIDE DISABLED #endif - -////////////////////////////////////////////////////////////////////////////// -// Motor arming -// -// The default is for MODE2 setups. We wait for throttle to reach -// zero, and rudder to be at maximum extent. -#ifndef MOTOR_ARM_CONDITION -# define MOTOR_ARM_CONDITION (g.rc_3.control_in == 0 && g.rc_4.control_in > 2700) -#endif - -#ifndef MOTOR_DISARM_CONDITION -# define MOTOR_DISARM_CONDITION (g.rc_3.control_in == 0 && g.rc_4.control_in < -2700) -#endif diff --git a/ArduCopterMega/configs/tridge.h b/ArduCopterMega/configs/tridge.h deleted file mode 100644 index 5cadb5aa6b..0000000000 --- a/ArduCopterMega/configs/tridge.h +++ /dev/null @@ -1,49 +0,0 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- - -// Example config file. Use APM_Config.h.reference and the wiki to find additional defines to setup your plane. -// Once you upload the code, run the factory "reset" to save all config values to EEPROM. -// After reset, use the setup mode to set your radio limits for CH1-4, and to set your flight modes. - -// GPS is auto-selected - -#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD - -#define SERIAL0_BAUD 115200 -#define SERIAL3_BAUD 115200 - -// Hardware in the loop protocol -#if 0 -# define HIL_MODE HIL_MODE_NONE -#else -# define HIL_MODE HIL_MODE_ATTITUDE -# define HIL_PROTOCOL HIL_PROTOCOL_MAVLINK -# define HIL_PORT 0 -#endif - -// You can set your gps protocol here for your actual -// hardware and leave it without affecting the hardware -// in the loop simulation - -// Ground control station comms -#define GCS_PROTOCOL GCS_PROTOCOL_MAVLINK -#define GCS_PORT 0 - -//#define RADIO_OVERRIDE_DEFAULTS { 1500, 1500, 1000, 1500, 1000, 1000, 1000, 1815 } - -#define FLIGHT_MODE_CHANNEL CH_8 - -#define FLIGHT_MODE_1 STABILIZE -#define FLIGHT_MODE_2 ALT_HOLD -#define FLIGHT_MODE_3 ACRO -#define FLIGHT_MODE_4 STABILIZE -#define FLIGHT_MODE_5 STABILIZE -#define FLIGHT_MODE_6 STABILIZE - -//#define ALWAYS_RESET_RADIO_RANGE 1 -//#define ALWAYS_RESET_MODES 1 - -#define GPS_PROTOCOL GPS_PROTOCOL_UBLOX - -// Use MODE1 arming -#define MOTOR_ARM_CONDITION (g.rc_3.control_in == 0 && g.rc_1.control_in > 2700) -#define MOTOR_DISARM_CONDITION (g.rc_3.control_in == 0 && g.rc_1.control_in < -2700) diff --git a/ArduCopterMega/control_modes.pde b/ArduCopterMega/control_modes.pde index 8747ba8a62..2af4e17b5f 100644 --- a/ArduCopterMega/control_modes.pde +++ b/ArduCopterMega/control_modes.pde @@ -6,14 +6,12 @@ void read_control_switch() //motor_armed = (switchPosition < 5); if (oldSwitchPosition != switchPosition){ + set_mode(g.flight_modes[switchPosition]); oldSwitchPosition = switchPosition; prev_WP = current_loc; - Serial.print("Changed to flight mode: "); - Serial.println(flight_mode_strings[g.flight_modes[switchPosition]]); - // reset navigation integrators // ------------------------- reset_I(); diff --git a/ArduCopterMega/motors.pde b/ArduCopterMega/motors.pde index cf0a6f98eb..386136396b 100644 --- a/ArduCopterMega/motors.pde +++ b/ArduCopterMega/motors.pde @@ -6,28 +6,24 @@ void arm_motors() { static byte arming_counter; - // Serial.printf("rc3: %u rc4: %u\n", g.rc_3.control_in, g.rc_4.control_in); + // Arm motor output : Throttle down and full yaw right for more than 2 seconds - if (MOTOR_ARM_CONDITION) { - if (arming_counter > ARM_DELAY) { - if (!motor_armed) { - gcs.send_text(SEVERITY_MEDIUM, "Arming motors"); + if (g.rc_3.control_in == 0){ + if (g.rc_4.control_in > 2700) { + if (arming_counter > ARM_DELAY) { + motor_armed = true; + } else{ + arming_counter++; } - motor_armed = true; - } else{ - arming_counter++; - } - } else if (MOTOR_DISARM_CONDITION) { - if (arming_counter > DISARM_DELAY){ - if (motor_armed) { - gcs.send_text(SEVERITY_MEDIUM, "Dis-arming motors"); + }else if (g.rc_4.control_in < -2700) { + if (arming_counter > DISARM_DELAY){ + motor_armed = false; + }else{ + arming_counter++; } - motor_armed = false; - } else { - arming_counter++; + }else{ + arming_counter = 0; } - } else { - arming_counter = 0; } } diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index 719878ee81..60fee7f8ef 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -123,22 +123,6 @@ void init_ardupilot() init_rc_in(); // sets up rc channels from radio init_rc_out(); // sets up the timer libs - -#ifdef ALWAYS_RESET_RADIO_RANGE - { - RC_Channel *rcp = &g.rc_1; - for (unsigned char i=0; i<8; i++) { - rcp[i].radio_min = 1000; - rcp[i].radio_max = 2000; - rcp[i].save_eeprom(); - } - } -#endif - -#ifdef ALWAYS_RESET_MODES - default_flight_modes(); -#endif - init_camera(); #if HIL_MODE != HIL_MODE_ATTITUDE adc.Init(); // APM ADC library initialization