From 52f6e56f721df43379c1ea276560f23be5fe4e58 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 28 Jan 2012 13:12:19 +0900 Subject: [PATCH 01/18] AP_ADC library - tiny fix to make example sketch work after some changes to AP_PeriodicProcess library --- libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde index cf1b051f25..89f71bd3d6 100644 --- a/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde +++ b/libraries/AP_ADC/examples/AP_ADC_test/AP_ADC_test.pde @@ -12,7 +12,7 @@ FastSerialPort0(Serial); // FTDI/console Arduino_Mega_ISR_Registry isr_registry; -AP_PeriodicProcess adc_scheduler; +AP_TimerProcess adc_scheduler; unsigned long timer; From 2e1e34fb9c6d74c273e0f36200ee217b32dc79ce Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 28 Jan 2012 13:21:35 +0900 Subject: [PATCH 02/18] Arduino 1.0 - small fix to make AP_Baro_MS5611_test.pde compile under Arduino 1.0 --- .../examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde index a1ff95ac38..4b963bc2df 100644 --- a/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde +++ b/libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde @@ -1,8 +1,8 @@ #include +#include #include #include // ArduPilot Mega Vector/Matrix math Library -#include #include #include #include From 8bcc567406a43cf5038af45c56c4edc3855f6781 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 28 Jan 2012 17:54:20 +0900 Subject: [PATCH 03/18] AP_PID, AP_RC_Channel, FastSerial - small changes to make example sketches compile again --- libraries/AP_PID/examples/AP_pid/AP_pid.pde | 17 ++++++++++------- libraries/AP_RC_Channel/AP_RC_Channel.cpp | 3 +++ .../examples/AP_RC_Channel/AP_RC_Channel.pde | 1 - .../examples/FastSerial/FastSerial.pde | 7 ++++--- 4 files changed, 17 insertions(+), 11 deletions(-) diff --git a/libraries/AP_PID/examples/AP_pid/AP_pid.pde b/libraries/AP_PID/examples/AP_pid/AP_pid.pde index ca46f3d6db..5d7cf59df4 100644 --- a/libraries/AP_PID/examples/AP_pid/AP_pid.pde +++ b/libraries/AP_PID/examples/AP_pid/AP_pid.pde @@ -3,21 +3,26 @@ 2010 Code by Jason Short. DIYDrones.com */ +#include #include #include #include // ArduPilot RC Library -#include // ArduPilot Mega RC Library +#include // ArduPilot Mega RC Library long radio_in; long radio_trim; -PID pid(); +Arduino_Mega_ISR_Registry isr_registry; +AP_PID pid; +APM_RC_APM1 APM_RC; void setup() { - Serial.begin(38400); - Serial.println("ArduPilot Mega PID library test"); - APM_RC.Init(); // APM Radio initialization + Serial.begin(115200); + Serial.println("ArduPilot Mega AP_PID library test"); + + isr_registry.init(); + APM_RC.Init(&isr_registry); // APM Radio initialization delay(1000); //rc.trim(); @@ -27,12 +32,10 @@ void setup() pid.kI(0); pid.kD(0.5); pid.imax(50); - pid.save_gains(); pid.kP(0); pid.kI(0); pid.kD(0); pid.imax(0); - pid.load_gains(); Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); } diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.cpp b/libraries/AP_RC_Channel/AP_RC_Channel.cpp index eea2e91b33..af9365dd84 100644 --- a/libraries/AP_RC_Channel/AP_RC_Channel.cpp +++ b/libraries/AP_RC_Channel/AP_RC_Channel.cpp @@ -88,11 +88,14 @@ AP_RC_Channel::set_pwm(int pwm) control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; + /* + // coming soon ?? if(expo) { long temp = control_in; temp = (temp * temp) / (long)_high; control_in = (int)((control_in >= 0) ? temp : -temp); } + */ } } diff --git a/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde b/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde index f636c37aa1..a7de6e0fd4 100644 --- a/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde +++ b/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde @@ -29,7 +29,6 @@ AP_RC rc; void setup() { Serial.begin(115200); - //Serial.begin(38400); Serial.println("ArduPilot RC Channel test"); rc.init(); // APM Radio initialization diff --git a/libraries/FastSerial/examples/FastSerial/FastSerial.pde b/libraries/FastSerial/examples/FastSerial/FastSerial.pde index 4a9ae94b51..a1af765233 100644 --- a/libraries/FastSerial/examples/FastSerial/FastSerial.pde +++ b/libraries/FastSerial/examples/FastSerial/FastSerial.pde @@ -17,8 +17,9 @@ #undef PROGMEM #define PROGMEM __attribute__(( section(".progmem.data") )) -#undef PSTR -#define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); &__c[0];})) +# undef PSTR +# define PSTR(s) (__extension__({static prog_char __c[] PROGMEM = (s); \ + (prog_char_t *)&__c[0];})) // // Create a FastSerial driver that looks just like the stock Arduino @@ -38,7 +39,7 @@ void setup(void) // // Set the speed for our replacement serial port. // - Serial.begin(38400); + Serial.begin(115200); // // Test printing things From b543bbf514f4e74342a15856e5fc0d24122b3533 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 28 Jan 2012 17:56:56 +0900 Subject: [PATCH 04/18] Arduino 1.0 - GPS_IMU - small fix to make it compile. Does anyone use this lib?! --- libraries/GPS_IMU/GPS_IMU.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libraries/GPS_IMU/GPS_IMU.cpp b/libraries/GPS_IMU/GPS_IMU.cpp index e6975ccdc2..62ef39b968 100755 --- a/libraries/GPS_IMU/GPS_IMU.cpp +++ b/libraries/GPS_IMU/GPS_IMU.cpp @@ -4,7 +4,11 @@ #include "GPS_IMU.h" #include -#include "WProgram.h" +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif // Constructors //////////////////////////////////////////////////////////////// From a685510c38bba52c606554e97b16a06b84c8d463 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 22 Jan 2012 21:58:23 -0800 Subject: [PATCH 05/18] changed tuning range --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 58f240169a..1722c166e8 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1961,7 +1961,7 @@ static void tuning(){ break; case CH6_LOITER_P: - g.rc_6.set_range(0,1000); + g.rc_6.set_range(0,2000); g.pi_loiter_lat.kP(tuning_value); g.pi_loiter_lon.kP(tuning_value); break; From 8fcb2931d76aa7b12028e4e7daf862502eb94837 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Mon, 23 Jan 2012 14:10:03 -0800 Subject: [PATCH 06/18] Cosmetic changes GPS LED lock waits for home_is_set to be true --- ArduCopter/ArduCopter.pde | 4 +--- ArduCopter/Camera.pde | 5 +++-- ArduCopter/control_modes.pde | 1 - ArduCopter/leds.pde | 6 +++++- ArduCopter/motors.pde | 2 +- ArduCopter/setup.pde | 2 +- 6 files changed, 11 insertions(+), 9 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 1722c166e8..4aa9ecadd4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -325,9 +325,6 @@ static int16_t y_actual_speed; static int16_t x_rate_error; static int16_t y_rate_error; -//static int16_t my_max_speed; // used for debugging logs -//static int16_t target_x_rate; - //////////////////////////////////////////////////////////////////////////////// // Radio //////////////////////////////////////////////////////////////////////////////// @@ -1883,6 +1880,7 @@ adjust_altitude() manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); update_throttle_cruise(); + }else if (g.rc_3.control_in >= 650){ // we add 0 to 100 PWM to hover manual_boost = g.rc_3.control_in - 650; diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index 047a7061ba..f0f3df9f48 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -4,8 +4,9 @@ static void init_camera() { - APM_RC.enable_out(CH_CAM_PITCH); - APM_RC.enable_out(CH_CAM_ROLL); + APM_RC.enable_out(CH_CAM_PITCH); + APM_RC.enable_out(CH_CAM_ROLL); + // ch 6 high(right) is down. g.rc_camera_pitch.set_angle(4500); g.rc_camera_roll.set_angle(4500); diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 7beab99fbe..8911fb0962 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -154,7 +154,6 @@ static void read_trim_switch() // 1 = takeoff // 2 = WP 2 // 3 = command total - } } } diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index 69a2d8a4c6..97b05f746e 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -19,7 +19,11 @@ static void update_GPS_light(void) switch (g_gps->status()){ case(2): - digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix. + if(home_is_set) { // JLN update + digitalWrite(C_LED_PIN, LED_ON); //Turn LED C on when gps has valid fix AND home is set. + } else { + digitalWrite(C_LED_PIN, LED_OFF); + } break; case(1): diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 7cba786cae..0744b52855 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -1,7 +1,7 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // 10 = 1 second -#define ARM_DELAY 30 +#define ARM_DELAY 20 #define DISARM_DELAY 20 #define LEVEL_DELAY 100 diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 96e65b9c29..682e450229 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -1096,7 +1096,7 @@ static void print_enabled(boolean b) static void init_esc() { - motors_output_enable(); + motors_output_enable(); while(1){ read_radio(); delay(100); From e94ac68e4d7a08360f77baeeab0b5dbf09843a83 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 24 Jan 2012 00:13:56 -0800 Subject: [PATCH 07/18] formatting --- ArduCopter/sensors.pde | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 271c57ed37..2e1ed8ecd5 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -52,8 +52,8 @@ static void init_barometer(void) static void reset_baro(void) { - ground_pressure = barometer.get_pressure(); - ground_temperature = barometer.get_temperature(); + ground_pressure = barometer.get_pressure(); + ground_temperature = barometer.get_temperature(); } static int32_t read_barometer(void) @@ -107,8 +107,8 @@ static void read_battery(void) battery_voltage1 = 0; return; } - - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) + + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9; if(g.battery_monitoring == 4) { current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin From 5185901f792327e03b883ff9216701142f2cfaf7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 28 Jan 2012 21:21:43 -0800 Subject: [PATCH 08/18] Added D term to APM_PI - need to refactor this as a parent class and two child classes to save code space, remove dupes --- libraries/AC_PID/AC_PID.cpp | 118 ++++++++++++++++++++++++++ libraries/AC_PID/AC_PID.h | 152 ++++++++++++++++++++++++++++++++++ libraries/AC_PID/keywords.txt | 9 ++ 3 files changed, 279 insertions(+) create mode 100755 libraries/AC_PID/AC_PID.cpp create mode 100755 libraries/AC_PID/AC_PID.h create mode 100755 libraries/AC_PID/keywords.txt diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp new file mode 100755 index 0000000000..a50f10d6a0 --- /dev/null +++ b/libraries/AC_PID/AC_PID.cpp @@ -0,0 +1,118 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_PID.cpp +/// @brief Generic PID algorithm + +#include +#include "AC_PID.h" + + +int32_t AC_PID::get_p(int32_t error) +{ + return (float)error * _kp; +} + +int32_t AC_PID::get_i(int32_t error, float dt) +{ + if((_ki != 0) && (dt != 0)){ + _integrator += ((float)error * _ki) * dt; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + } + return _integrator; +} + +int32_t AC_PID::get_d(int32_t input, float dt) +{ + if ((_kd != 0) && (dt != 0)) { + _derivative = (input - _last_input) / dt; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + _derivative = _last_derivative + + (dt / ( _filter + dt)) * (_derivative - _last_derivative); + + // update state + _last_input = input; + _last_derivative = _derivative; + + // add in derivative component + return _kd * _derivative; + } +} + +int32_t AC_PID::get_pi(int32_t error, float dt) +{ + return get_p(error) + get_i(error, dt); +} + + +int32_t AC_PID::get_pid(int32_t error, float dt) +{ + return get_p(error) + get_i(error, dt) + get_d(error, dt); +} + + + +/* +int32_t AC_PID::get_pid(int32_t error, float dt) +{ + // Compute proportional component + _output = error * _kp; + + // Compute derivative component if time has elapsed + if ((fabs(_kd) > 0) && (dt > 0)) { + + _derivative = (error - _last_input) / dt; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + _derivative = _last_derivative + + (dt / ( _filter + dt)) * (_derivative - _last_derivative); + + // update state + _last_input = error; + _last_derivative = _derivative; + + // add in derivative component + _output += _kd * _derivative; + } + + // Compute integral component if time has elapsed + if ((fabs(_ki) > 0) && (dt > 0)) { + _integrator += (error * _ki) * dt; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + _output += _integrator; + } + + return _output; +} +*/ + + +void +AC_PID::reset_I() +{ + _integrator = 0; + _last_input = 0; + _last_derivative = 0; +} + +void +AC_PID::load_gains() +{ + _group.load(); +} + +void +AC_PID::save_gains() +{ + _group.save(); +} diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h new file mode 100755 index 0000000000..f9b4e359de --- /dev/null +++ b/libraries/AC_PID/AC_PID.h @@ -0,0 +1,152 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AC_PID.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#ifndef AC_PID_h +#define AC_PID_h + +#include +#include // for fabs() + +/// @class AC_PID +/// @brief Object managing one PID control +class AC_PID { +public: + + /// Constructor for PID that saves its settings to EEPROM + /// + /// @note PIDs must be named to avoid either multiple parameters with the + /// same name, or an overly complex constructor. + /// + /// @param key Storage key assigned to this PID. Should be unique. + /// @param name Name by which the PID is known, or NULL for an anonymous PID. + /// The name is prefixed to the P, I, D, IMAX variable names when + /// they are reported. + /// @param initial_p Initial value for the P term. + /// @param initial_i Initial value for the I term. + /// @param initial_d Initial value for the D term. + /// @param initial_imax Initial value for the imax term.4 + /// + AC_PID(AP_Var::Key key, + const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 0.0, + const float &initial_d = 0.0, + const int16_t &initial_imax = 0.0) : + + _group(key, name), + // group, index, initial value, name + _kp (&_group, 0, initial_p, PSTR("P")), + _ki (&_group, 1, initial_i, PSTR("I")), + _kd (&_group, 2, initial_d, PSTR("D")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + // no need for explicit load, assuming that the main code uses AP_Var::load_all. + } + + /// Constructor for PID that does not save its settings. + /// + /// @param name Name by which the PID is known, or NULL for an anonymous PID. + /// The name is prefixed to the P, I, D, IMAX variable names when + /// they are reported. + /// @param initial_p Initial value for the P term. + /// @param initial_i Initial value for the I term. + /// @param initial_d Initial value for the D term. + /// @param initial_imax Initial value for the imax term.4 + /// + AC_PID(const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 0.0, + const float &initial_d = 0.0, + const int16_t &initial_imax = 0.0) : + + _group(AP_Var::k_key_none, name), + // group, index, initial value, name + _kp (&_group, 0, initial_p, PSTR("P")), + _ki (&_group, 1, initial_i, PSTR("I")), + _kd (&_group, 2, initial_d, PSTR("D")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + } + + /// Iterate the PID, return the new control value + /// + /// Positive error produces positive output. + /// + /// @param error The measured error value + /// @param dt The time delta in milliseconds (note + /// that update interval cannot be more + /// than 65.535 seconds due to limited range + /// of the data type). + /// @param scaler An arbitrary scale factor + /// + /// @returns The updated control output. + /// + int32_t get_pid(int32_t error, float dt); + int32_t get_pi(int32_t error, float dt); + int32_t get_p(int32_t error); + int32_t get_i(int32_t error, float dt); + int32_t get_d(int32_t error, float dt); + + + /// Reset the PID integrator + /// + void reset_I(); + + /// Load gain properties + /// + void load_gains(); + + /// Save gain properties + /// + void save_gains(); + + /// @name parameter accessors + //@{ + + /// Overload the function call operator to permit relatively easy initialisation + void operator() (const float p, + const float i, + const float d, + const int16_t imaxval) { + _kp = p; _ki = i; _kd = d; _imax = imaxval; + } + + float kP() const { return _kp.get(); } + float kI() const { return _ki.get(); } + float kD() const { return _kd.get(); } + int16_t imax() const { return _imax.get(); } + + void kP(const float v) { _kp.set(v); } + void kI(const float v) { _ki.set(v); } + void kD(const float v) { _kd.set(v); } + void imax(const int16_t v) { _imax.set(abs(v)); } + + float get_integrator() const { return _integrator; } + +private: + AP_Var_group _group; + AP_Float16 _kp; + AP_Float16 _ki; + AP_Float16 _kd; + AP_Int16 _imax; + + float _integrator; ///< integrator value + int32_t _last_input; ///< last input for derivative + float _last_derivative; ///< last derivative for low-pass filter + float _output; + float _derivative; + + /// Low pass filter cut frequency for derivative calculation. + /// + static const float _filter = 7.9577e-3; // Set to "1 / ( 2 * PI * f_cut )"; + // Examples for _filter: + // f_cut = 10 Hz -> _filter = 15.9155e-3 + // f_cut = 15 Hz -> _filter = 10.6103e-3 + // f_cut = 20 Hz -> _filter = 7.9577e-3 + // f_cut = 25 Hz -> _filter = 6.3662e-3 + // f_cut = 30 Hz -> _filter = 5.3052e-3 +}; + +#endif diff --git a/libraries/AC_PID/keywords.txt b/libraries/AC_PID/keywords.txt new file mode 100755 index 0000000000..68e861fd00 --- /dev/null +++ b/libraries/AC_PID/keywords.txt @@ -0,0 +1,9 @@ +PID KEYWORD1 +get_pid KEYWORD2 +reset_I KEYWORD2 +kP KEYWORD2 +kD KEYWORD2 +kI KEYWORD2 +imax KEYWORD2 +load_gains KEYWORD2 +save_gains KEYWORD2 From 0213f4dd88d4727aa045932a464482991e5bd1e0 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 28 Jan 2012 22:00:05 -0800 Subject: [PATCH 09/18] 2.2B6 - Please verify Heli still functions properly. Added AC_PID lib Updated landing code bug fixes --- ArduCopter/ArduCopter.pde | 144 ++++++++++-------- ArduCopter/Attitude.pde | 229 ++++++++++++---------------- ArduCopter/GCS_Mavlink.pde | 19 ++- ArduCopter/Log.pde | 2 +- ArduCopter/Parameters.h | 85 +++++------ ArduCopter/commands_logic.pde | 110 +++++++++----- ArduCopter/commands_process.pde | 9 +- ArduCopter/config.h | 119 +++++++-------- ArduCopter/control_modes.pde | 1 + ArduCopter/navigation.pde | 256 +++++++++++++------------------- ArduCopter/system.pde | 4 +- 11 files changed, 459 insertions(+), 519 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4aa9ecadd4..2279a72a08 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1,6 +1,6 @@ /// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#define THISFIRMWARE "ArduCopter V2.2 b4" +#define THISFIRMWARE "ArduCopter V2.2 b6" /* ArduCopter Version 2.2 Authors: Jason Short @@ -69,7 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // TimerProcess is the scheduler for MPU6000 reads. #include // ArduPilot Mega DCM Library #include // PI library -#include // PID library +#include // PID library #include // RC Channel Library #include // Range finder library #include // Optical Flow library @@ -312,11 +312,6 @@ static const char* flight_mode_strings[] = { //////////////////////////////////////////////////////////////////////////////// // The GPS based velocity calculated by offsetting the Latitude and Longitude // updated after GPS read - 5-10hz -static int16_t x_GPS_speed; -static int16_t y_GPS_speed; - -// The synthesized velocity calculated by fancy filtering and fusion -// updated at 50hz static int16_t x_actual_speed; static int16_t y_actual_speed; @@ -466,6 +461,8 @@ static uint8_t wp_verify_byte; // used for tracking state of navigating wa static int16_t waypoint_speed_gov; // Used to track how many cm we are from the "next_WP" location static int32_t long_error, lat_error; +// Are we navigating while holding a positon? This is set to false once the speed drops below 1m/s +static boolean loiter_override; //////////////////////////////////////////////////////////////////////////////// @@ -554,19 +551,16 @@ static int32_t altitude_error; static int16_t climb_rate; // The altitude as reported by Sonar in cm – Values are 20 to 700 generally. static int16_t sonar_alt; -// The previous altitude as reported by Sonar in cm for calculation of Climb Rate -static int16_t old_sonar_alt; // The climb_rate as reported by sonar in cm/s static int16_t sonar_rate; // The altitude as reported by Baro in cm – Values can be quite high static int32_t baro_alt; -// The previous altitude as reported by Baro in cm for calculation of Climb Rate -static int32_t old_baro_alt; // The climb_rate as reported by Baro in cm/s static int16_t baro_rate; // static boolean reset_throttle_flag; + //////////////////////////////////////////////////////////////////////////////// // flight modes //////////////////////////////////////////////////////////////////////////////// @@ -598,6 +592,10 @@ static int16_t manual_boost; static int16_t angle_boost; // Push copter down for clean landing static int16_t landing_boost; +// for controlling the landing throttle curve +//verifies landings +static int16_t ground_detector; + //////////////////////////////////////////////////////////////////////////////// // Navigation general @@ -823,7 +821,7 @@ void loop() uint32_t timer = micros(); // We want this to execute fast // ---------------------------- - if ((timer - fast_loopTimer) >= 5000) { + if ((timer - fast_loopTimer) >= 4500) { //PORTK |= B00010000; G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops fast_loopTimer = timer; @@ -849,7 +847,7 @@ void loop() // update our velocity estimate based on IMU at 50hz // ------------------------------------------------- - estimate_velocity(); + //estimate_velocity(); // check for new GPS messages // -------------------------- @@ -900,6 +898,12 @@ static void fast_loop() // IMU DCM Algorithm read_AHRS(); + if(takeoff_complete == false){ + // reset these I terms to prevent awkward tipping on takeoff + reset_rate_I(); + reset_stability_I(); + } + // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -1515,11 +1519,6 @@ void update_throttle_mode(void) if ((millis() - takeoff_timer) > 5000){ // we must be in the air by now takeoff_complete = true; - land_complete = false; - }else{ - // reset these I terms to prevent awkward tipping on takeoff - reset_rate_I(); - reset_stability_I(); } }else{ // we are on the ground @@ -1532,11 +1531,6 @@ void update_throttle_mode(void) // reset_baro(); } - // reset out i terms and takeoff timer - // ----------------------------------- - reset_rate_I(); - reset_stability_I(); - // remember our time since takeoff // ------------------------------- takeoff_timer = millis(); @@ -1593,12 +1587,8 @@ void update_throttle_mode(void) // how far off are we altitude_error = get_altitude_error(); - // get the AP throttle, if landing boost > 0 we are actually Landing - // This allows us to grab just the compensation. - if(landing_boost > 0) - nav_throttle = get_nav_throttle(-200); - else - nav_throttle = get_nav_throttle(altitude_error); + // get the AP throttle + nav_throttle = get_nav_throttle(altitude_error); // clear the new data flag invalid_throttle = false; @@ -1612,6 +1602,11 @@ void update_throttle_mode(void) //*/ } + // hack to remove the influence of the ground effect + if(current_loc.alt < 200 && landing_boost != 0) { + nav_throttle = min(nav_throttle, 0); + } + #if FRAME_CONFIG == HELI_FRAME throttle_out = heli_get_angle_boost(g.throttle_cruise + nav_throttle + get_z_damping() - landing_boost); #else @@ -1659,24 +1654,18 @@ static void update_navigation() case RTL: // We have reached Home if((wp_distance <= g.waypoint_radius) || check_missed_wp()){ - // if this value > 0, we are set to trigger auto_land after 30 seconds + // if auto_land_timer value > 0, we are set to trigger auto_land after 20 seconds set_mode(LOITER); auto_land_timer = millis(); break; } - // We wait until we've reached out new altitude before coming home - // Arg doesn't work, it - //if(alt_change_flag != REACHED_ALT){ - // wp_control = NO_NAV_MODE; - //}else{ - wp_control = WP_MODE; + wp_control = WP_MODE; - // calculates desired Yaw - #if FRAME_CONFIG == HELI_FRAME - update_auto_yaw(); - #endif - //} + // calculates desired Yaw + #if FRAME_CONFIG == HELI_FRAME + update_auto_yaw(); + #endif // calculates the desired Roll and Pitch update_nav_wp(); @@ -1687,7 +1676,15 @@ static void update_navigation() case POSITION: // This feature allows us to reposition the quad when the user lets // go of the sticks + if((abs(g.rc_2.control_in) + abs(g.rc_1.control_in)) > 500){ + // flag will reset when speed drops below .5 m/s + loiter_override = true; + } + + // Allow the user to take control temporarily, + // regain hold when the speed goes down to .5m/s + if(loiter_override){ // this sets the copter to not try and nav while we control it wp_control = NO_NAV_MODE; @@ -1695,8 +1692,11 @@ static void update_navigation() next_WP.lat = current_loc.lat; next_WP.lng = current_loc.lng; + if(g_gps->ground_speed < 50){ + loiter_override = false; + wp_control = LOITER_MODE; + } }else{ - // this is also set by GPS in update_nav wp_control = LOITER_MODE; } @@ -1712,7 +1712,10 @@ static void update_navigation() break; case LAND: - verify_land(); + if(g.sonar_enabled) + verify_land_sonar(); + else + verify_land_baro(); // calculates the desired Roll and Pitch update_nav_wp(); @@ -1795,6 +1798,10 @@ static void update_trig(void){ // updated at 10hz static void update_altitude() { + static int16_t old_sonar_alt = 0; + static int32_t old_baro_alt = 0; + static int16_t old_climb_rate = 0; + #if HIL_MODE == HIL_MODE_ATTITUDE // we are in the SIM, fake out the baro and Sonar int fake_relative_alt = g_gps->altitude - gps_base_alt; @@ -1815,7 +1822,7 @@ static void update_altitude() baro_rate = (temp + baro_rate) >> 1; old_baro_alt = baro_alt; - // sonar_alt is calculated in a faster loop and filtered with a mode filter + // Note: sonar_alt is calculated in a faster loop and filtered with a mode filter #endif @@ -1865,9 +1872,15 @@ static void update_altitude() climb_rate = baro_rate; } + // simple smoothing + climb_rate = (climb_rate + old_climb_rate)>>1; + // manage bad data climb_rate = constrain(climb_rate, -300, 300); + // save for filtering + old_climb_rate = climb_rate; + // update the target altitude next_WP.alt = get_new_altitude(); } @@ -1896,14 +1909,18 @@ static void tuning(){ switch(g.radio_tuning){ case CH6_DAMP: - g.rc_6.set_range(0,80); // 0 to 1 + g.rc_6.set_range(0,300); // 0 to 1 g.stablize_d.set(tuning_value); + + //g.rc_6.set_range(0,60); // 0 to 1 + //g.pid_rate_roll.kD(tuning_value); + //g.pid_rate_pitch.kD(tuning_value); break; case CH6_STABILIZE_KP: g.rc_6.set_range(0,8000); // 0 to 8 - g.pi_stabilize_roll.kP(tuning_value); - g.pi_stabilize_pitch.kP(tuning_value); + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); break; case CH6_STABILIZE_KI: @@ -1915,16 +1932,14 @@ static void tuning(){ case CH6_RATE_KP: g.rc_6.set_range(40,300); // 0 to .3 - g.pi_rate_roll.kP(tuning_value); - g.pi_rate_pitch.kP(tuning_value); - g.pi_acro_roll.kP(tuning_value); - g.pi_acro_pitch.kP(tuning_value); + g.pid_rate_roll.kP(tuning_value); + g.pid_rate_pitch.kP(tuning_value); break; case CH6_RATE_KI: g.rc_6.set_range(0,300); // 0 to .3 - g.pi_rate_roll.kI(tuning_value); - g.pi_rate_pitch.kI(tuning_value); + g.pid_rate_roll.kI(tuning_value); + g.pid_rate_pitch.kI(tuning_value); break; case CH6_YAW_KP: @@ -1934,12 +1949,12 @@ static void tuning(){ case CH6_YAW_RATE_KP: g.rc_6.set_range(0,1000); - g.pi_rate_yaw.kP(tuning_value); + g.pid_rate_yaw.kP(tuning_value); break; case CH6_THROTTLE_KP: g.rc_6.set_range(0,1000); // 0 to 1 - g.pi_throttle.kP(tuning_value); + g.pid_throttle.kP(tuning_value); break; case CH6_TOP_BOTTOM_RATIO: @@ -1966,8 +1981,8 @@ static void tuning(){ case CH6_NAV_P: g.rc_6.set_range(0,6000); - g.pi_nav_lat.kP(tuning_value); - g.pi_nav_lon.kP(tuning_value); + g.pid_nav_lat.kP(tuning_value); + g.pid_nav_lon.kP(tuning_value); break; #if FRAME_CONFIG == HELI_FRAME @@ -1984,20 +1999,20 @@ static void tuning(){ case CH6_OPTFLOW_KP: g.rc_6.set_range(0,5000); // 0 to 5 - g.pi_optflow_roll.kP(tuning_value); - g.pi_optflow_pitch.kP(tuning_value); + g.pid_optflow_roll.kP(tuning_value); + g.pid_optflow_pitch.kP(tuning_value); break; case CH6_OPTFLOW_KI: g.rc_6.set_range(0,10000); // 0 to 10 - g.pi_optflow_roll.kI(tuning_value); - g.pi_optflow_pitch.kI(tuning_value); + g.pid_optflow_roll.kI(tuning_value); + g.pid_optflow_pitch.kI(tuning_value); break; case CH6_OPTFLOW_KD: g.rc_6.set_range(0,200); // 0 to 0.2 - g.pi_optflow_roll.kD(tuning_value); - g.pi_optflow_pitch.kD(tuning_value); + g.pid_optflow_roll.kD(tuning_value); + g.pid_optflow_pitch.kD(tuning_value); break; } } @@ -2075,8 +2090,9 @@ static void update_nav_wp() }else if(wp_control == NO_NAV_MODE){ // clear out our nav so we can do things like land straight down + // or change Loiter position - // We bring in our iterms for wind control, but we don't navigate + // We bring copy over our Iterms for wind control, but we don't navigate nav_lon = g.pi_loiter_lon.get_integrator(); nav_lat = g.pi_loiter_lat.get_integrator(); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 0851d90333..d7c1c75c70 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -3,134 +3,131 @@ static int get_stabilize_roll(int32_t target_angle) { - int32_t error = 0; - int32_t rate = 0; - - static float current_rate = 0; - // angle error - error = wrap_180(target_angle - dcm.roll_sensor); + target_angle = wrap_180(target_angle - dcm.roll_sensor); #if FRAME_CONFIG == HELI_FRAME + // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); + target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: - rate = g.pi_stabilize_roll.get_pi(error, G_Dt); + target_angle = g.pi_stabilize_roll.get_pi(target_angle, G_Dt); // output control: - rate = constrain(rate, -4500, 4500); - return (int)rate; + return constrain(target_angle, -4500, 4500); #else + // limit the error we're feeding to the PID - error = constrain(error, -2500, 2500); + target_angle = constrain(target_angle, -2500, 2500); // conver to desired Rate: - rate = g.pi_stabilize_roll.get_p(error); + int32_t target_rate = g.pi_stabilize_roll.get_p(target_angle); + int16_t iterm = g.pi_stabilize_roll.get_i(target_angle, G_Dt); - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_roll.get_i(error, G_Dt); - - // rate control - error = rate - (omega.x * DEGX100); - rate = g.pi_rate_roll.get_pi(error, G_Dt); - - // D term - current_rate = (current_rate *.7) + (omega.x * DEGX100) * .3; - int16_t d_temp = current_rate * g.stablize_d; - rate -= d_temp; - - // output control: - rate = constrain(rate, -2500, 2500); - return (int)rate + iterm; + return get_rate_roll(target_rate) + iterm; #endif } static int get_stabilize_pitch(int32_t target_angle) { - int32_t error = 0; - int32_t rate = 0; - static float current_rate = 0; - // angle error - error = wrap_180(target_angle - dcm.pitch_sensor); + target_angle = wrap_180(target_angle - dcm.pitch_sensor); #if FRAME_CONFIG == HELI_FRAME // limit the error we're feeding to the PID - error = constrain(error, -4500, 4500); + target_angle = constrain(target_angle, -4500, 4500); // convert to desired Rate: - rate = g.pi_stabilize_pitch.get_pi(error, G_Dt); + target_angle = g.pi_stabilize_pitch.get_pi(target_angle, G_Dt); // output control: - rate = constrain(rate, -4500, 4500); - return (int)rate; + return constrain(target_angle, -4500, 4500); #else - // angle error - error = constrain(error, -2500, 2500); + // limit the error we're feeding to the PID + target_angle = constrain(target_angle, -2500, 2500); // conver to desired Rate: - rate = g.pi_stabilize_pitch.get_p(error); + int32_t target_rate = g.pi_stabilize_pitch.get_p(target_angle); + int16_t iterm = g.pi_stabilize_pitch.get_i(target_angle, G_Dt); - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_pitch.get_i(error, G_Dt); - - // rate control - error = rate - (omega.y * DEGX100); - - //error = rate - (omega.y * DEGX100); - rate = g.pi_rate_pitch.get_pi(error, G_Dt); - - // D term - current_rate = (current_rate *.7) + (omega.y * DEGX100) * .3; - int16_t d_temp = current_rate * g.stablize_d; - rate -= d_temp; - - // output control: - rate = constrain(rate, -2500, 2500); - return (int)rate + iterm; + return get_rate_pitch(target_rate) + iterm; #endif } - -#define YAW_ERROR_MAX 2000 static int get_stabilize_yaw(int32_t target_angle) { - int32_t error; - int32_t rate; - // angle error - error = wrap_180(target_angle - dcm.yaw_sensor); + target_angle = wrap_180(target_angle - dcm.yaw_sensor); // limit the error we're feeding to the PID - error = constrain(error, -YAW_ERROR_MAX, YAW_ERROR_MAX); + target_angle = constrain(target_angle, -2000, 2000); - // convert to desired Rate: - rate = g.pi_stabilize_yaw.get_p(error); - - // experiment to pipe iterm directly into the output - int16_t iterm = g.pi_stabilize_yaw.get_i(error, G_Dt); + // conver to desired Rate: + int32_t target_rate = g.pi_stabilize_yaw.get_p(target_angle); + int16_t iterm = g.pi_stabilize_yaw.get_i(target_angle, G_Dt); #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters - if( !g.heli_ext_gyro_enabled ) { - error = rate - (omega.z * DEGX100); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); + if(!g.heli_ext_gyro_enabled){ + return get_rate_yaw(target_rate) + iterm; + }else{ + return constrain((target_rate + iterm), -4500, 4500); } - // output control: - rate = constrain(rate, -4500, 4500); #else - error = rate - (omega.z * DEGX100); - rate = g.pi_rate_yaw.get_pi(error, G_Dt); + return get_rate_yaw(target_rate) + iterm; +#endif +} + +static int +get_rate_roll(int32_t target_rate) +{ + static int32_t last_rate = 0; + int32_t current_rate = (omega.x * DEGX100); + + // rate control + target_rate = target_rate - current_rate; + target_rate = g.pid_rate_roll.get_pid(target_rate, G_Dt); + + // Dampening + target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500); + last_rate = current_rate; // output control: - int16_t yaw_input = 1400 + abs(g.rc_4.control_in); - // smoother Yaw control: - rate = constrain(rate, -yaw_input, yaw_input); -#endif + return constrain(target_rate, -2500, 2500); +} - return (int)rate + iterm; +static int +get_rate_pitch(int32_t target_rate) +{ + static int32_t last_rate = 0; + int32_t current_rate = (omega.y * DEGX100); + + // rate control + target_rate = target_rate - current_rate; + target_rate = g.pid_rate_pitch.get_pid(target_rate, G_Dt); + + // Dampening + target_rate -= constrain((current_rate - last_rate) * g.stablize_d, -500, 500); + last_rate = current_rate; + + // output control: + return constrain(target_rate, -2500, 2500); +} + +static int +get_rate_yaw(int32_t target_rate) +{ + // rate control + target_rate = target_rate - (omega.z * DEGX100); + target_rate = g.pid_rate_yaw.get_pid(target_rate, G_Dt); + + // output control: + int16_t yaw_limit = 1400 + abs(g.rc_4.control_in); + + // smoother Yaw control: + return constrain(target_rate, -yaw_limit, yaw_limit); } #define ALT_ERROR_MAX 400 @@ -138,10 +135,8 @@ static int16_t get_nav_throttle(int32_t z_error) { static int16_t old_output = 0; - //static int16_t rate_d = 0; - - int16_t rate_error; - int16_t output; + int16_t rate_error = 0; + int16_t output = 0; // limit error to prevent I term run up z_error = constrain(z_error, -ALT_ERROR_MAX, ALT_ERROR_MAX); @@ -155,17 +150,10 @@ get_nav_throttle(int32_t z_error) // calculate rate error rate_error = rate_error - climb_rate; - // limit the rate - iterm is not used - output = constrain((int)g.pi_throttle.get_p(rate_error), -160, 180); - - // a positive climb rate means we're going up - //rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain - - // slight adjustment to alt hold output - //output -= constrain(rate_d, -25, 25); + // limit the rate + output = constrain(g.pid_throttle.get_pid(rate_error, .1), -160, 180); // light filter of output - //output = (old_output * 3 + output) / 4; output = (old_output + output) / 2; // save our output @@ -175,33 +163,6 @@ get_nav_throttle(int32_t z_error) return output + iterm; } -static int -get_rate_roll(int32_t target_rate) -{ - int32_t error = (target_rate * 3.5) - (omega.x * DEGX100); - error = constrain(error, -20000, 20000); - return g.pi_acro_roll.get_pi(error, G_Dt); -} - -static int -get_rate_pitch(int32_t target_rate) -{ - int32_t error = (target_rate * 3.5) - (omega.y * DEGX100); - error = constrain(error, -20000, 20000); - return g.pi_acro_pitch.get_pi(error, G_Dt); -} - -static int -get_rate_yaw(int32_t target_rate) -{ - - int32_t error = (target_rate * 4.5) - (omega.z * DEGX100); - target_rate = g.pi_rate_yaw.get_pi(error, G_Dt); - - // output control: - return (int)constrain(target_rate, -2500, 2500); -} - // Keeps old data out of our calculation / logs static void reset_nav_params(void) { @@ -247,17 +208,15 @@ static void reset_I_all(void) static void reset_rate_I() { - g.pi_rate_roll.reset_I(); - g.pi_rate_pitch.reset_I(); - g.pi_acro_roll.reset_I(); - g.pi_acro_pitch.reset_I(); - g.pi_rate_yaw.reset_I(); + g.pid_rate_roll.reset_I(); + g.pid_rate_pitch.reset_I(); + g.pid_rate_yaw.reset_I(); } static void reset_optflow_I(void) { - g.pi_optflow_roll.reset_I(); - g.pi_optflow_pitch.reset_I(); + g.pid_optflow_roll.reset_I(); + g.pid_optflow_pitch.reset_I(); of_roll = 0; of_pitch = 0; } @@ -272,15 +231,15 @@ static void reset_wind_I(void) static void reset_nav_I(void) { // Rate control for WP navigation - g.pi_nav_lat.reset_I(); - g.pi_nav_lon.reset_I(); + g.pid_nav_lat.reset_I(); + g.pid_nav_lon.reset_I(); } static void reset_throttle_I(void) { // For Altitude Hold g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); + g.pid_throttle.reset_I(); } static void reset_stability_I(void) @@ -325,7 +284,7 @@ static int get_angle_boost(int value) float temp = cos_pitch_x * cos_roll_x; temp = 1.0 - constrain(temp, .5, 1.0); int16_t output = temp * value; - return constrain(output, 0, 100); + return constrain(output, 0, 200); // return (int)(temp * value); } @@ -482,9 +441,9 @@ get_of_roll(int32_t control_roll) // only stop roll if caller isn't modifying roll if( control_roll == 0 && current_loc.alt < 1500) { - new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + //new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ - g.pi_optflow_roll.reset_I(); + g.pid_optflow_roll.reset_I(); tot_x_cm = 0; } // limit amount of change and maximum angle @@ -516,10 +475,10 @@ get_of_pitch(int32_t control_pitch) // only stop roll if caller isn't modifying pitch if( control_pitch == 0 && current_loc.alt < 1500 ) { - new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + //new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ tot_y_cm = 0; - g.pi_optflow_pitch.reset_I(); + g.pid_optflow_pitch.reset_I(); } // limit amount of change diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 44e74f21bf..355038a9c2 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -824,7 +824,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_ACTION_EMCY_LAND: - //set_mode(LAND); + set_mode(LAND); break; case MAV_ACTION_HALT: @@ -1529,6 +1529,23 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) imu.set_gyro(gyros); */ //imu.set_accel(accels); + + // rad/sec // JLN update - FOR HIL SIMULATION WITH AEROSIM + Vector3f gyros; + gyros.x = (float)packet.rollspeed / 1000.0; + gyros.y = (float)packet.pitchspeed / 1000.0; + gyros.z = (float)packet.yawspeed / 1000.0; + + imu.set_gyro(gyros); + + // m/s/s + Vector3f accels; + accels.x = (float)packet.roll * gravity / 1000.0; + accels.y = (float)packet.pitch * gravity / 1000.0; + accels.z = (float)packet.yaw * gravity / 1000.0; + + imu.set_accel(accels); + break; } #endif diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 090e2b3501..9b1511c07e 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -590,7 +590,7 @@ static void Log_Write_Control_Tuning() DataFlash.WriteInt(climb_rate); // 10 DataFlash.WriteInt(g.rc_3.servo_out); // 11 DataFlash.WriteInt(g.pi_alt_hold.get_integrator()); // 12 - DataFlash.WriteInt(g.pi_throttle.get_integrator()); // 13 + DataFlash.WriteInt(g.pid_throttle.get_integrator()); // 13 DataFlash.WriteByte(END_BYTE); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4c061a6026..0dc92f2d19 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -17,7 +17,7 @@ public: // The increment will prevent old parameters from being used incorrectly // by newer code. // - static const uint16_t k_format_version = 114; + static const uint16_t k_format_version = 115; // The parameter software_type is set up solely for ground station use // and identifies the software type (eg ArduPilotMega versus ArduCopterMega) @@ -26,7 +26,6 @@ public: // static const uint16_t k_software_type = 10; // 0 for APM trunk - // // Parameter identities. // // The enumeration defined here is used to ensure that every parameter @@ -168,23 +167,20 @@ public: // 235: PI/D Controllers // k_param_stabilize_d = 234, - k_param_pi_rate_roll = 235, - k_param_pi_rate_pitch, - k_param_pi_rate_yaw, + k_param_pid_rate_roll = 235, + k_param_pid_rate_pitch, + k_param_pid_rate_yaw, k_param_pi_stabilize_roll, k_param_pi_stabilize_pitch, k_param_pi_stabilize_yaw, k_param_pi_loiter_lat, k_param_pi_loiter_lon, - k_param_pi_nav_lat, - k_param_pi_nav_lon, + k_param_pid_nav_lat, + k_param_pid_nav_lon, k_param_pi_alt_hold, - k_param_pi_throttle, - k_param_pi_acro_roll, - k_param_pi_acro_pitch, - k_param_pi_optflow_roll, - k_param_pi_optflow_pitch, // 250 - k_param_loiter_d, + k_param_pid_throttle, + k_param_pid_optflow_roll, + k_param_pid_optflow_pitch, // 250 // 254,255: reserved }; @@ -286,31 +282,24 @@ public: AP_Float camera_pitch_gain; AP_Float camera_roll_gain; AP_Float stablize_d; - AP_Float loiter_d; // PI/D controllers - APM_PI pi_rate_roll; - APM_PI pi_rate_pitch; - APM_PI pi_rate_yaw; + AC_PID pid_rate_roll; + AC_PID pid_rate_pitch; + AC_PID pid_rate_yaw; + AC_PID pid_nav_lat; + AC_PID pid_nav_lon; - APM_PI pi_stabilize_roll; - APM_PI pi_stabilize_pitch; - APM_PI pi_stabilize_yaw; + AC_PID pid_throttle; + AC_PID pid_optflow_roll; + AC_PID pid_optflow_pitch; APM_PI pi_loiter_lat; APM_PI pi_loiter_lon; - - APM_PI pi_nav_lat; - APM_PI pi_nav_lon; - + APM_PI pi_stabilize_roll; + APM_PI pi_stabilize_pitch; + APM_PI pi_stabilize_yaw; APM_PI pi_alt_hold; - APM_PI pi_throttle; - - APM_PI pi_acro_roll; - APM_PI pi_acro_pitch; - - PID pi_optflow_roll; - PID pi_optflow_pitch; uint8_t junk; @@ -369,7 +358,7 @@ public: radio_tuning (0, k_param_radio_tuning, PSTR("TUNE")), frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")), top_bottom_ratio (TOP_BOTTOM_RATIO, k_param_top_bottom_ratio, PSTR("TB_RATIO")), - ch7_option (CH7_SAVE_WP, k_param_ch7_option, PSTR("CH7_OPT")), + ch7_option (CH7_OPTION, k_param_ch7_option, PSTR("CH7_OPT")), #if FRAME_CONFIG == HELI_FRAME heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")), @@ -409,34 +398,32 @@ public: //------------------------------------------------------------------------------------------------------------------- camera_pitch_gain (CAM_PITCH_GAIN, k_param_camera_pitch_gain, PSTR("CAM_P_G")), camera_roll_gain (CAM_ROLL_GAIN, k_param_camera_roll_gain, PSTR("CAM_R_G")), - stablize_d (STABILIZE_D, k_param_stabilize_d, PSTR("STAB_D")), - loiter_d (LOITER_D, k_param_loiter_d, PSTR("LOITER_D")), + + // PID controller group key name initial P initial I initial D initial imax + //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- + pid_rate_roll (k_param_pid_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX * 100), + pid_rate_pitch (k_param_pid_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX * 100), + pid_rate_yaw (k_param_pid_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX * 100), + + pid_nav_lat (k_param_pid_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), + pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), + + pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), + pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_IMAX * 100), + pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100), + // PI controller group key name initial P initial I initial imax //-------------------------------------------------------------------------------------------------------------------------------------------------------------------- - pi_rate_roll (k_param_pi_rate_roll, PSTR("RATE_RLL_"), RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_IMAX * 100), - pi_rate_pitch (k_param_pi_rate_pitch, PSTR("RATE_PIT_"), RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_IMAX * 100), - pi_rate_yaw (k_param_pi_rate_yaw, PSTR("RATE_YAW_"), RATE_YAW_P, RATE_YAW_I, RATE_YAW_IMAX * 100), - pi_stabilize_roll (k_param_pi_stabilize_roll, PSTR("STB_RLL_"), STABILIZE_ROLL_P, STABILIZE_ROLL_I, STABILIZE_ROLL_IMAX * 100), pi_stabilize_pitch (k_param_pi_stabilize_pitch, PSTR("STB_PIT_"), STABILIZE_PITCH_P, STABILIZE_PITCH_I, STABILIZE_PITCH_IMAX * 100), pi_stabilize_yaw (k_param_pi_stabilize_yaw, PSTR("STB_YAW_"), STABILIZE_YAW_P, STABILIZE_YAW_I, STABILIZE_YAW_IMAX * 100), + pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), ALT_HOLD_P, ALT_HOLD_I, ALT_HOLD_IMAX), pi_loiter_lat (k_param_pi_loiter_lat, PSTR("HLD_LAT_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), pi_loiter_lon (k_param_pi_loiter_lon, PSTR("HLD_LON_"), LOITER_P, LOITER_I, LOITER_IMAX * 100), - pi_nav_lat (k_param_pi_nav_lat, PSTR("NAV_LAT_"), NAV_P, NAV_I, NAV_IMAX * 100), - pi_nav_lon (k_param_pi_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_IMAX * 100), - - pi_alt_hold (k_param_pi_alt_hold, PSTR("THR_ALT_"), THR_HOLD_P, THR_HOLD_I, THR_HOLD_IMAX), - pi_throttle (k_param_pi_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_IMAX), - - pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100), - pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100), - - pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), - pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100), junk(0) // XXX just so that we can add things without worrying about the trailing comma { diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index d8bd2bbd36..d491201529 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -16,6 +16,7 @@ static void process_nav_command() break; case MAV_CMD_NAV_LAND: // 21 LAND to Waypoint + yaw_mode = YAW_HOLD; do_land(); break; @@ -121,8 +122,6 @@ static void process_now_command() static bool verify_must() { - //Serial.printf("vmust: %d\n", command_nav_ID); - switch(command_nav_queue.id) { case MAV_CMD_NAV_TAKEOFF: @@ -130,7 +129,11 @@ static bool verify_must() break; case MAV_CMD_NAV_LAND: - return verify_land(); + if(g.sonar_enabled == true){ + return verify_land_sonar(); + }else{ + return verify_land_baro(); + } break; case MAV_CMD_NAV_WAYPOINT: @@ -265,13 +268,21 @@ static void do_land() { wp_control = LOITER_MODE; + // just to make sure land_complete = false; + // landing boost lowers the main throttle to mimmick + // the effect of a user's hand + landing_boost = 0; + + // A counter that goes up if our climb rate stalls out. + ground_detector = 0; + // hold at our current location set_next_WP(¤t_loc); - // Set a new target altitude - set_new_altitude(-200); + // Set a new target altitude very low, incase we are landing on a hill! + set_new_altitude(-1000); } static void do_loiter_unlimited() @@ -341,46 +352,71 @@ static bool verify_takeoff() } // called at 10hz -static bool verify_land() +static bool verify_land_sonar() { - static int16_t velocity_land = -1; - static float icount = 0; + static float icount = 1; - // landing detector - if (current_loc.alt > 300){ - velocity_land = 2000; + if(current_loc.alt > 300){ + wp_control = LOITER_MODE; icount = 1; - + ground_detector = 0; }else{ - // a LP filter used to tell if we have landed - // will drive to 0 if we are on the ground - maybe, the baro is noisy - velocity_land = ((velocity_land * 7) + climb_rate ) / 8; + // begin to pull down on the throttle + landing_boost++; } - if ((current_loc.alt - home.alt) < 200){ - // don't bank to hold position - wp_control = NO_NAV_MODE; - // try and come down faster - //landing_boost++; - //landing_boost = min(landing_boost, 15); - float tmp = (1.75 * icount * icount) - (7.2 * icount); - landing_boost = tmp; - landing_boost = constrain(landing_boost, 1, 200); - icount += 0.4; - - //Serial.printf("lb:%d, %1.4f, ic:%1.4f\n",landing_boost, tmp, icount); - - }else{ - landing_boost = 0; - wp_control = LOITER_MODE; + if(current_loc.alt < 200 ){ + wp_control = NO_NAV_MODE; } - //if(/*(current_loc.alt < 100) && */ (velocity_land < 20)){ - if((landing_boost > 150) || (velocity_land < 20)){ - icount = 1; - land_complete = true; - landing_boost = 0; - return true; + if(current_loc.alt < 150 ){ + //rapid throttle reduction + int16_t lb = (1.75 * icount * icount) - (7.2 * icount); + icount++; + lb = constrain(lb, 0, 180); + landing_boost += lb; + //Serial.printf("%0.0f, %d, %d, %d\n", icount, current_loc.alt, landing_boost, lb); + + if(current_loc.alt < 40 || abs(climb_rate) < 20) { + if(ground_detector++ > 20) { + land_complete = true; + ground_detector = 0; + icount = 1; + // init disarm motors + init_disarm_motors(); + return true; + } + } + } + return false; +} + +static bool verify_land_baro() +{ + if(current_loc.alt > 300){ + wp_control = LOITER_MODE; + ground_detector = 0; + }else{ + // begin to pull down on the throttle + landing_boost++; + landing_boost = min(landing_boost, 40); + } + if(current_loc.alt < 200 ){ + wp_control = NO_NAV_MODE; + } + + if(current_loc.alt < 150 ){ + if(abs(climb_rate) < 20) { + if(ground_detector++ > 30) { + //landing_boost = 100; + //Serial.print("land_complete\n"); + land_complete = true; + ground_detector = 0; + // init disarm motors + init_disarm_motors(); + return true; + } + } } return false; } diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 929c5c35b6..4c3b94eda7 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -62,14 +62,7 @@ static void update_commands() g.command_index = command_nav_index = 255; // if we are on the ground, enter stabilize, else Land if (land_complete == true){ - // So what state does this leave us in? - // We are still in the same mode as what landed us, - // so maybe we try to continue to descend just in case we are still in the air - // This will also drive down the Iterm to -300 - - // We can't disarm the motors easily. We could very well be wrong - // - //init_disarm_motors(); + // we will disarm the motors after landing. } else { set_mode(LAND); } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 4d07062990..2fc2e3e66e 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -525,114 +525,98 @@ #ifdef MOTORS_JD880 # define STABILIZE_ROLL_P 3.6 # define STABILIZE_ROLL_I 0.0 -# define STABILIZE_ROLL_IMAX 40.0 // degrees +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 3.6 # define STABILIZE_PITCH_I 0.0 -# define STABILIZE_PITCH_IMAX 40.0 // degrees +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif #ifdef MOTORS_JD850 # define STABILIZE_ROLL_P 4.0 # define STABILIZE_ROLL_I 0.0 -# define STABILIZE_ROLL_IMAX 40.0 // degrees +# define STABILIZE_ROLL_IMAX 40.0 // degrees # define STABILIZE_PITCH_P 4.0 # define STABILIZE_PITCH_I 0.0 -# define STABILIZE_PITCH_IMAX 40.0 // degrees +# define STABILIZE_PITCH_IMAX 40.0 // degrees #endif #ifndef STABILIZE_D -# define STABILIZE_D .03 +# define STABILIZE_D .12 #endif // Jasons default values that are good for smaller payload motors. #ifndef STABILIZE_ROLL_P -# define STABILIZE_ROLL_P 4.6 +# define STABILIZE_ROLL_P 4.5 #endif #ifndef STABILIZE_ROLL_I -# define STABILIZE_ROLL_I 0.1 +# define STABILIZE_ROLL_I 0.0 #endif #ifndef STABILIZE_ROLL_IMAX # define STABILIZE_ROLL_IMAX 40 // degrees #endif #ifndef STABILIZE_PITCH_P -# define STABILIZE_PITCH_P 4.6 +# define STABILIZE_PITCH_P 4.5 #endif #ifndef STABILIZE_PITCH_I -# define STABILIZE_PITCH_I 0.1 +# define STABILIZE_PITCH_I 0.0 #endif #ifndef STABILIZE_PITCH_IMAX # define STABILIZE_PITCH_IMAX 40 // degrees #endif -////////////////////////////////////////////////////////////////////////////// -// Acro Rate Control -// -#ifndef ACRO_ROLL_P -# define ACRO_ROLL_P 0.155 +#ifndef STABILIZE_YAW_P +# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy #endif -#ifndef ACRO_ROLL_I -# define ACRO_ROLL_I 0.0 +#ifndef STABILIZE_YAW_I +# define STABILIZE_YAW_I 0.01 #endif -#ifndef ACRO_ROLL_IMAX -# define ACRO_ROLL_IMAX 15 // degrees +#ifndef STABILIZE_YAW_IMAX +# define STABILIZE_YAW_IMAX 8 // degrees * 100 #endif -#ifndef ACRO_PITCH_P -# define ACRO_PITCH_P 0.155 -#endif -#ifndef ACRO_PITCH_I -# define ACRO_PITCH_I 0 //0.18 -#endif -#ifndef ACRO_PITCH_IMAX -# define ACRO_PITCH_IMAX 15 // degrees -#endif ////////////////////////////////////////////////////////////////////////////// // Stabilize Rate Control // #ifndef RATE_ROLL_P -# define RATE_ROLL_P 0.155 +# define RATE_ROLL_P 0.14 #endif #ifndef RATE_ROLL_I -# define RATE_ROLL_I 0.0 +# define RATE_ROLL_I 0.0 +#endif +#ifndef RATE_ROLL_D +# define RATE_ROLL_D 0 #endif #ifndef RATE_ROLL_IMAX # define RATE_ROLL_IMAX 15 // degrees #endif #ifndef RATE_PITCH_P -# define RATE_PITCH_P 0.155 +# define RATE_PITCH_P 0.14 #endif #ifndef RATE_PITCH_I -# define RATE_PITCH_I 0 //0.18 +# define RATE_PITCH_I 0 // 0.18 +#endif +#ifndef RATE_PITCH_D +# define RATE_PITCH_D 0 // 0.002 #endif #ifndef RATE_PITCH_IMAX # define RATE_PITCH_IMAX 15 // degrees #endif -////////////////////////////////////////////////////////////////////////////// -// YAW Control -// -#ifndef STABILIZE_YAW_P -# define STABILIZE_YAW_P 7.5 // increase for more aggressive Yaw Hold, decrease if it's bouncy -#endif -#ifndef STABILIZE_YAW_I -# define STABILIZE_YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance -#endif -#ifndef STABILIZE_YAW_IMAX -# define STABILIZE_YAW_IMAX 8 // degrees * 100 -#endif - #ifndef RATE_YAW_P -# define RATE_YAW_P .13 // used to control response in turning +# define RATE_YAW_P .13 #endif #ifndef RATE_YAW_I -# define RATE_YAW_I 0.0 +# define RATE_YAW_I 0.0 +#endif +#ifndef RATE_YAW_D +# define RATE_YAW_D .002 #endif #ifndef RATE_YAW_IMAX -# define RATE_YAW_IMAX 50 +# define RATE_YAW_IMAX 50 // degrees #endif @@ -640,37 +624,37 @@ // Loiter control gains // #ifndef LOITER_P -# define LOITER_P 1.5 // was .25 in previous +# define LOITER_P .4 // was .25 in previous #endif #ifndef LOITER_I -# define LOITER_I 0.09 // Wind control +# define LOITER_I 0.2 // Wind control #endif #ifndef LOITER_IMAX -# define LOITER_IMAX 30 // degrees° -#endif -#ifndef LOITER_D -# define LOITER_D 2.2 // rate control +# define LOITER_IMAX 30 // degrees #endif ////////////////////////////////////////////////////////////////////////////// // WP Navigation control gains // #ifndef NAV_P -# define NAV_P 3.0 // 3 was causing rapid oscillations in Loiter +# define NAV_P 2.3 // 3 was causing rapid oscillations in Loiter #endif #ifndef NAV_I # define NAV_I 0 // #endif +#ifndef NAV_D +# define NAV_D 0.015 // +#endif #ifndef NAV_IMAX # define NAV_IMAX 30 // degrees #endif #ifndef WAYPOINT_SPEED_MAX -# define WAYPOINT_SPEED_MAX 600 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MAX 600 // 6m/s error = 13mph #endif #ifndef WAYPOINT_SPEED_MIN -# define WAYPOINT_SPEED_MIN 100 // for 6m/s error = 13mph +# define WAYPOINT_SPEED_MIN 100 // 1m/s #endif @@ -678,28 +662,31 @@ // Throttle control gains // #ifndef THROTTLE_CRUISE -# define THROTTLE_CRUISE 350 // +# define THROTTLE_CRUISE 350 // #endif -#ifndef THR_HOLD_P -# define THR_HOLD_P 0.4 // +#ifndef ALT_HOLD_P +# define ALT_HOLD_P 0.4 // #endif -#ifndef THR_HOLD_I -# define THR_HOLD_I 0.01 // with 4m error, 12.5s windup +#ifndef ALT_HOLD_I +# define ALT_HOLD_I 0.02 #endif -#ifndef THR_HOLD_IMAX -# define THR_HOLD_IMAX 300 +#ifndef ALT_HOLD_IMAX +# define ALT_HOLD_IMAX 300 #endif // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.5 // +# define THROTTLE_P 0.5 // #endif #ifndef THROTTLE_I -# define THROTTLE_I 0.0 // +# define THROTTLE_I 0.0 // +#endif +#ifndef THROTTLE_D +# define THROTTLE_D 0.02 // #endif #ifndef THROTTLE_IMAX -# define THROTTLE_IMAX 300 +# define THROTTLE_IMAX 300 #endif diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 8911fb0962..87965160eb 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -114,6 +114,7 @@ static void read_trim_switch() // reset the mission CH7_wp_index = 0; g.command_total.set_and_save(1); + set_mode(RTL); return; } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index a8795de1eb..a0eb0e00d7 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -7,13 +7,11 @@ static byte navigate() { // waypoint distance from plane in meters // --------------------------------------- - wp_distance = get_distance(¤t_loc, &next_WP); - home_distance = get_distance(¤t_loc, &home); + wp_distance = get_distance(¤t_loc, &next_WP); + home_distance = get_distance(¤t_loc, &home); if (wp_distance < 0){ - //gcs_send_text_P(SEVERITY_HIGH,PSTR(" WP error - distance < 0")); - //Serial.println(wp_distance,DEC); - //print_current_waypoints(); + // something went very wrong return 0; } @@ -61,9 +59,8 @@ static void calc_XY_velocity(){ int16_t y_estimate = (float)(g_gps->latitude - last_latitude) * tmp; // slight averaging filter - x_GPS_speed = (x_GPS_speed + x_estimate) >> 1; - y_GPS_speed = (y_GPS_speed + y_estimate) >> 1; - //*/ + x_actual_speed = (x_actual_speed + x_estimate) >> 1; + y_actual_speed = (y_actual_speed + y_estimate) >> 1; /* // Ryan Beall's forward estimator: @@ -99,67 +96,7 @@ static void calc_location_error(struct Location *next_loc) lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North } -/* -//static void calc_loiter3(int x_error, int y_error) -{ - static int32_t gps_lat_I = 0; - static int32_t gps_lon_I = 0; - - // If close to goal <1m reset the I term - if (abs(x_error) < 50) - gps_lon_I = 0; - if (abs(y_error) < 50) - gps_lat_I = 0; - - gps_lon_I += x_error; - gps_lat_I += y_error; - - gps_lon_I = constrain(gps_lon_I,-3000,3000); - gps_lat_I = constrain(gps_lat_I,-3000,3000); - - int16_t lon_P = 1.2 * (float)x_error; - int16_t lon_I = 0.1 * (float)gps_lon_I; //.1 - int16_t lon_D = 3 * x_GPS_speed ; // this controls the small bumps - - int16_t lat_P = 1.2 * (float)y_error; - int16_t lat_I = 0.1 * (float)gps_lat_I; - int16_t lat_D = 3 * y_GPS_speed ; - - //limit of terms - lon_I = constrain(lon_I,-3000,3000); - lat_I = constrain(lat_I,-3000,3000); - lon_D = constrain(lon_D,-500,500); //this controls the long distance dampimg - lat_D = constrain(lat_D,-500,500); //this controls the long distance dampimg - - nav_lon = lon_P + lon_I - lon_D; - nav_lat = lat_P + lat_I - lat_D; - - Serial.printf("%d, %d, %d, %d, %d, %d\n", - lon_P, lat_P, - lon_I, lat_I, - lon_D, lat_D); - -} -*/ - #define NAV_ERR_MAX 800 -static void calc_loiter1(int x_error, int y_error) -{ - int16_t lon_PI = g.pi_loiter_lon.get_pi(x_error, dTnav); - int16_t lon_D = g.loiter_d * x_actual_speed ; // this controls the small bumps - - int16_t lat_PI = g.pi_loiter_lat.get_pi(y_error, dTnav); - int16_t lat_D = g.loiter_d * y_actual_speed ; - - //limit of terms - lon_D = constrain(lon_D,-500,500); - lat_D = constrain(lat_D,-500,500); - - nav_lon = constrain(lon_PI - lon_D, -2500, 2500); - nav_lat = constrain(lat_PI - lat_D, -2500, 2500); -} - - static void calc_loiter(int x_error, int y_error) { // East/West @@ -167,22 +104,20 @@ static void calc_loiter(int x_error, int y_error) int16_t x_target_speed = g.pi_loiter_lon.get_p(x_error); int16_t x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); x_rate_error = x_target_speed - x_actual_speed; - nav_lon_p = x_rate_error * g.loiter_d; - - nav_lon_p = constrain(nav_lon_p, -1200, 1200); - nav_lon = nav_lon_p + x_iterm; - nav_lon = constrain(nav_lon, -2500, 2500); // North/South y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); int16_t y_target_speed = g.pi_loiter_lat.get_p(y_error); int16_t y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); y_rate_error = y_target_speed - y_actual_speed; - nav_lat_p = y_rate_error * g.loiter_d; - nav_lat_p = constrain(nav_lat_p, -1200, 1200); - nav_lat = nav_lat_p + y_iterm; - nav_lat = constrain(nav_lat, -2500, 2500); + calc_nav_lon(x_rate_error); + calc_nav_lat(y_rate_error); + + nav_lat = nav_lat + y_iterm; + nav_lon = nav_lon + x_iterm; + + /* int8_t ttt = 1.0/dTnav; @@ -204,7 +139,7 @@ static void calc_loiter(int x_error, int y_error) //*/ /* - int16_t t1 = g.pi_nav_lon.get_integrator(); // X + int16_t t1 = g.pid_nav_lon.get_integrator(); // X Serial.printf("%d, %1.4f, %d, %d, %d, %d, %d, %d, %d, %d\n", wp_distance, //1 dTnav, //2 @@ -219,33 +154,91 @@ static void calc_loiter(int x_error, int y_error) //*/ } +static void calc_nav_rate(int max_speed) +{ + // push us towards the original track + update_crosstrack(); + + // nav_bearing includes crosstrack + float temp = (9000l - nav_bearing) * RADX100; + + x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 + x_rate_error = constrain(x_rate_error, -1000, 1000); + int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); + + y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 + y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum + int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); + + calc_nav_lon(x_rate_error); + calc_nav_lat(y_rate_error); + + nav_lon = nav_lon + x_iterm; + nav_lat = nav_lat + y_iterm; + + /* + Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat, + x_iterm, + y_iterm, + crosstrack_error); + //*/ + + // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() + + /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", + max_speed, + x_actual_speed, + y_actual_speed, + x_rate_error, + y_rate_error, + nav_lon, + nav_lat);*/ +} + + +static void calc_nav_lon(int rate) +{ + nav_lon = g.pid_nav_lon.get_pid(rate, dTnav); + nav_lon = get_corrected_angle(rate, nav_lon); + nav_lon = constrain(nav_lon, -3000, 3000); +} + +static void calc_nav_lat(int rate) +{ + nav_lat = g.pid_nav_lon.get_pid(rate, dTnav); + nav_lat = get_corrected_angle(rate, nav_lat); + nav_lat = constrain(nav_lat, -3000, 3000); +} + +static int16_t get_corrected_angle(int16_t desired_rate, int16_t rate_out) +{ + int16_t tt = desired_rate; + // scale down the desired rate and square it + desired_rate = desired_rate / 20; + desired_rate = desired_rate * desired_rate; + int16_t tmp = 0; + + if (tt > 0){ + tmp = rate_out + (rate_out - desired_rate); + tmp = max(tmp, rate_out); + }else if (tt < 0){ + tmp = rate_out + (rate_out + desired_rate); + tmp = min(tmp, rate_out); + } + //Serial.printf("rate:%d, norm:%d, out:%d \n", tt, rate_out, tmp); + return tmp; +} + //wp_distance,ttt, y_error, y_GPS_speed, y_actual_speed, y_target_speed, y_rate_error, nav_lat, y_iterm, t2 -#define ERR_GAIN .01 -// called at 50hz -static void estimate_velocity() -{ - // we need to extimate velocity when below GPS threshold of 1.5m/s - //if(g_gps->ground_speed < 120){ - // some smoothing to prevent bumpy rides - x_actual_speed = (x_actual_speed * 15 + x_GPS_speed) / 16; - y_actual_speed = (y_actual_speed * 15 + y_GPS_speed) / 16; - - // integration of nav_p angle - //x_actual_speed += (nav_lon_p >>2); - //y_actual_speed += (nav_lat_p >>2); - - // this is just what worked best in SIM - //x_actual_speed = (x_actual_speed * 2 + x_GPS_speed * 1) / 4; - //y_actual_speed = (y_actual_speed * 2 + y_GPS_speed * 1) / 4; - - //}else{ - // less smoothing needed since the GPS already filters - // x_actual_speed = (x_actual_speed * 3 + x_GPS_speed) / 4; - // y_actual_speed = (y_actual_speed * 3 + y_GPS_speed) / 4; - //} -} // this calculation rotates our World frame of reference to the copter's frame of reference // We use the DCM's matrix to precalculate these trig values at 50hz @@ -291,55 +284,6 @@ static int16_t calc_desired_speed(int16_t max_speed) return max_speed; } -static void calc_nav_rate(int max_speed) -{ - // push us towards the original track - update_crosstrack(); - - // nav_bearing includes crosstrack - float temp = (9000l - nav_bearing) * RADX100; - - x_rate_error = (cos(temp) * max_speed) - x_actual_speed; // 413 - x_rate_error = constrain(x_rate_error, -1000, 1000); - int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); - nav_lon_p = g.pi_nav_lon.get_p(x_rate_error); - nav_lon = nav_lon_p + x_iterm; - nav_lon = constrain(nav_lon, -3000, 3000); - - y_rate_error = (sin(temp) * max_speed) - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -1000, 1000); // added a rate error limit to keep pitching down to a minimum - int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); - nav_lat_p = g.pi_nav_lat.get_p(y_rate_error); - nav_lat = nav_lat_p + y_iterm; - nav_lat = constrain(nav_lat, -3000, 3000); - - /* - Serial.printf("max_sp %d,\t x_sp %d, y_sp %d,\t x_re: %d, y_re: %d, \tnav_lon: %d, nav_lat: %d, Xi:%d, Yi:%d, \t XE %d \n", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat, - x_iterm, - y_iterm, - crosstrack_error); - //*/ - - - // nav_lat and nav_lon will be rotated to the angle of the quad in calc_nav_pitch_roll() - - /*Serial.printf("max_speed: %d, xspeed: %d, yspeed: %d, x_re: %d, y_re: %d, nav_lon: %ld, nav_lat: %ld ", - max_speed, - x_actual_speed, - y_actual_speed, - x_rate_error, - y_rate_error, - nav_lon, - nav_lat);*/ -} - static void update_crosstrack(void) { @@ -433,7 +377,7 @@ static int32_t get_new_altitude() } int32_t diff = abs(next_WP.alt - target_altitude); - int8_t _scale = 3; + int8_t _scale = 4; if (next_WP.alt < target_altitude){ // we are below the target alt @@ -444,12 +388,12 @@ static int32_t get_new_altitude() } }else { // we are above the target, going down - if(diff < 600){ - _scale = 4; - } - if(diff < 300){ + if(diff < 400){ _scale = 5; } + if(diff < 100){ + _scale = 6; + } } int32_t change = (millis() - alt_change_timer) >> _scale; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index d4ad437754..9cf32e522a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -354,8 +354,8 @@ static void init_ardupilot() Log_Write_Startup(); Log_Write_Data(10, g.pi_stabilize_roll.kP()); Log_Write_Data(11, g.pi_stabilize_pitch.kP()); - Log_Write_Data(12, g.pi_rate_roll.kP()); - Log_Write_Data(13, g.pi_rate_pitch.kP()); + Log_Write_Data(12, g.pid_rate_roll.kP()); + Log_Write_Data(13, g.pid_rate_pitch.kP()); #endif SendDebug("\nReady to FLY "); From 9dd1730ccec4100fe8f5451b848493bd400f2087 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 28 Jan 2012 22:16:24 -0800 Subject: [PATCH 10/18] reduce test.pde size for 1280 users --- ArduCopter/test.pde | 284 +++++++++++++++++++++++--------------------- 1 file changed, 151 insertions(+), 133 deletions(-) diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index e1fc53df4d..9bca37873a 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -94,7 +94,7 @@ test_mode(uint8_t argc, const Menu::arg *argv) { //Serial.printf_P(PSTR("Test Mode\n\n")); test_menu.run(); - return 0; + return 0; } static int8_t @@ -469,7 +469,7 @@ test_ins(uint8_t argc, const Menu::arg *argv) static int8_t test_imu(uint8_t argc, const Menu::arg *argv) { - #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included print_test_disabled(); return (0); #else @@ -515,7 +515,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) //dcm.kp_yaw(0.02); //dcm.ki_yaw(0); - imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); + imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); report_imu(); imu.init_gyro(delay, flash_leds); @@ -555,7 +555,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) compass.calculate(dcm.get_dcm_matrix()); } } - fast_loopTimer = millis(); + fast_loopTimer = millis(); } if(Serial.available() > 0){ return (0); @@ -731,59 +731,68 @@ test_tuning(uint8_t argc, const Menu::arg *argv) static int8_t test_battery(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - //delta_ms_medium_loop = 100; + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else + print_hit_enter(); + while(1){ + delay(100); + read_radio(); + read_battery(); + if (g.battery_monitoring == 3){ + Serial.printf_P(PSTR("V: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } else { + Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } + APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_3, g.rc_3.radio_in); + APM_RC.OutputCh(MOT_4, g.rc_3.radio_in); - while(1){ - delay(100); - read_radio(); - read_battery(); - if (g.battery_monitoring == 3){ - Serial.printf_P(PSTR("V: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); - } else { - Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); + if(Serial.available() > 0){ + return (0); + } } - APM_RC.OutputCh(MOT_1, g.rc_3.radio_in); - APM_RC.OutputCh(MOT_2, g.rc_3.radio_in); - APM_RC.OutputCh(MOT_3, g.rc_3.radio_in); - APM_RC.OutputCh(MOT_4, g.rc_3.radio_in); - - if(Serial.available() > 0){ - return (0); - } - } - return (0); + return (0); + #endif } - static int8_t test_relay(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - delay(1000); + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else - while(1){ - Serial.printf_P(PSTR("Relay on\n")); - relay.on(); - delay(3000); - if(Serial.available() > 0){ - return (0); - } + print_hit_enter(); + delay(1000); - Serial.printf_P(PSTR("Relay off\n")); - relay.off(); - delay(3000); - if(Serial.available() > 0){ - return (0); + while(1){ + Serial.printf_P(PSTR("Relay on\n")); + relay.on(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } + + Serial.printf_P(PSTR("Relay off\n")); + relay.off(); + delay(3000); + if(Serial.available() > 0){ + return (0); + } } - } + #endif } + static int8_t test_wp(uint8_t argc, const Menu::arg *argv) { @@ -810,20 +819,20 @@ test_wp(uint8_t argc, const Menu::arg *argv) /* print_hit_enter(); delay(1000); - while(1){ - if (Serial3.available()){ - digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS - Serial1.write(Serial3.read()); - digitalWrite(B_LED_PIN, LED_OFF); - } - if (Serial1.available()){ - digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS - Serial3.write(Serial1.read()); - digitalWrite(C_LED_PIN, LED_OFF); - } - if(Serial.available() > 0){ - return (0); - } + while(1){ + if (Serial3.available()){ + digitalWrite(B_LED_PIN, LED_ON); // Blink Yellow LED if we are sending data to GPS + Serial1.write(Serial3.read()); + digitalWrite(B_LED_PIN, LED_OFF); + } + if (Serial1.available()){ + digitalWrite(C_LED_PIN, LED_ON); // Blink Red LED if we are receiving data from GPS + Serial3.write(Serial1.read()); + digitalWrite(C_LED_PIN, LED_OFF); + } + if(Serial.available() > 0){ + return (0); + } } */ //} @@ -836,7 +845,7 @@ test_wp(uint8_t argc, const Menu::arg *argv) Serial.printf_P(PSTR("Begin XBee X-CTU Range and RSSI Test:\n")); while(1){ - if (Serial3.available()) + if (Serial3.available()) Serial3.write(Serial3.read()); if(Serial.available() > 0){ @@ -850,30 +859,30 @@ test_wp(uint8_t argc, const Menu::arg *argv) static int8_t test_baro(uint8_t argc, const Menu::arg *argv) { - print_hit_enter(); - init_barometer(); + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else + print_hit_enter(); + init_barometer(); - while(1){ - delay(100); - int32_t alt = read_barometer(); // calls barometer.read() + while(1){ + delay(100); + int32_t alt = read_barometer(); // calls barometer.read() - #if defined( __AVR_ATmega1280__ ) - Serial.printf_P(PSTR("alt: %ldcm\n"),alt); - - #else - int32_t pres = barometer.get_pressure(); - int16_t temp = barometer.get_temperature(); - int32_t raw_pres = barometer.get_raw_pressure(); - int32_t raw_temp = barometer.get_raw_temp(); - Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," - " raw pres: %ld, raw temp: %ld\n"), - alt, pres ,temp, raw_pres, raw_temp); - #endif - if(Serial.available() > 0){ - return (0); + int32_t pres = barometer.get_pressure(); + int16_t temp = barometer.get_temperature(); + int32_t raw_pres = barometer.get_raw_pressure(); + int32_t raw_temp = barometer.get_raw_temp(); + Serial.printf_P(PSTR("alt: %ldcm, pres: %ldmbar, temp: %d/100degC," + " raw pres: %ld, raw temp: %ld\n"), + alt, pres ,temp, raw_pres, raw_temp); + if(Serial.available() > 0){ + return (0); + } } - } - return 0; + return 0; + #endif } #endif @@ -881,35 +890,38 @@ test_baro(uint8_t argc, const Menu::arg *argv) static int8_t test_mag(uint8_t argc, const Menu::arg *argv) { - if(g.compass_enabled) { - //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); - - print_hit_enter(); - - while(1){ - delay(100); - if (compass.read()) { - compass.calculate(dcm.get_dcm_matrix()); - Vector3f maggy = compass.get_offsets(); - Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), - (wrap_360(ToDeg(compass.heading) * 100)) /100, - compass.mag_x, - compass.mag_y, - compass.mag_z); - } else { - Serial.println_P(PSTR("not healthy")); - } - - if(Serial.available() > 0){ - return (0); - } - } - } else { - Serial.printf_P(PSTR("Compass: ")); - print_enabled(false); + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); return (0); - } - return (0); + #else + if(g.compass_enabled) { + print_hit_enter(); + + while(1){ + delay(100); + if (compass.read()) { + compass.calculate(dcm.get_dcm_matrix()); + Vector3f maggy = compass.get_offsets(); + Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), + (wrap_360(ToDeg(compass.heading) * 100)) /100, + compass.mag_x, + compass.mag_y, + compass.mag_z); + } else { + Serial.println_P(PSTR("not healthy")); + } + + if(Serial.available() > 0){ + return (0); + } + } + } else { + Serial.printf_P(PSTR("Compass: ")); + print_enabled(false); + return (0); + } + return (0); + #endif } /* @@ -1023,25 +1035,31 @@ test_optflow(uint8_t argc, const Menu::arg *argv) /* test the dataflash is working */ + static int8_t test_logging(uint8_t argc, const Menu::arg *argv) { - Serial.println_P(PSTR("Testing dataflash logging")); - if (!DataFlash.CardInserted()) { - Serial.println_P(PSTR("ERR: No dataflash inserted")); - return 0; - } - DataFlash.ReadManufacturerID(); - Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), - (unsigned)DataFlash.df_manufacturer, - (unsigned)DataFlash.df_device); - Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), - (unsigned)DataFlash.df_NumPages+1, - (unsigned)DataFlash.df_PageSize); - DataFlash.StartRead(DataFlash.df_NumPages+1); - Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), - (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); - return 0; + #if defined( __AVR_ATmega1280__ ) // determines if optical flow code is included + print_test_disabled(); + return (0); + #else + Serial.println_P(PSTR("Testing dataflash logging")); + if (!DataFlash.CardInserted()) { + Serial.println_P(PSTR("ERR: No dataflash inserted")); + return 0; + } + DataFlash.ReadManufacturerID(); + Serial.printf_P(PSTR("Manufacturer: 0x%02x Device: 0x%04x\n"), + (unsigned)DataFlash.df_manufacturer, + (unsigned)DataFlash.df_device); + Serial.printf_P(PSTR("NumPages: %u PageSize: %u\n"), + (unsigned)DataFlash.df_NumPages+1, + (unsigned)DataFlash.df_PageSize); + DataFlash.StartRead(DataFlash.df_NumPages+1); + Serial.printf_P(PSTR("Format version: %lx Expected format version: %lx\n"), + (unsigned long)DataFlash.ReadLong(), (unsigned long)DF_LOGGING_FORMAT); + return 0; + #endif } @@ -1061,11 +1079,11 @@ static int8_t //} // clear home - {Location t = {0, 0, 0, 0, 0, 0}; + {Location t = {0, 0, 0, 0, 0, 0}; set_cmd_with_index(t,0);} // CMD opt pitch alt/cm - {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; + {Location t = {MAV_CMD_NAV_TAKEOFF, WP_OPTION_RELATIVE, 0, 100, 0, 0}; set_cmd_with_index(t,1);} if (!strcmp_P(argv[1].str, PSTR("wp"))) { @@ -1074,25 +1092,25 @@ static int8_t {Location t = {MAV_CMD_NAV_WAYPOINT, WP_OPTION_RELATIVE, 15, 0, 0, 0}; set_cmd_with_index(t,2);} // CMD opt - {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; + {Location t = {MAV_CMD_NAV_RETURN_TO_LAUNCH, WP_OPTION_YAW, 0, 0, 0, 0}; set_cmd_with_index(t,3);} // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; set_cmd_with_index(t,4);} } else { //2250 = 25 meteres // CMD opt p1 //alt //NS //WE - {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 + {Location t = {MAV_CMD_NAV_LOITER_TIME, 0, 10, 0, 0, 0}; // 19 set_cmd_with_index(t,2);} // CMD opt dir angle/deg deg/s relative - {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; + {Location t = {MAV_CMD_CONDITION_YAW, 0, 1, 360, 60, 1}; set_cmd_with_index(t,3);} // CMD opt - {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; + {Location t = {MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0}; set_cmd_with_index(t,4);} } From 2a8044c666dbdd237d1efc2c809b8bf38c6bb4d0 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 28 Jan 2012 22:35:57 -0800 Subject: [PATCH 11/18] Landing update for better baro landing --- ArduCopter/commands_logic.pde | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index d491201529..8a09f52a71 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -366,7 +366,7 @@ static bool verify_land_sonar() } if(current_loc.alt < 200 ){ - wp_control = NO_NAV_MODE; + wp_control = NO_NAV_MODE; } if(current_loc.alt < 150 ){ @@ -401,15 +401,15 @@ static bool verify_land_baro() landing_boost++; landing_boost = min(landing_boost, 40); } + if(current_loc.alt < 200 ){ wp_control = NO_NAV_MODE; } if(current_loc.alt < 150 ){ if(abs(climb_rate) < 20) { + landing_boost++; if(ground_detector++ > 30) { - //landing_boost = 100; - //Serial.print("land_complete\n"); land_complete = true; ground_detector = 0; // init disarm motors From 0bf594483f0b105b0b30e4ae0a955bf9efb128f4 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 28 Jan 2012 22:52:17 -0800 Subject: [PATCH 12/18] Landing now disarms automatically. --- Tools/autotest/arducopter.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 21f0b705c5..bfe1930485 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -519,10 +519,10 @@ def fly_ArduCopter(viewerip=None): print("land failed") failed = True - print("# disarm motors") - if not disarm_motors(mavproxy, mav): - print("disarm_motors failed") - failed = True + #print("# disarm motors") + #if not disarm_motors(mavproxy, mav): + # print("disarm_motors failed") + # failed = True except pexpect.TIMEOUT, e: failed = True From a1ac33007780062426e310e9572148d03b719c06 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 28 Jan 2012 22:52:39 -0800 Subject: [PATCH 13/18] to speed up testing suite --- Tools/autotest/mission2.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/autotest/mission2.txt b/Tools/autotest/mission2.txt index e921ac757c..7fc51ab508 100644 --- a/Tools/autotest/mission2.txt +++ b/Tools/autotest/mission2.txt @@ -23,7 +23,7 @@ QGC WPL 110 # MAV_CMD_NAV_LOITER_TURNS # Turns lat lon alt continue -6 0 3 18 2 0 0 0 0 0 20 1 +#6 0 3 18 2 0 0 0 0 0 20 1 # MAV_CMD_DO_SET_ROI, MAV_ROI_WPNEXT = 1 # MAV_ROI WP index ROI index lat lon alt continue From 4099455bef3a016ee168d4c9d0780da2ef673fea Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sun, 29 Jan 2012 15:45:20 +0800 Subject: [PATCH 14/18] APM Planner 1.1.30 add OF_LOITER add AC TUNE options fix default alt issues. make Scripts, 1 at a time. update aerosim-rc plugin. add new "follow me" mode- need a nmea gps and xbee on pc. control-F > follow me --- .../ArdupilotMegaPlanner/ArdupilotMega.csproj | 9 + Tools/ArdupilotMegaPlanner/Common.cs | 35 +- .../GCSViews/Configuration.Designer.cs | 76 +- .../GCSViews/Configuration.resx | 4162 ++++++++++++++--- .../GCSViews/FlightData.resx | 2 +- .../GCSViews/FlightPlanner.cs | 12 +- .../GCSViews/Simulation.cs | 4 - Tools/ArdupilotMegaPlanner/MAVLink.cs | 2 +- .../Properties/AssemblyInfo.cs | 2 +- Tools/ArdupilotMegaPlanner/Script.cs | 4 +- .../SerialInput.Designer.cs | 135 + Tools/ArdupilotMegaPlanner/SerialInput.cs | 224 + Tools/ArdupilotMegaPlanner/SerialInput.resx | 208 + Tools/ArdupilotMegaPlanner/temp.Designer.cs | 13 + Tools/ArdupilotMegaPlanner/temp.cs | 7 + 15 files changed, 4147 insertions(+), 748 deletions(-) create mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs create mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.cs create mode 100644 Tools/ArdupilotMegaPlanner/SerialInput.resx diff --git a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj index ca896af5e0..a0ee6c833b 100644 --- a/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj +++ b/Tools/ArdupilotMegaPlanner/ArdupilotMega.csproj @@ -253,6 +253,12 @@ XorPlus.cs + + Form + + + SerialInput.cs + Firmware.cs @@ -432,6 +438,9 @@ XorPlus.cs + + SerialInput.cs + Configuration.cs diff --git a/Tools/ArdupilotMegaPlanner/Common.cs b/Tools/ArdupilotMegaPlanner/Common.cs index 014f0498e7..bc4bfa801c 100644 --- a/Tools/ArdupilotMegaPlanner/Common.cs +++ b/Tools/ArdupilotMegaPlanner/Common.cs @@ -198,11 +198,43 @@ namespace ArdupilotMega this.Lng = pll.Lng; } + public PointLatLngAlt(PointLatLngAlt plla) + { + this.Lat = plla.Lat; + this.Lng = plla.Lng; + this.Alt = plla.Alt; + this.color = plla.color; + this.Tag = plla.Tag; + } + public PointLatLng Point() { return new PointLatLng(Lat, Lng); } + public override bool Equals(Object pllaobj) + { + PointLatLngAlt plla = (PointLatLngAlt)pllaobj; + + if (plla == null) + return false; + + if (this.Lat == plla.Lat && + this.Lng == plla.Lng && + this.Alt == plla.Alt && + this.color == plla.color && + this.Tag == plla.Tag) + { + return true; + } + return false; + } + + public override int GetHashCode() + { + return (int)((Lat + Lng + Alt) * 100); + } + /// /// Calc Distance in M /// @@ -263,7 +295,8 @@ namespace ArdupilotMega RTL = 6, // AUTO control CIRCLE = 7, POSITION = 8, - LAND = 9 // AUTO control + LAND = 9, // AUTO control + OF_LOITER = 10 } public static void linearRegression() diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs index b41cf713b8..55fa2ecbeb 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.Designer.cs @@ -30,8 +30,8 @@ { this.components = new System.ComponentModel.Container(); System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(Configuration)); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle1 = new System.Windows.Forms.DataGridViewCellStyle(); - System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle2 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle3 = new System.Windows.Forms.DataGridViewCellStyle(); + System.Windows.Forms.DataGridViewCellStyle dataGridViewCellStyle4 = new System.Windows.Forms.DataGridViewCellStyle(); this.Params = new System.Windows.Forms.DataGridView(); this.Command = new System.Windows.Forms.DataGridViewTextBoxColumn(); this.Value = new System.Windows.Forms.DataGridViewTextBoxColumn(); @@ -285,6 +285,8 @@ this.BUT_load = new ArdupilotMega.MyButton(); this.toolTip1 = new System.Windows.Forms.ToolTip(this.components); this.BUT_compare = new ArdupilotMega.MyButton(); + this.myLabel2 = new ArdupilotMega.MyLabel(); + this.TUNE = new System.Windows.Forms.ComboBox(); ((System.ComponentModel.ISupportInitialize)(this.Params)).BeginInit(); this.ConfigTabs.SuspendLayout(); this.TabAP.SuspendLayout(); @@ -405,14 +407,14 @@ this.Params.AllowUserToAddRows = false; this.Params.AllowUserToDeleteRows = false; resources.ApplyResources(this.Params, "Params"); - dataGridViewCellStyle1.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle1.BackColor = System.Drawing.Color.Maroon; - dataGridViewCellStyle1.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle1.ForeColor = System.Drawing.Color.White; - dataGridViewCellStyle1.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle1.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle1.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle1; + dataGridViewCellStyle3.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle3.BackColor = System.Drawing.Color.Maroon; + dataGridViewCellStyle3.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle3.ForeColor = System.Drawing.Color.White; + dataGridViewCellStyle3.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle3.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle3.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.ColumnHeadersDefaultCellStyle = dataGridViewCellStyle3; this.Params.ColumnHeadersHeightSizeMode = System.Windows.Forms.DataGridViewColumnHeadersHeightSizeMode.AutoSize; this.Params.Columns.AddRange(new System.Windows.Forms.DataGridViewColumn[] { this.Command, @@ -421,14 +423,14 @@ this.mavScale, this.RawValue}); this.Params.Name = "Params"; - dataGridViewCellStyle2.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; - dataGridViewCellStyle2.BackColor = System.Drawing.SystemColors.ActiveCaption; - dataGridViewCellStyle2.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); - dataGridViewCellStyle2.ForeColor = System.Drawing.SystemColors.WindowText; - dataGridViewCellStyle2.SelectionBackColor = System.Drawing.SystemColors.Highlight; - dataGridViewCellStyle2.SelectionForeColor = System.Drawing.SystemColors.HighlightText; - dataGridViewCellStyle2.WrapMode = System.Windows.Forms.DataGridViewTriState.True; - this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle2; + dataGridViewCellStyle4.Alignment = System.Windows.Forms.DataGridViewContentAlignment.MiddleLeft; + dataGridViewCellStyle4.BackColor = System.Drawing.SystemColors.ActiveCaption; + dataGridViewCellStyle4.Font = new System.Drawing.Font("Microsoft Sans Serif", 8.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + dataGridViewCellStyle4.ForeColor = System.Drawing.SystemColors.WindowText; + dataGridViewCellStyle4.SelectionBackColor = System.Drawing.SystemColors.Highlight; + dataGridViewCellStyle4.SelectionForeColor = System.Drawing.SystemColors.HighlightText; + dataGridViewCellStyle4.WrapMode = System.Windows.Forms.DataGridViewTriState.True; + this.Params.RowHeadersDefaultCellStyle = dataGridViewCellStyle4; this.Params.RowHeadersVisible = false; this.Params.CellValueChanged += new System.Windows.Forms.DataGridViewCellEventHandler(this.Params_CellValueChanged); // @@ -1089,6 +1091,8 @@ // // TabAC // + this.TabAC.Controls.Add(this.myLabel2); + this.TabAC.Controls.Add(this.TUNE); this.TabAC.Controls.Add(this.myLabel1); this.TabAC.Controls.Add(this.CH7_OPT); this.TabAC.Controls.Add(this.groupBox17); @@ -2092,6 +2096,40 @@ this.BUT_compare.UseVisualStyleBackColor = true; this.BUT_compare.Click += new System.EventHandler(this.BUT_compare_Click); // + // myLabel2 + // + resources.ApplyResources(this.myLabel2, "myLabel2"); + this.myLabel2.Name = "myLabel2"; + this.myLabel2.resize = false; + // + // TUNE + // + this.TUNE.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.TUNE.FormattingEnabled = true; + this.TUNE.Items.AddRange(new object[] { + resources.GetString("TUNE.Items"), + resources.GetString("TUNE.Items1"), + resources.GetString("TUNE.Items2"), + resources.GetString("TUNE.Items3"), + resources.GetString("TUNE.Items4"), + resources.GetString("TUNE.Items5"), + resources.GetString("TUNE.Items6"), + resources.GetString("TUNE.Items7"), + resources.GetString("TUNE.Items8"), + resources.GetString("TUNE.Items9"), + resources.GetString("TUNE.Items10"), + resources.GetString("TUNE.Items11"), + resources.GetString("TUNE.Items12"), + resources.GetString("TUNE.Items13"), + resources.GetString("TUNE.Items14"), + resources.GetString("TUNE.Items15"), + resources.GetString("TUNE.Items16"), + resources.GetString("TUNE.Items17"), + resources.GetString("TUNE.Items18"), + resources.GetString("TUNE.Items19")}); + resources.ApplyResources(this.TUNE, "TUNE"); + this.TUNE.Name = "TUNE"; + // // Configuration // resources.ApplyResources(this, "$this"); @@ -2479,5 +2517,7 @@ private System.Windows.Forms.Label label48; private MyLabel myLabel1; private System.Windows.Forms.ComboBox CH7_OPT; + private MyLabel myLabel2; + private System.Windows.Forms.ComboBox TUNE; } } diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx index e77e622e34..3b22247b59 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Configuration.resx @@ -192,6 +192,2346 @@ Top, Bottom, Left, Right + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 0 + + + groupBox1 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 1 + + + groupBox2 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 2 + + + groupBox15 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 3 + + + groupBox16 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 4 + + + groupBox14 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 5 + + + groupBox13 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 6 + + + groupBox12 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 7 + + + groupBox11 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 8 + + + groupBox10 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 9 + + + groupBox9 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 10 + + + groupBox8 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 11 + + + 4, 22 + + + 0, 0, 0, 0 + + + 722, 434 + + + 0 + + + ArduPlane + + + TabAP + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 0 + + + 358, 297 + + + 53, 23 + + + 20 + + + Ch6 Opt + + + myLabel2 + + + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabAC + + + 0 + + + CH6_NONE + + + CH6_STABILIZE_KP + + + CH6_STABILIZE_KI + + + CH6_YAW_KP + + + CH6_RATE_KP + + + CH6_RATE_KI + + + CH6_YAW_RATE_KP + + + CH6_THROTTLE_KP + + + CH6_TOP_BOTTOM_RATIO + + + CH6_RELAY + + + CH6_TRAVERSE_SPEED + + + CH6_NAV_P + + + CH6_LOITER_P + + + CH6_HELI_EXTERNAL_GYRO + + + CH6_THR_HOLD_KP + + + CH6_Z_GAIN + + + CH6_DAMP + + + CH6_OPTFLOW_KP + + + CH6_OPTFLOW_KI + + + CH6_OPTFLOW_KD + + + 417, 297 + + + 112, 21 + + + 19 + + + TUNE + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 1 + + + 358, 270 + + + 53, 23 + + + 18 + + + Ch7 Opt + + + myLabel1 + + + ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabAC + + + 2 + + + Do Nothing + + + + + + + + + Simple Mode + + + RTL + + + + + + + + + Save WP + + + 417, 270 + + + 112, 21 + + + 17 + + + CH7_OPT + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 3 + + + ACRO_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 0 + + + label27 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 1 + + + ACRO_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 2 + + + label29 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 3 + + + ACRO_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 4 + + + label33 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox17 + + + 5 + + + 182, 337 + + + 170, 91 + + + 13 + + + Acro Pitch + + + groupBox17 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 4 + + + label14 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 0 + + + THR_RATE_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 1 + + + THR_RATE_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 2 + + + label20 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 3 + + + THR_RATE_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 4 + + + label25 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox5 + + + 5 + + + 6, 221 + + + 170, 110 + + + 16 + + + Throttle Rate + + + groupBox5 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 5 + + + ACRO_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 0 + + + label40 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 1 + + + ACRO_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 2 + + + label44 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 3 + + + ACRO_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 4 + + + label48 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox18 + + + 5 + + + 6, 337 + + + 170, 91 + + + 14 + + + Acro Roll + + + groupBox18 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 6 + + + True + + + 3, 198 + + + 154, 17 + + + 13 + + + Lock Pitch and Roll Values + + + CHK_lockrollpitch + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 7 + + + WP_SPEED_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 0 + + + label9 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 1 + + + NAV_LAT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 2 + + + label13 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 3 + + + NAV_LAT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 4 + + + label15 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 5 + + + NAV_LAT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 6 + + + label16 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox4 + + + 7 + + + 534, 107 + + + 170, 108 + + + 0 + + + Nav WP + + + groupBox4 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 8 + + + XTRK_GAIN_SC1 + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 0 + + + label18 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox6 + + + 1 + + + 358, 221 + + + 170, 43 + + + 2 + + + Crosstrack Correction + + + groupBox6 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 9 + + + THR_ALT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 0 + + + label19 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 1 + + + THR_ALT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 2 + + + label21 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 3 + + + THR_ALT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 4 + + + label22 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox7 + + + 5 + + + 182, 221 + + + 170, 110 + + + 3 + + + Altitude Hold + + + groupBox7 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 10 + + + HLD_LAT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 0 + + + label28 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 1 + + + HLD_LAT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 2 + + + label30 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 3 + + + HLD_LAT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 4 + + + label31 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox19 + + + 5 + + + 531, 6 + + + 170, 95 + + + 6 + + + Loiter + + + groupBox19 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 11 + + + STB_YAW_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 0 + + + label32 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 1 + + + STB_YAW_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 2 + + + label34 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 3 + + + STB_YAW_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 4 + + + label35 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox20 + + + 5 + + + 358, 6 + + + 170, 95 + + + 7 + + + Stabilize Yaw + + + groupBox20 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 12 + + + STB_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 0 + + + label36 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 1 + + + STB_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 2 + + + label41 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 3 + + + STB_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 4 + + + label42 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox21 + + + 5 + + + 182, 6 + + + 170, 95 + + + 8 + + + Stabilize Pitch + + + groupBox21 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 13 + + + STB_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 0 + + + label43 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 1 + + + STB_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 2 + + + label45 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 3 + + + STB_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 4 + + + label46 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox22 + + + 5 + + + 6, 6 + + + 170, 95 + + + 9 + + + Stabilize Roll + + + groupBox22 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 14 + + + RATE_YAW_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 0 + + + label47 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 1 + + + RATE_YAW_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 2 + + + label77 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 3 + + + RATE_YAW_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 4 + + + label82 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox23 + + + 5 + + + 358, 107 + + + 170, 91 + + + 10 + + + Rate Yaw + + + groupBox23 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 15 + + + RATE_PIT_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 0 + + + label84 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 1 + + + RATE_PIT_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 2 + + + label86 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 3 + + + RATE_PIT_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 4 + + + label87 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox24 + + + 5 + + + 182, 107 + + + 170, 91 + + + 11 + + + Rate Pitch + + + groupBox24 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 16 + + + RATE_RLL_IMAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 0 + + + label88 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 1 + + + RATE_RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 2 + + + label90 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 3 + + + RATE_RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 4 + + + label91 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox25 + + + 5 + + + 6, 107 + + + 170, 91 + + + 12 + + + Rate Roll + + + groupBox25 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAC + + + 17 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 1 + + + ArduCopter + + + TabAC + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 1 + + + label26 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 0 + + + CMB_videoresolutions + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 1 + + + label12 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 2 + + + CHK_GDIPlus + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 3 + + + label24 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 4 + + + CHK_loadwponconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 5 + + + label23 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 6 + + + NUM_tracklength + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 7 + + + CHK_speechaltwarning + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 8 + + + label108 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 9 + + + CHK_resetapmonconnect + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 10 + + + CHK_mavdebug + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 11 + + + label107 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 12 + + + CMB_raterc + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 13 + + + label104 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 14 + + + label103 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 15 + + + label102 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 16 + + + label101 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 17 + + + CMB_ratestatus + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 18 + + + CMB_rateposition + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 19 + + + CMB_rateattitude + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 20 + + + label99 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 21 + + + label98 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 22 + + + label97 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 23 + + + CMB_speedunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 24 + + + CMB_distunits + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 25 + + + label96 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 26 + + + label95 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 27 + + + CHK_speechbattery + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 28 + + + CHK_speechcustom + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 29 + + + CHK_speechmode + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 30 + + + CHK_speechwaypoint + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 31 + + + label94 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 32 + + + CMB_osdcolor + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 33 + + + CMB_language + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 34 + + + label93 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 35 + + + CHK_enablespeech + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 36 + + + CHK_hudshow + + + System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 37 + + + label92 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 38 + + + CMB_videosources + + + System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabPlanner + + + 39 + + + BUT_Joystick + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabPlanner + + + 40 + + + BUT_videostop + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabPlanner + + + 41 + + + BUT_videostart + + + ArdupilotMega.MyButton, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc + + + TabPlanner + + + 42 + + + 4, 22 + + + 3, 3, 3, 3 + + + 722, 434 + + + 2 + + + Planner + + + TabPlanner + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 2 + + + 4, 22 + + + 722, 434 + + + 3 + + + Setup + + + TabSetup + + + System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + ConfigTabs + + + 3 + + + 52, 18 + + + 278, 0 + + + 0, 0, 0, 0 + + + 0, 0 + + + 730, 460 + + + 62 + + + ConfigTabs + + + System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + $this + + + 2 + + + THR_FS_VALUE + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 0 + + + label5 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 1 + + + THR_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 2 + + + label6 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 3 + + + THR_MIN + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 4 + + + label7 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 5 + + + TRIM_THROTTLE + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 6 + + + label8 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox3 + + + 7 + + + 405, 217 + + + 195, 108 + + + 0 + + + Throttle 0-100% + + + groupBox3 + + + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + TabAP + + + 0 + 111, 82 @@ -384,29 +2724,125 @@ 7 - - 405, 217 + + ARSPD_RATIO - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 0 + + + label1 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 1 + + + ARSPD_FBW_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 2 + + + label2 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 3 + + + ARSPD_FBW_MIN + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 4 + + + label3 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 5 + + + TRIM_ARSPD_CM + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 6 + + + label4 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox1 + + + 7 + + + 406, 325 + + 195, 108 - - 0 + + 1 - - Throttle 0-100% + + Airspeed m/s - - groupBox3 + + groupBox1 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 0 + + 1 111, 82 @@ -600,29 +3036,101 @@ 7 - - 406, 325 + + LIM_PITCH_MIN - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 0 + + + label39 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 1 + + + LIM_PITCH_MAX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 2 + + + label38 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 3 + + + LIM_ROLL_CD + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 4 + + + label37 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox2 + + + 5 + + + 205, 325 + + 195, 108 - - 1 + + 2 - - Airspeed m/s + + Navigation Angles - - groupBox1 + + groupBox2 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 1 + + 2 111, 59 @@ -768,29 +3276,77 @@ 5 - - 205, 325 + + XTRK_ANGLE_CD - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 0 + + + label79 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 1 + + + XTRK_GAIN_SC + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 2 + + + label80 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox15 + + + 3 + + + 4, 325 + + 195, 108 - - 2 + + 3 - - Navigation Angles + + Xtrack Pids - - groupBox2 + + groupBox15 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 2 + + 3 111, 36 @@ -888,29 +3444,101 @@ 3 - - 4, 325 + + KFF_PTCH2THR - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 0 + + + label83 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 1 + + + KFF_RDDRMIX + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 2 + + + label78 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 3 + + + KFF_PTCHCOMP + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 4 + + + label81 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox16 + + + 5 + + + 205, 217 + + 195, 108 - - 3 + + 4 - - Xtrack Pids + + Other Mix's - - groupBox15 + + groupBox16 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 3 + + 4 111, 13 @@ -1056,29 +3684,125 @@ 5 - - 205, 217 + + ENRGY2THR_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 0 + + + label73 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 1 + + + ENRGY2THR_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 2 + + + label74 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 3 + + + ENRGY2THR_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 4 + + + label75 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 5 + + + ENRGY2THR_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 6 + + + label76 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox14 + + + 7 + + + 4, 217 + + 195, 108 - - 4 + + 5 - - Other Mix's + + Energy/Alt Pid - - groupBox16 + + groupBox14 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 4 + + 5 111, 82 @@ -1272,29 +3996,125 @@ 7 - - 4, 217 + + ALT2PTCH_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 0 + + + label69 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 1 + + + ALT2PTCH_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 2 + + + label70 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 3 + + + ALT2PTCH_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 4 + + + label71 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 5 + + + ALT2PTCH_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 6 + + + label72 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox13 + + + 7 + + + 406, 109 + + 195, 108 - - 5 + + 6 - - Energy/Alt Pid + + Nav Pitch Alt Pid - - groupBox14 + + groupBox13 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 5 + + 6 111, 82 @@ -1488,29 +4308,125 @@ 7 - - 406, 109 + + ARSP2PTCH_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 0 + + + label65 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 1 + + + ARSP2PTCH_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 2 + + + label66 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 3 + + + ARSP2PTCH_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 4 + + + label67 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 5 + + + ARSP2PTCH_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 6 + + + label68 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox12 + + + 7 + + + 205, 109 + + 195, 108 - - 6 + + 7 - - Nav Pitch Alt Pid + + Nav Pitch AS Pid - - groupBox13 + + groupBox12 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 6 + + 7 111, 82 @@ -1704,29 +4620,125 @@ 7 - - 205, 109 + + HDNG2RLL_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 0 + + + label61 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 1 + + + HDNG2RLL_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 2 + + + label62 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 3 + + + HDNG2RLL_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 4 + + + label63 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 5 + + + HDNG2RLL_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 6 + + + label64 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox11 + + + 7 + + + 4, 109 + + 195, 108 - - 7 + + 8 - - Nav Pitch AS Pid + + Nav Roll Pid - - groupBox12 + + groupBox11 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 7 + + 8 111, 82 @@ -1920,29 +4932,125 @@ 7 - - 4, 109 + + YW2SRV_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 0 + + + label57 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 1 + + + YW2SRV_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 2 + + + label58 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 3 + + + YW2SRV_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 4 + + + label59 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 5 + + + YW2SRV_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 6 + + + label60 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox10 + + + 7 + + + 406, 1 + + 195, 108 - - 8 + + 9 - - Nav Roll Pid + + Servo Yaw Pid - - groupBox11 + + groupBox10 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 8 + + 9 111, 82 @@ -2136,29 +5244,125 @@ 7 - - 406, 1 + + PTCH2SRV_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 0 + + + label53 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 1 + + + PTCH2SRV_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 2 + + + label54 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 3 + + + PTCH2SRV_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 4 + + + label55 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 5 + + + PTCH2SRV_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 6 + + + label56 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox9 + + + 7 + + + 205, 1 + + 195, 108 - - 9 + + 10 - - Servo Yaw Pid + + Servo Pitch Pid - - groupBox10 + + groupBox9 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 9 + + 10 111, 82 @@ -2352,29 +5556,125 @@ 7 - - 205, 1 + + RLL2SRV_IMAX - + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 0 + + + label49 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 1 + + + RLL2SRV_D + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 2 + + + label50 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 3 + + + RLL2SRV_I + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 4 + + + label51 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 5 + + + RLL2SRV_P + + + System.Windows.Forms.NumericUpDown, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 6 + + + label52 + + + System.Windows.Forms.Label, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + groupBox8 + + + 7 + + + 4, 1 + + 195, 108 - - 10 + + 11 - - Servo Pitch Pid + + Servo Roll Pid - - groupBox9 + + groupBox8 - + System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - + TabAP - - 10 + + 11 111, 82 @@ -2568,126 +5868,6 @@ 7 - - 4, 1 - - - 195, 108 - - - 11 - - - Servo Roll Pid - - - groupBox8 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAP - - - 11 - - - 4, 22 - - - 0, 0, 0, 0 - - - 722, 434 - - - 0 - - - ArduPlane - - - TabAP - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 0 - - - 358, 270 - - - 53, 23 - - - 18 - - - Ch7 Opt - - - myLabel1 - - - ArdupilotMega.MyLabel, ArdupilotMegaPlanner, Version=1.0.0.0, Culture=neutral, PublicKeyToken=38326cb7e06851fc - - - TabAC - - - 0 - - - Do Nothing - - - - - - - - - Simple Mode - - - RTL - - - - - - - - - Save WP - - - 417, 270 - - - 112, 21 - - - 17 - - - CH7_OPT - - - System.Windows.Forms.ComboBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 1 - 80, 63 @@ -2832,30 +6012,6 @@ 5 - - 182, 337 - - - 170, 91 - - - 13 - - - Acro Pitch - - - groupBox17 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 2 - NoControl @@ -3000,30 +6156,6 @@ 5 - - 6, 221 - - - 170, 110 - - - 16 - - - Throttle Rate - - - groupBox5 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 3 - 80, 63 @@ -3168,57 +6300,6 @@ 5 - - 6, 337 - - - 170, 91 - - - 14 - - - Acro Roll - - - groupBox18 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 4 - - - True - - - 3, 198 - - - 154, 17 - - - 13 - - - Lock Pitch and Roll Values - - - CHK_lockrollpitch - - - System.Windows.Forms.CheckBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 5 - 80, 86 @@ -3411,30 +6492,6 @@ 7 - - 534, 107 - - - 170, 108 - - - 0 - - - Nav WP - - - groupBox4 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 6 - 80, 13 @@ -3483,30 +6540,6 @@ 1 - - 358, 221 - - - 170, 43 - - - 2 - - - Crosstrack Correction - - - groupBox6 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 7 - 80, 63 @@ -3651,30 +6684,6 @@ 5 - - 182, 221 - - - 170, 110 - - - 3 - - - Altitude Hold - - - groupBox7 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 8 - 80, 61 @@ -3819,30 +6828,6 @@ 5 - - 531, 6 - - - 170, 95 - - - 6 - - - Loiter - - - groupBox19 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 9 - 80, 63 @@ -3987,30 +6972,6 @@ 5 - - 358, 6 - - - 170, 95 - - - 7 - - - Stabilize Yaw - - - groupBox20 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 10 - 80, 63 @@ -4155,30 +7116,6 @@ 5 - - 182, 6 - - - 170, 95 - - - 8 - - - Stabilize Pitch - - - groupBox21 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 11 - 80, 63 @@ -4323,30 +7260,6 @@ 5 - - 6, 6 - - - 170, 95 - - - 9 - - - Stabilize Roll - - - groupBox22 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 12 - 80, 63 @@ -4491,30 +7404,6 @@ 5 - - 358, 107 - - - 170, 91 - - - 10 - - - Rate Yaw - - - groupBox23 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 13 - 80, 63 @@ -4659,30 +7548,6 @@ 5 - - 182, 107 - - - 170, 91 - - - 11 - - - Rate Pitch - - - groupBox24 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 14 - 80, 63 @@ -4827,57 +7692,6 @@ 5 - - 6, 107 - - - 170, 91 - - - 12 - - - Rate Roll - - - groupBox25 - - - System.Windows.Forms.GroupBox, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - TabAC - - - 15 - - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 1 - - - ArduCopter - - - TabAC - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 1 - NoControl @@ -4953,6 +7767,9 @@ 2 + + 17, 17 + NoControl @@ -4968,9 +7785,6 @@ GDI+ (old type) - - 17, 17 - OpenGL = Disabled GDI+ = Enabled @@ -6035,87 +8849,6 @@ GDI+ = Enabled 42 - - 4, 22 - - - 3, 3, 3, 3 - - - 722, 434 - - - 2 - - - Planner - - - TabPlanner - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 2 - - - 4, 22 - - - 722, 434 - - - 3 - - - Setup - - - TabSetup - - - System.Windows.Forms.TabPage, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - ConfigTabs - - - 3 - - - 52, 18 - - - 278, 0 - - - 0, 0, 0, 0 - - - 0, 0 - - - 730, 460 - - - 62 - - - ConfigTabs - - - System.Windows.Forms.TabControl, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 - - - $this - - - 2 - 0, 0 @@ -6269,6 +9002,9 @@ GDI+ = Enabled 5 + + 17, 17 + Bottom, Left diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx index 9064d01ee2..c5638dfb2d 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightData.resx @@ -134,7 +134,7 @@ Point Camera Here - 175, 48 + 175, 70 contextMenuStrip1 diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs index 1e0723f1f9..01bbbac698 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/FlightPlanner.cs @@ -290,11 +290,13 @@ namespace ArdupilotMega.GCSViews } float ans; - if (float.TryParse(TXT_homealt.Text, out result) && float.TryParse(cell.Value.ToString(), out ans)) + if (float.TryParse(cell.Value.ToString(), out ans)) { ans = (int)ans; - if (alt != 0) + if (alt != 0) // use passed in value; cell.Value = alt.ToString(); + if (ans == 0) + cell.Value = 50; // online verify height if (isonline && CHK_geheight.Checked) { @@ -314,7 +316,7 @@ namespace ArdupilotMega.GCSViews { cell.Value = (float.Parse(TXT_homealt.Text) + int.Parse(TXT_DefaultAlt.Text)).ToString(); } // is relative and check height - else if (float.TryParse(TXT_homealt.Text, out result) && isonline && CHK_geheight.Checked) + else if (isonline && CHK_geheight.Checked) { alt = (int)getGEAlt(lat, lng); @@ -328,10 +330,6 @@ namespace ArdupilotMega.GCSViews cell.Style.BackColor = Color.LightGreen; } } - else - { - cell.Value = 100 ; - } } cell.DataGridView.EndEdit(); diff --git a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs index 97dd323d1e..ad649e5a85 100644 --- a/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs +++ b/Tools/ArdupilotMegaPlanner/GCSViews/Simulation.cs @@ -348,7 +348,6 @@ namespace ArdupilotMega.GCSViews ArdupilotMega.MainV2.config["REV_pitch"] = CHKREV_pitch.Checked.ToString(); ArdupilotMega.MainV2.config["REV_rudder"] = CHKREV_rudder.Checked.ToString(); ArdupilotMega.MainV2.config["GPSrate"] = GPSrate.Text; - ArdupilotMega.MainV2.config["Xplanes"] = RAD_softXplanes.Checked.ToString(); ArdupilotMega.MainV2.config["MAVrollgain"] = TXT_rollgain.Text; ArdupilotMega.MainV2.config["MAVpitchgain"] = TXT_pitchgain.Text; @@ -389,9 +388,6 @@ namespace ArdupilotMega.GCSViews case "GPSrate": GPSrate.Text = ArdupilotMega.MainV2.config[key].ToString(); break; - case "Xplanes": - RAD_softXplanes.Checked = bool.Parse(ArdupilotMega.MainV2.config[key].ToString()); - break; case "MAVrollgain": TXT_rollgain.Text = ArdupilotMega.MainV2.config[key].ToString(); break; diff --git a/Tools/ArdupilotMegaPlanner/MAVLink.cs b/Tools/ArdupilotMegaPlanner/MAVLink.cs index 0f98ceb923..6581e2e9df 100644 --- a/Tools/ArdupilotMegaPlanner/MAVLink.cs +++ b/Tools/ArdupilotMegaPlanner/MAVLink.cs @@ -2006,7 +2006,7 @@ namespace ArdupilotMega } catch (Exception e) { Console.WriteLine("MAVLink readpacket read error: " + e.Message); break; } - if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M') // out of sync + if (temp[0] != 254 && temp[0] != 'U' || lastbad[0] == 'I' && lastbad[1] == 'M' || lastbad[1] == 'G') // out of sync { if (temp[0] >= 0x20 && temp[0] <= 127 || temp[0] == '\n' || temp[0] == '\r') { diff --git a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs index 7ac34587f4..13bdc50258 100644 --- a/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs +++ b/Tools/ArdupilotMegaPlanner/Properties/AssemblyInfo.cs @@ -34,5 +34,5 @@ using System.Resources; // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] -[assembly: AssemblyFileVersion("1.1.29")] +[assembly: AssemblyFileVersion("1.1.30")] [assembly: NeutralResourcesLanguageAttribute("")] diff --git a/Tools/ArdupilotMegaPlanner/Script.cs b/Tools/ArdupilotMegaPlanner/Script.cs index 016bde55bd..df94c21309 100644 --- a/Tools/ArdupilotMegaPlanner/Script.cs +++ b/Tools/ArdupilotMegaPlanner/Script.cs @@ -10,8 +10,8 @@ namespace ArdupilotMega { DateTime timeout = DateTime.Now; List items = new List(); - Microsoft.Scripting.Hosting.ScriptEngine engine; - Microsoft.Scripting.Hosting.ScriptScope scope; + static Microsoft.Scripting.Hosting.ScriptEngine engine; + static Microsoft.Scripting.Hosting.ScriptScope scope; // keeps history MAVLink.__mavlink_rc_channels_override_t rc = new MAVLink.__mavlink_rc_channels_override_t(); diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs b/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs new file mode 100644 index 0000000000..62d2900dd9 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/SerialInput.Designer.cs @@ -0,0 +1,135 @@ +namespace ArdupilotMega +{ + partial class SerialInput + { + /// + /// Required designer variable. + /// + private System.ComponentModel.IContainer components = null; + + /// + /// Clean up any resources being used. + /// + /// true if managed resources should be disposed; otherwise, false. + protected override void Dispose(bool disposing) + { + if (disposing && (components != null)) + { + components.Dispose(); + } + base.Dispose(disposing); + } + + #region Windows Form Designer generated code + + /// + /// Required method for Designer support - do not modify + /// the contents of this method with the code editor. + /// + private void InitializeComponent() + { + System.ComponentModel.ComponentResourceManager resources = new System.ComponentModel.ComponentResourceManager(typeof(SerialInput)); + this.CMB_serialport = new System.Windows.Forms.ComboBox(); + this.BUT_connect = new ArdupilotMega.MyButton(); + this.CMB_baudrate = new System.Windows.Forms.ComboBox(); + this.label1 = new System.Windows.Forms.Label(); + this.LBL_location = new System.Windows.Forms.Label(); + this.textBox1 = new System.Windows.Forms.TextBox(); + this.SuspendLayout(); + // + // CMB_serialport + // + this.CMB_serialport.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.CMB_serialport.FormattingEnabled = true; + this.CMB_serialport.Location = new System.Drawing.Point(13, 13); + this.CMB_serialport.Name = "CMB_serialport"; + this.CMB_serialport.Size = new System.Drawing.Size(121, 21); + this.CMB_serialport.TabIndex = 0; + // + // BUT_connect + // + this.BUT_connect.Location = new System.Drawing.Point(279, 12); + this.BUT_connect.Name = "BUT_connect"; + this.BUT_connect.Size = new System.Drawing.Size(75, 23); + this.BUT_connect.TabIndex = 1; + this.BUT_connect.Text = "Connect"; + this.BUT_connect.UseVisualStyleBackColor = true; + this.BUT_connect.Click += new System.EventHandler(this.BUT_connect_Click); + // + // CMB_baudrate + // + this.CMB_baudrate.DropDownStyle = System.Windows.Forms.ComboBoxStyle.DropDownList; + this.CMB_baudrate.FormattingEnabled = true; + this.CMB_baudrate.Items.AddRange(new object[] { + "4800", + "9600", + "14400", + "19200", + "28800", + "38400", + "57600", + "115200"}); + this.CMB_baudrate.Location = new System.Drawing.Point(140, 12); + this.CMB_baudrate.Name = "CMB_baudrate"; + this.CMB_baudrate.Size = new System.Drawing.Size(121, 21); + this.CMB_baudrate.TabIndex = 2; + // + // label1 + // + this.label1.AutoSize = true; + this.label1.Location = new System.Drawing.Point(90, 47); + this.label1.Name = "label1"; + this.label1.Size = new System.Drawing.Size(187, 13); + this.label1.TabIndex = 3; + this.label1.Text = "Pick the Nmea gps port and baud rate\r\n"; + // + // LBL_location + // + this.LBL_location.AutoSize = true; + this.LBL_location.Font = new System.Drawing.Font("Microsoft Sans Serif", 14.25F, System.Drawing.FontStyle.Regular, System.Drawing.GraphicsUnit.Point, ((byte)(0))); + this.LBL_location.Location = new System.Drawing.Point(9, 78); + this.LBL_location.Name = "LBL_location"; + this.LBL_location.Size = new System.Drawing.Size(50, 24); + this.LBL_location.TabIndex = 4; + this.LBL_location.Text = "0,0,0"; + // + // textBox1 + // + this.textBox1.Enabled = false; + this.textBox1.Location = new System.Drawing.Point(19, 126); + this.textBox1.Multiline = true; + this.textBox1.Name = "textBox1"; + this.textBox1.Size = new System.Drawing.Size(335, 133); + this.textBox1.TabIndex = 5; + this.textBox1.Text = resources.GetString("textBox1.Text"); + // + // SerialInput + // + this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); + this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; + this.ClientSize = new System.Drawing.Size(369, 300); + this.Controls.Add(this.textBox1); + this.Controls.Add(this.LBL_location); + this.Controls.Add(this.label1); + this.Controls.Add(this.CMB_baudrate); + this.Controls.Add(this.BUT_connect); + this.Controls.Add(this.CMB_serialport); + this.Icon = ((System.Drawing.Icon)(resources.GetObject("$this.Icon"))); + this.Name = "SerialInput"; + this.Text = "Follow Me"; + this.FormClosing += new System.Windows.Forms.FormClosingEventHandler(this.SerialOutput_FormClosing); + this.ResumeLayout(false); + this.PerformLayout(); + + } + + #endregion + + private System.Windows.Forms.ComboBox CMB_serialport; + private MyButton BUT_connect; + private System.Windows.Forms.ComboBox CMB_baudrate; + private System.Windows.Forms.Label label1; + private System.Windows.Forms.Label LBL_location; + private System.Windows.Forms.TextBox textBox1; + } +} \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.cs b/Tools/ArdupilotMegaPlanner/SerialInput.cs new file mode 100644 index 0000000000..d7ebd2c98d --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/SerialInput.cs @@ -0,0 +1,224 @@ +using System; +using System.Collections.Generic; +using System.ComponentModel; +using System.Data; +using System.Drawing; +using System.Linq; +using System.Text; +using System.Windows.Forms; +using System.IO.Ports; + +namespace ArdupilotMega +{ + public partial class SerialInput : Form + { + System.Threading.Thread t12; + static bool threadrun = false; + static internal SerialPort comPort = new SerialPort(); + static internal PointLatLngAlt lastgotolocation = new PointLatLngAlt(0, 0, 0, "Goto last"); + static internal PointLatLngAlt gotolocation = new PointLatLngAlt(0, 0, 0, "Goto"); + static internal int intalt = 100; + + public SerialInput() + { + InitializeComponent(); + + CMB_serialport.DataSource = SerialPort.GetPortNames(); + + if (threadrun) + { + BUT_connect.Text = "Stop"; + } + } + + private void BUT_connect_Click(object sender, EventArgs e) + { + if (comPort.IsOpen) + { + threadrun = false; + comPort.Close(); + BUT_connect.Text = "Connect"; + } + else + { + try + { + comPort.PortName = CMB_serialport.Text; + } + catch { MessageBox.Show("Invalid PortName"); return; } + try { + comPort.BaudRate = int.Parse(CMB_baudrate.Text); + } catch {MessageBox.Show("Invalid BaudRate"); return;} + try { + comPort.Open(); + } catch {MessageBox.Show("Error Connecting\nif using com0com please rename the ports to COM??"); return;} + + + string alt = "100"; + + if (MainV2.cs.firmware == MainV2.Firmwares.ArduCopter2) + { + alt = (10 * MainV2.cs.multiplierdist).ToString("0"); + } + else + { + alt = (100 * MainV2.cs.multiplierdist).ToString("0"); + } + if (DialogResult.Cancel == Common.InputBox("Enter Alt", "Enter Alt", ref alt)) + return; + + intalt = (int)(100 * MainV2.cs.multiplierdist); + if (!int.TryParse(alt, out intalt)) + { + MessageBox.Show("Bad Alt"); + return; + } + + t12 = new System.Threading.Thread(new System.Threading.ThreadStart(mainloop)) + { + IsBackground = true, + Name = "Nmea Input" + }; + t12.Start(); + + BUT_connect.Text = "Stop"; + } + } + + void mainloop() + { + + + threadrun = true; + int counter = 0; + while (threadrun) + { + try + { + string line = comPort.ReadLine(); + + + //string line = string.Format("$GP{0},{1:HHmmss},{2},{3},{4},{5},{6},{7},{8},{9},{10},{11},{12},{13},", "GGA", DateTime.Now.ToUniversalTime(), Math.Abs(lat * 100), MainV2.cs.lat < 0 ? "S" : "N", Math.Abs(lng * 100), MainV2.cs.lng < 0 ? "W" : "E", MainV2.cs.gpsstatus, MainV2.cs.satcount, MainV2.cs.gpshdop, MainV2.cs.alt, "M", 0, "M", ""); + if (line.StartsWith("$GPGGA")) // + { + int c1 = line.IndexOf(',',0)+ 1; + int c2 = line.IndexOf(',', c1) + 1; + int c3 = line.IndexOf(',', c2) + 1; + int c4 = line.IndexOf(',', c3 ) + 1; + int c5 = line.IndexOf(',', c4 ) + 1; + int c6 = line.IndexOf(',', c5 ) + 1; + int c7 = line.IndexOf(',', c6 ) + 1; + int c8 = line.IndexOf(',', c7 ) + 1; + int c9 = line.IndexOf(',', c8 ) + 1; + int c10 = line.IndexOf(',', c9 ) + 1; + int c11 = line.IndexOf(',', c10 ) + 1; + int c12 = line.IndexOf(',', c11) + 1; + + gotolocation.Lat = double.Parse(line.Substring(c2, c3 - c2 - 1)) / 100.0; + + gotolocation.Lat = (int)gotolocation.Lat + ((gotolocation.Lat - (int)gotolocation.Lat) / 0.60); + + if (line.Substring(c3, c4 - c3 - 1) == "S") + gotolocation.Lat *= -1; + + gotolocation.Lng = double.Parse(line.Substring(c4, c5 - c4 - 1)) / 100.0; + + gotolocation.Lng = (int)gotolocation.Lng + ((gotolocation.Lng - (int)gotolocation.Lng) / 0.60); + + if (line.Substring(c5, c6 - c5 - 1) == "W") + gotolocation.Lng *= -1; + + gotolocation.Alt = intalt; // double.Parse(line.Substring(c9, c10 - c9 - 1)) + + + } + + + if (counter % 10 == 0 && gotolocation.Lat != 0 && gotolocation.Lng != 0 && gotolocation.Alt != 0) // 200 * 10 = 2 sec /// lastgotolocation != gotolocation && + { + Console.WriteLine("Sending follow wp " +DateTime.Now.ToString("h:MM:ss")+" "+ gotolocation.Lat + " " + gotolocation.Lng + " " +gotolocation.Alt); + lastgotolocation = new PointLatLngAlt(gotolocation); + + Locationwp gotohere = new Locationwp(); + + gotohere.id = (byte)MAVLink.MAV_CMD.WAYPOINT; + gotohere.alt = (float)(gotolocation.Alt); + gotohere.lat = (float)(gotolocation.Lat); + gotohere.lng = (float)(gotolocation.Lng); + + try + { + updateLocationLabel(gotohere); + } + catch { } + + if (MainV2.comPort.BaseStream.IsOpen && MainV2.givecomport == false) + { + try + { + MainV2.givecomport = true; + + MainV2.comPort.setWP(gotohere, 0, MAVLink.MAV_FRAME.MAV_FRAME_GLOBAL_RELATIVE_ALT, (byte)2); + + MainV2.givecomport = false; + } + catch { MainV2.givecomport = false; } + } + } + + System.Threading.Thread.Sleep(200); + counter++; + } + catch { System.Threading.Thread.Sleep(2000); } + } + } + + private void updateLocationLabel(Locationwp plla) + { + this.BeginInvoke((MethodInvoker)delegate + { + LBL_location.Text = gotolocation.Lat + " " + gotolocation.Lng + " " + gotolocation.Alt; + } + ); + + } + + private void SerialOutput_FormClosing(object sender, FormClosingEventArgs e) + { + } + + // Calculates the checksum for a sentence + string GetChecksum(string sentence) + { + // Loop through all chars to get a checksum + int Checksum = 0; + foreach (char Character in sentence.ToCharArray()) + { + switch (Character) + { + case '$': + // Ignore the dollar sign + break; + case '*': + // Stop processing before the asterisk + continue; + default: + // Is this the first value for the checksum? + if (Checksum == 0) + { + // Yes. Set the checksum to the value + Checksum = Convert.ToByte(Character); + } + else + { + // No. XOR the checksum with this character's value + Checksum = Checksum ^ Convert.ToByte(Character); + } + break; + } + } + // Return the checksum formatted as a two-character hexadecimal + return Checksum.ToString("X2"); + } + + } +} diff --git a/Tools/ArdupilotMegaPlanner/SerialInput.resx b/Tools/ArdupilotMegaPlanner/SerialInput.resx new file mode 100644 index 0000000000..62276c20c7 --- /dev/null +++ b/Tools/ArdupilotMegaPlanner/SerialInput.resx @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + text/microsoft-resx + + + 2.0 + + + System.Resources.ResXResourceReader, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + System.Resources.ResXResourceWriter, System.Windows.Forms, Version=2.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089 + + + What this does. +1. gets the current gps coords from a nmea gps. +2. sends a guided mode WP to the AP every 2 seconds. + +How to use it +1. connect to ap. +2. take off, test guided mode is working. +3. open this and pick your comport, and baud rate for your nmea gps. +4. it should now be following you. + + + + + AAABAAEAICAAAAEAIACoEAAAFgAAACgAAAAgAAAAQAAAAAEAIAAAAAAAABAAABILAAASCwAAAAAAAAAA + AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADOxkjAtnoOAKpJ4vyiK + c+8nh3D/J4Zv/yeHcP8oi3PvKpJ4vy6fg4AzsZIwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADjGo2AyspPfLZ+D/yiQ + d/8hlXj/G6F9/xeqg/8XqYL/GKqD/xuhfv8ilnn/KZB3/y2fhP8yspPfN8ajYAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAADvRrDA1vpzfL6uN/yel + hP8XvJD/DMyY/wfQl/8FzJP/A8qS/wPJkf8EypL/BsyU/wnRmP8PzZn/Gb2R/yemhP8tqoz/Mb2a3zbQ + qkAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAA4y6ZgMbWV/yin + iP8WwZP/Btqf/wDPlf8AyI7/A8aP/yfNnv9T2LP/UNax/03XsP8506b/G8ya/wHKkf8F0Zf/CNuf/xLB + kv8fpYT/J7KQ/y7IomAAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAAAAAANcajny+w + kf8hqoj/CNSd/wDRlf8Axor/Hcyd/3Lhwf+p7Nj/o+vV/57m0/+X5dD/k+TN/4/jzf+K5Mz/fuHH/0PW + rf8HzJT/ANCT/wDRlv8OpX//HayI/yrFn58AAAAAAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAADDC + nmAtro7/H62J/wPWmv8Ay47/AMaO/3XhxP+e6tT/mObP/5Pjy/+Q4sr/jODJ/4ffx/+C3MT/f9vC/3nb + wf9y2r7/adq7/2DauP8ZzZv/Fdae/8T/9/9WxKj/HKuI/y7IomAAAAAAAAAAAAAAAAD///8AAAAAAAAA + AAAiuZMwKKyM/x6ohf8C1Zr/AMmL/wHGjv+49OL///////////9+3ML/f9zD/4Dcwv9+28L/e9rA/3bZ + vv9w1rr/Z9S4/17Rs/9Qz63/Qcyn/3LewP////////////n///8MpH7/JbKP/zXQqUAAAAAAAAAAAP// + /wAAAAAAAAAAABymhN8dnn//BNGa/wDKjP8AxY3/sfHf/////////////////2nXt/9w1rv/c9e8/3TX + vP9x17z/a9W5/2TTtf9Y0K//SMyp/zXFoP9i07X/////////////////f/LR/wDQlf8epYT/Mb2a3wAA + AAAAAAAA////AAAAAAADlnJgFZR1/wq4iv8AzpH/AMCD/4rmzf//////////////////////WdGv/2PU + tf9p1rf/atS4/2nUtv9i0rT/Vc+u/0fKpv8zxZz/Ws+w//////////////////////8GyJL/ANCS/xLB + kv8tq4z/OMajYAAAAAD///8AAAAAAACHZt8NkW//ANKV/wDChP9i27r//////////////////////9Dx + 6P9MzKn/Vc+v/17Rsv9g0rP/XNCx/1XNrv9Fyaf/McSd/1fPr///////////////////////QM2m/ynK + oP8JzJX/C9yh/ymmhf80spPfAAAAAP///wAAcUwwAHtc/wCrfP8AyIv/AMKK//////////////////// + /////////////5Dgyv9Gyqb/TMyq/07Nq/9MzKn/Qcmj/y/Fnf9Wzq3//////////////////////57k + 0v8av5T/Lceg/yzOo/8M05v/Hr6T/zCghf80spIw////AABoRYAAclT/AL2H/wDBhf9R1rL///////// + ////////4vfw//////////////////H8+P9KzKn/Ocah/zTFnv8qwpj/Us2t//////////////////// + ////////DLqM/yDBlv8wxp//OM6m/xPPm/8Xz53/LZF5/y+fg4////8AAGNAvwB7Wf8Aw4j/ALyC/4bj + yP+g5tL/g93E/2HSsv9Pzqz/Us6s//////////////////////9Yzq//Gr2S/0jLp/////////////// + /////////////yrDm/8SvI//JMGY/zDHn/81zKT/Is2e/xTUnf8nl3v/LJJ5v////wAAXz3vAIlg/wDA + hf8AuoD/quzZ/5Hjyv9628D/ada2/1jRsP9Jy6f/a9a4//////////////////////+Y4s7///////// + //////////////////+c4tD/AbaH/xW8kf8jwZj/LcWd/y/Jn/8kzJ3/E9Ca/yGjgf8ri3Tv////AABd + PP8Ak2b/AL6D/w/Ekv+m6tf/j+HJ/3vawP9p1rf/W9Gx/0rNqf85yJ//Nsaf//////////////////// + /////////////////////////////wCwe/8AtoT/ELqP/xu+k/8jwZj/KMeb/yHKm/8QzZf/HqyG/ymI + cf////8AAF07/wCSZP8AvYL/GMWU/6Dn1P+K38f/ddi+/27Wuf+E3MX/leHN/6fm1f+l5tX/neLQ//// + ////////////////////////////////////////j9/J/27Vuv9Tzq7/JsKY/xa/kv8aw5T/FcaW/wvL + lf8aqoT/J4dw/////wAAXTv/AJFk/wC9gP8GwY3/mObQ/5rkz/+26dv/y/Hl/8Dt3/+06tz/pebV/5bg + zP+g5NL//////////////v///f7+//7+/v//////7fn2////////////tOnb/6Ll0v+v6Nj/jeDI/zXK + o/8IxJD/BMqS/xaqgv8lh2//////AABeO+8AgVf/AL1//wDBif/R9uv/1PPq/8Tv5P+36t3/rujY/6Lk + 0v+U4cv/jt7J//j8+///////+/38//f8+//2+/r/+Pz7//3+/v/m9/P/9Pv6//D6+P9/28L/jd7J/5jj + z/+h5dL/qOvX/4Hmyf8f1J//E596/yOJcO////8AAGA8vwB3U/8p06P/hufM/8Ty5f/D7+T/s+vb/6bm + 1P+c4c//j9/K/4vcyP/t+fb///7///j8/P/0+/r/8vr5//P7+f/1+/r/+/39///////i9fL/ZNO1/3HW + vP992sH/htzG/4vhyv+S5dD/mO7W/6X74v80noT/Io90v////wAAZkCAAHla/33ny/945cb/nunV/7Xr + 3v+l5tT/luDN/4ndxv992cL/1vLq//v9/P/1+/n/8vv4//L69//z+/j/9Pv5/7Xo2//x+vn///////// + //+y59n/aNS3/3LWvP932r//fNzD/4Ljyf+J7ND/l/bd/yORdf8knH6A////AABuRzAAdlT/Xc6x/23o + xv9s4MH/qurZ/5jiz/+I3cb/edjA/8ju5f/3/Pv/8vv4//H6+P/y+/j/6/f0/7np3v/7/fz//v7+/6fk + 1f+56tz///////////9h0bT/aNW4/23Wu/9v3L//dOLG/37w0f9m1rn/Hpt8/ymujTD///8AAAAAAACD + X98po4X/Z+7K/1vgvP+A4sf/jOHK/3rZwv+r59f/9Pv6/+/69//v+vf/8vr4/9fy6/9n0rf/VM6t/6Di + 0v/N7+f/adO4/1PMrf9t1Lr/i9zI/1/Rs/9h0rX/ZNe4/2bbvf9s5sb/ePfV/z2ylf8lrozfAAAAAP// + /wAAAAAAAJNsYAWQbf9U1rP/Vee//0rYsf993sb/pebV//P7+v/s+Pb/6/f1/+749v+s5tj/Vc2u/1jP + r/9ZzrD/btW5/1bOr/9Wza//Vs6v/1fOr/9Z0LD/WdCy/1vTtP9d1rX/Xt+8/2btyP9k4L//IaaF/y7D + nmAAAAAA////AAAAAAAAAAAAD6J9zyCjgv9S68L/P9+0/2Pevv/5////7/v6/+v59//j9/L/gtvF/1PN + r/9Wz7D/Wc+x/1nQsf9Zz7H/WM6w/1fPsP9UzrD/VM+w/1TPrv9U0a//U9Oy/1Tatv9Z5sD/Y/LL/zSx + lP8qupbPAAAAAAAAAAD///8AAAAAAAAAAAAYto4wGaeE/y23lP8+5rn/6/////j////w//3/ve/i/2bV + uP9Tzq7/Vc+v/1jPsP9Z0LL/WM+w/1fOsf9Wz7D/Us2w/1HOrf9Qzq3/T9Cu/0zSr/9M2LP/TeC5/1bt + xP9HxaX/KLKQ/zTPqDAAAAAAAAAAAP///wAAAAAAAAAAAAAAAAAkvpdgG6iF/y++m//e/////P///3rl + yf9G0K3/VdKy/1bPsf9Wz7H/Vs6w/1bPsP9Sza//Ucyu/0/Nrf9NzKz/S82s/0fOrP9G0a7/QdWv/0Le + tP9I6L7/Q8Ok/yitjP8yyKJgAAAAAAAAAAAAAAAA////AAAAAAAAAAAAAAAAAAAAAAAmwJlgG6iF/yK3 + kP8k3q7/H9el/x7Pn/8tzKT/Q9Cs/1HQsP9Q0K7/TM6u/0nMrf9Hzaz/RMyp/0LNqf8+zqn/ONGo/zTV + qf833rD/O+S4/zvCof8orIv/MMSfYAAAAAAAAAAAAAAAAAAAAAD///8AAAAAAAAAAAAAAAAAAAAAAAAA + AAAkvpdgG6iE/xukgv8gy53/HNql/xzRn/8czJz/HcmZ/yXJnP8qyp7/Lcqg/yzLn/8nypz/JMqc/yTO + n/8l1KT/KN2r/y3Tpv8nq4n/JaqJ/yzAm2AAAAAAAAAAAAAAAAAAAAAAAAAAAP///wAAAAAAAAAAAAAA + AAAAAAAAAAAAAAAAAAAato8wFKN/zxCScv8RnHn/DbqM/wjIlP8GyZT/BsaS/wbFkf8GxZH/B8WR/wfH + k/8IypX/DMmV/xG3jP8WoX3/Fph2/xqkgs8ft5EwAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA////AAAA + AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAJVvYACGZM8Aelr/AHlZ/wCFX/8AiWL/AJlr/wCb + bP8AlGf/AI5k/wB/W/8AeFj/AHtb/wCHZd8ClXBgAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAD///8AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAABwSzAAaESAAGI/vwBf + Pd8AXTz/AF08/wBdPP8AXz3fAGJAvwBoRIAAcUswAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA + AAAAAAAAAAAAAP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP// + /wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP///wD///8A////AP// + /wD///8A////AP///wD///8A/+AD//+AAP/+AAA//AAAH/gAAA/wAAAH4AAAA+AAAAPAAAABwAAAAYAA + AACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAgAAAAIAAAACAAAAAwAAAAcAAAAHgAAAD4AAAA/AA + AAf4AAAP/AAAH/4AAD//gAD//+AD//////8= + + + \ No newline at end of file diff --git a/Tools/ArdupilotMegaPlanner/temp.Designer.cs b/Tools/ArdupilotMegaPlanner/temp.Designer.cs index 734a6833ad..d09cc97373 100644 --- a/Tools/ArdupilotMegaPlanner/temp.Designer.cs +++ b/Tools/ArdupilotMegaPlanner/temp.Designer.cs @@ -45,6 +45,7 @@ this.BUT_clearcustommaps = new ArdupilotMega.MyButton(); this.BUT_lang_edit = new ArdupilotMega.MyButton(); this.BUT_georefimage = new ArdupilotMega.MyButton(); + this.BUT_follow_me = new ArdupilotMega.MyButton(); this.SuspendLayout(); // // button1 @@ -212,11 +213,22 @@ this.BUT_georefimage.Text = "Geo ref images"; this.BUT_georefimage.Click += new System.EventHandler(this.BUT_georefimage_Click); // + // BUT_follow_me + // + this.BUT_follow_me.Location = new System.Drawing.Point(525, 164); + this.BUT_follow_me.Name = "BUT_follow_me"; + this.BUT_follow_me.Size = new System.Drawing.Size(75, 23); + this.BUT_follow_me.TabIndex = 17; + this.BUT_follow_me.Text = "Follow Me"; + this.BUT_follow_me.UseVisualStyleBackColor = true; + this.BUT_follow_me.Click += new System.EventHandler(this.BUT_follow_me_Click); + // // temp // this.AutoScaleDimensions = new System.Drawing.SizeF(6F, 13F); this.AutoScaleMode = System.Windows.Forms.AutoScaleMode.Font; this.ClientSize = new System.Drawing.Size(731, 281); + this.Controls.Add(this.BUT_follow_me); this.Controls.Add(this.BUT_georefimage); this.Controls.Add(this.BUT_lang_edit); this.Controls.Add(this.BUT_clearcustommaps); @@ -261,6 +273,7 @@ private MyButton BUT_clearcustommaps; private MyButton BUT_lang_edit; private MyButton BUT_georefimage; + private MyButton BUT_follow_me; //private SharpVectors.Renderers.Forms.SvgPictureBox svgPictureBox1; } diff --git a/Tools/ArdupilotMegaPlanner/temp.cs b/Tools/ArdupilotMegaPlanner/temp.cs index c59d018d59..394ab8b1d5 100644 --- a/Tools/ArdupilotMegaPlanner/temp.cs +++ b/Tools/ArdupilotMegaPlanner/temp.cs @@ -871,5 +871,12 @@ namespace ArdupilotMega { new georefimage().Show(); } + + private void BUT_follow_me_Click(object sender, EventArgs e) + { + SerialInput si = new SerialInput(); + MainV2.fixtheme((Form)si); + si.Show(); + } } } From 1cbbe7bc7d6aee285aa0c9a44dfb10213a0ebf5c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 29 Jan 2012 15:59:22 +0900 Subject: [PATCH 15/18] Optflow - minor fix to D term defaulting --- ArduCopter/Parameters.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 0dc92f2d19..9d356f9877 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -410,8 +410,8 @@ public: pid_nav_lon (k_param_pid_nav_lon, PSTR("NAV_LON_"), NAV_P, NAV_I, NAV_D, NAV_IMAX * 100), pid_throttle (k_param_pid_throttle, PSTR("THR_RATE_"), THROTTLE_P, THROTTLE_I, THROTTLE_D, THROTTLE_IMAX), - pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_IMAX * 100), - pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100), + pid_optflow_roll (k_param_pid_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), + pid_optflow_pitch (k_param_pid_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D,OPTFLOW_IMAX * 100), // PI controller group key name initial P initial I initial imax From cff6799d185ef41885978a04045c70cd0d0c2176 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 29 Jan 2012 21:14:37 +0900 Subject: [PATCH 16/18] TradHeli - fix to servo limits. They had been unnecessarily limited to the collective pitch's min and max but actually there are cases (for example when the swash is leaning over 45degrees) where one servo goes well beyond the collective pitch's min or max. --- ArduCopter/heli.pde | 93 +++++++++++++++++---------------------------- 1 file changed, 34 insertions(+), 59 deletions(-) diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index 810d57e605..e2900234e3 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -7,6 +7,7 @@ static bool heli_swash_initialised = false; static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1000) +static float heli_coll_scalar = 1; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500) // heli_servo_averaging: // 0 or 1 = no averaging, 250hz @@ -20,27 +21,12 @@ static int heli_throttle_mid = 0; // throttle mid point in pwm form (i.e. 0 ~ 1 static void heli_reset_swash() { // free up servo ranges - if( g.heli_servo_1.get_reverse() ) { - g.heli_servo_1.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); - }else{ - g.heli_servo_1.radio_min = g.rc_3.radio_min + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = g.rc_3.radio_max + (g.heli_servo_1.radio_trim-1500); - } - if( g.heli_servo_2.get_reverse() ) { - g.heli_servo_2.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500); - }else{ - g.heli_servo_2.radio_min = g.rc_3.radio_min + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = g.rc_3.radio_max + (g.heli_servo_2.radio_trim-1500); - } - if( g.heli_servo_3.get_reverse() ) { - g.heli_servo_3.radio_min = 3000 - g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = 3000 - g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); - }else{ - g.heli_servo_3.radio_min = g.rc_3.radio_min + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = g.rc_3.radio_max + (g.heli_servo_3.radio_trim-1500); - } + g.heli_servo_1.radio_min = 1000; + g.heli_servo_1.radio_max = 2000; + g.heli_servo_2.radio_min = 1000; + g.heli_servo_2.radio_max = 2000; + g.heli_servo_3.radio_min = 1000; + g.heli_servo_3.radio_max = 2000; // pitch factors heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); @@ -52,6 +38,9 @@ static void heli_reset_swash() heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); + // set throttle scaling + heli_coll_scalar = ((float)(g.rc_3.radio_max - g.rc_3.radio_min))/1000.0; + // we must be in set-up mode so mark swash as uninitialised heli_swash_initialised = false; } @@ -60,7 +49,6 @@ static void heli_reset_swash() static void heli_init_swash() { int i; - float coll_range_comp = 1; // factor to negate collective range's effect on roll & pitch range // swash servo initialisation g.heli_servo_1.set_range(0,1000); @@ -75,45 +63,29 @@ static void heli_init_swash() } g.heli_coll_mid = constrain(g.heli_coll_mid, g.heli_coll_min, g.heli_coll_max); - // calculate compensation for collective range on roll & pitch range - if( g.heli_coll_max - g.heli_coll_min > 100 ) - coll_range_comp = 1000 / (g.heli_coll_max - g.heli_coll_min); - // calculate throttle mid point - heli_throttle_mid = (g.heli_coll_mid-g.heli_coll_min)*(1000.0/(g.heli_coll_max-g.heli_coll_min)); + heli_throttle_mid = ((float)(g.heli_coll_mid-g.heli_coll_min))/((float)(g.heli_coll_max-g.heli_coll_min))*1000.0; + + // determine scalar throttle input + heli_coll_scalar = ((float)(g.heli_coll_max-g.heli_coll_min))/1000.0; // pitch factors - heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)) * coll_range_comp; - heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)) * coll_range_comp; - heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)) * coll_range_comp; + heli_pitchFactor[CH_1] = cos(radians(g.heli_servo1_pos - g.heli_phase_angle)); + heli_pitchFactor[CH_2] = cos(radians(g.heli_servo2_pos - g.heli_phase_angle)); + heli_pitchFactor[CH_3] = cos(radians(g.heli_servo3_pos - g.heli_phase_angle)); // roll factors - heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)) * coll_range_comp; - heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)) * coll_range_comp; - heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)) * coll_range_comp; + heli_rollFactor[CH_1] = cos(radians(g.heli_servo1_pos + 90 - g.heli_phase_angle)); + heli_rollFactor[CH_2] = cos(radians(g.heli_servo2_pos + 90 - g.heli_phase_angle)); + heli_rollFactor[CH_3] = cos(radians(g.heli_servo3_pos + 90 - g.heli_phase_angle)); // servo min/max values - if( g.heli_servo_1.get_reverse() ) { - g.heli_servo_1.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); - }else{ - g.heli_servo_1.radio_min = g.heli_coll_min + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_1.radio_max = g.heli_coll_max + (g.heli_servo_1.radio_trim-1500); - } - if( g.heli_servo_2.get_reverse() ) { - g.heli_servo_2.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_2.radio_trim-1500); - }else{ - g.heli_servo_2.radio_min = g.heli_coll_min + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_2.radio_max = g.heli_coll_max + (g.heli_servo_2.radio_trim-1500); - } - if( g.heli_servo_3.get_reverse() ) { - g.heli_servo_3.radio_min = 3000 - g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = 3000 - g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); - }else{ - g.heli_servo_3.radio_min = g.heli_coll_min + (g.heli_servo_3.radio_trim-1500); - g.heli_servo_3.radio_max = g.heli_coll_max + (g.heli_servo_3.radio_trim-1500); - } + g.heli_servo_1.radio_min = 1000; + g.heli_servo_1.radio_max = 2000; + g.heli_servo_2.radio_min = 1000; + g.heli_servo_2.radio_max = 2000; + g.heli_servo_3.radio_min = 1000; + g.heli_servo_3.radio_max = 2000; // reset the servo averaging for( i=0; i<=3; i++ ) @@ -148,13 +120,15 @@ static void heli_move_servos_to_mid() // static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_out) { - int yaw_offset = 0; + int yaw_offset = 0; + int coll_out_scaled; if( g.heli_servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)? // check if we need to freeup the swash if( heli_swash_initialised ) { heli_reset_swash(); } + coll_out_scaled = coll_out * heli_coll_scalar + g.rc_3.radio_min - 1000; }else{ // regular flight mode // check if we need to reinitialise the swash @@ -166,19 +140,20 @@ static void heli_move_swash(int roll_out, int pitch_out, int coll_out, int yaw_o roll_out = constrain(roll_out, (int)-g.heli_roll_max, (int)g.heli_roll_max); pitch_out = constrain(pitch_out, (int)-g.heli_pitch_max, (int)g.heli_pitch_max); coll_out = constrain(coll_out, 0, 1000); + coll_out_scaled = coll_out * heli_coll_scalar + g.heli_coll_min - 1000; // rudder feed forward based on collective #if HIL_MODE == HIL_MODE_DISABLED // don't do rudder feed forward in simulator if( !g.heli_ext_gyro_enabled ) { - yaw_offset = g.heli_coll_yaw_effect * (coll_out - g.heli_coll_mid); + yaw_offset = g.heli_coll_yaw_effect * (coll_out_scaled - g.heli_coll_mid); } #endif } // swashplate servos - g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out + (g.heli_servo_1.radio_trim-1500); - g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out + (g.heli_servo_2.radio_trim-1500); - g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out + (g.heli_servo_3.radio_trim-1500); + g.heli_servo_1.servo_out = (heli_rollFactor[CH_1] * roll_out + heli_pitchFactor[CH_1] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_1.radio_trim-1500); + g.heli_servo_2.servo_out = (heli_rollFactor[CH_2] * roll_out + heli_pitchFactor[CH_2] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_2.radio_trim-1500); + g.heli_servo_3.servo_out = (heli_rollFactor[CH_3] * roll_out + heli_pitchFactor[CH_3] * pitch_out)/10 + coll_out_scaled + (g.heli_servo_3.radio_trim-1500); g.heli_servo_4.servo_out = yaw_out + yaw_offset; // use servo_out to calculate pwm_out and radio_out From af0b0d5a7f712f5e6a07b3c227f4fcc356e2ed98 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 29 Jan 2012 16:14:56 -0800 Subject: [PATCH 17/18] made Loiter over ride exit dependent not on speed, but centered sticks. --- ArduCopter/ArduCopter.pde | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 2279a72a08..ad6f19c6e4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1686,15 +1686,15 @@ static void update_navigation() // regain hold when the speed goes down to .5m/s if(loiter_override){ // this sets the copter to not try and nav while we control it - wp_control = NO_NAV_MODE; + wp_control = NO_NAV_MODE; // reset LOITER to current position - next_WP.lat = current_loc.lat; - next_WP.lng = current_loc.lng; + next_WP.lat = current_loc.lat; + next_WP.lng = current_loc.lng; - if(g_gps->ground_speed < 50){ - loiter_override = false; - wp_control = LOITER_MODE; + if((g.rc_2.control_in + g.rc_1.control_in) == 0){ + loiter_override = false; + wp_control = LOITER_MODE; } }else{ wp_control = LOITER_MODE; From 370d633f51bbdc2d7957206a587b8e99f147cb41 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 29 Jan 2012 16:27:13 -0800 Subject: [PATCH 18/18] Lowering the throttle_P values by default --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 2fc2e3e66e..cbe6cab5e3 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -677,7 +677,7 @@ // RATE control #ifndef THROTTLE_P -# define THROTTLE_P 0.5 // +# define THROTTLE_P 0.35 // #endif #ifndef THROTTLE_I # define THROTTLE_I 0.0 //