From 9a0539f39e4f6a8901883c7bf47ff3bdecd3d1b6 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 09:36:36 -0800 Subject: [PATCH 01/18] tuned down Alt hold D gain --- ArduCopter/Attitude.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 38ae8970b1..c52e782114 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -187,7 +187,7 @@ get_nav_throttle(int32_t z_error) 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 + rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain // slight adjustment to alt hold output output -= constrain(rate_d, -25, 25); From 8de6e0e22370a8673031566d90772af8d63f57fc Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 12:46:40 -0800 Subject: [PATCH 02/18] made landing shoot for below home by 2 meters to avoid bounce up. --- ArduCopter/commands_logic.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index baa1296784..90ec232f7f 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -271,7 +271,7 @@ static void do_land() set_next_WP(¤t_loc); // Set a new target altitude - set_new_altitude(0); + set_new_altitude(-200); } static void do_loiter_unlimited() @@ -364,7 +364,7 @@ static bool verify_land() wp_control = NO_NAV_MODE; // try and come down faster landing_boost++; - landing_boost = min(landing_boost, 20); + landing_boost = min(landing_boost, 30); }else{ landing_boost = 0; wp_control = LOITER_MODE; From 05dce91c241f8d2dc8148d4f0d01b6b3e43b1457 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 12:47:22 -0800 Subject: [PATCH 03/18] fixed GPS bug - moved to fast loop location --- ArduCopter/ArduCopter.pde | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ad56bcfb30..4ae3e71add 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -858,6 +858,12 @@ void loop() // ------------------------------------------------- estimate_velocity(); + // check for new GPS messages + // -------------------------- + if(GPS_enabled){ + update_GPS(); + } + // perform 10hz tasks // ------------------ medium_loop(); @@ -879,7 +885,7 @@ void loop() Log_Write_Performance(); gps_fix_count = 0; - perf_mon_counter = 0; + perf_mon_counter = 0; } //PORTK &= B10111111; } @@ -901,10 +907,6 @@ static void fast_loop() // IMU DCM Algorithm read_AHRS(); - if(GPS_enabled){ - update_GPS(); - } - // custom code/exceptions for flight modes // --------------------------------------- update_yaw_mode(); @@ -1292,8 +1294,8 @@ static void update_GPS(void) }else{ // after 12 reads we guess we may have lost GPS signal, stop navigating // we have lost GPS signal for a moment. Reduce our error to avoid flyaways - nav_roll = 0; - nav_pitch = 0; + nav_roll >>= 1; + nav_pitch >>= 1; } if (g_gps->new_data && g_gps->fix) { From 589f8bc557ed08fc1e90e1ac8e5e8d601eb58a28 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 12:48:02 -0800 Subject: [PATCH 04/18] Disabled Ryans filter until we get real-world testing in. --- ArduCopter/navigation.pde | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index d763360880..76760a0619 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -46,25 +46,26 @@ static void calc_XY_velocity(){ static int32_t last_longitude = 0; static int32_t last_latitude = 0; - static int16_t x_speed_old = 0; - static int16_t y_speed_old = 0; + //static int16_t x_speed_old = 0; + //static int16_t y_speed_old = 0; // y_GPS_speed positve = Up // x_GPS_speed positve = Right // this speed is ~ in cm because we are using 10^7 numbers from GPS float tmp = 1.0/dTnav; - //float tmp = 5; // straightforward approach: - /* + ///* int16_t x_estimate = (float)(g_gps->longitude - last_longitude) * tmp; 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; - */ + //*/ + /* // Ryan Beall's forward estimator: int16_t x_speed_new = (float)(g_gps->longitude - last_longitude) * tmp; int16_t y_speed_new = (float)(g_gps->latitude - last_latitude) * tmp; @@ -74,6 +75,7 @@ static void calc_XY_velocity(){ x_speed_old = x_speed_new; y_speed_old = y_speed_new; + */ last_longitude = g_gps->longitude; last_latitude = g_gps->latitude; From c970ba4f93a3871ea9e589e414b640baef0590b2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 16:47:08 -0800 Subject: [PATCH 05/18] made resetting throttle cruise a function --- ArduCopter/ArduCopter.pde | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4ae3e71add..ee03ee2688 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1877,17 +1877,11 @@ adjust_altitude() // we remove 0 to 100 PWM from hover manual_boost = g.rc_3.control_in - 180; manual_boost = max(-120, manual_boost); - g.throttle_cruise += g.pi_alt_hold.get_integrator(); - g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); - + 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; - g.throttle_cruise += g.pi_alt_hold.get_integrator(); - g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); - + update_throttle_cruise(); }else { manual_boost = 0; } From 414a2581a0deeacef3cb3353428c98ffbb6497ab Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 16:47:38 -0800 Subject: [PATCH 06/18] Made RTL always the current Altitude --- ArduCopter/commands.pde | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index cb3767332b..1561001d25 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -107,10 +107,13 @@ static void set_cmd_with_index(struct Location temp, int i) static int32_t read_alt_to_hold() { + return current_loc.alt; + /* if(g.RTL_altitude <= 0) return current_loc.alt; else return g.RTL_altitude;// + home.alt; + */ } From 24b760d48f541eff96f469b45d16379916a09b5a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jan 2012 16:48:05 -0800 Subject: [PATCH 07/18] added update throttle cruise function --- ArduCopter/system.pde | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index e1f3537781..f5ffa7241a 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -558,9 +558,7 @@ static void set_mode(byte mode) if(throttle_mode == THROTTLE_MANUAL){ // reset all of the throttle iterms - g.pi_alt_hold.reset_I(); - g.pi_throttle.reset_I(); - + update_throttle_cruise(); }else { // an automatic throttle // todo: replace with a throttle cruise estimator @@ -605,6 +603,16 @@ init_simple_bearing() initial_simple_bearing = dcm.yaw_sensor; } +static void update_throttle_cruise() +{ + int16_t tmp = g.pi_alt_hold.get_integrator(); + if(tmp != 0){ + g.throttle_cruise += tmp; + g.pi_alt_hold.reset_I(); + g.pi_throttle.reset_I(); + } +} + static void init_throttle_cruise() { From 4e9c66831529728b661b9d52a94e87923710648d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 14 Jan 2012 12:34:29 +0900 Subject: [PATCH 08/18] OpticalFlow - bug fix - highest resolution was incorrectly set as 1200 instead of 1600 --- .../AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp | 13 +++++++++---- libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h | 6 +++--- .../AP_OpticalFlow_test/AP_OpticalFlow_test.pde | 5 ++--- 3 files changed, 14 insertions(+), 10 deletions(-) diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp index 3ea0e60590..dfd6ae870e 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.cpp @@ -237,17 +237,17 @@ AP_OpticalFlow_ADNS3080::set_led_always_on( bool alwaysOn ) write_register(ADNS3080_CONFIGURATION_BITS, regVal); } -// returns resolution (either 400 or 1200 counts per inch) +// returns resolution (either 400 or 1600 counts per inch) int AP_OpticalFlow_ADNS3080::get_resolution() { if( (read_register(ADNS3080_CONFIGURATION_BITS) & 0x10) == 0 ) return 400; else - return 1200; + return 1600; } -// set parameter to 400 or 1200 counts per inch +// set parameter to 400 or 1600 counts per inch void AP_OpticalFlow_ADNS3080::set_resolution(int resolution) { @@ -255,12 +255,17 @@ AP_OpticalFlow_ADNS3080::set_resolution(int resolution) if( resolution == ADNS3080_RESOLUTION_400 ) { regVal &= ~0x10; - }else if( resolution == ADNS3080_RESOLUTION_1200) { + scaler = AP_OPTICALFLOW_ADNS3080_SCALER; + }else if( resolution == ADNS3080_RESOLUTION_1600) { regVal |= 0x10; + scaler = AP_OPTICALFLOW_ADNS3080_SCALER * 4; } delayMicroseconds(50); // small delay write_register(ADNS3080_CONFIGURATION_BITS, regVal); + + // this will affect conversion factors so update them + update_conversion_factors(); } // get_frame_rate_auto - return whether frame rate is set to "auto" or manual diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h index 79be9492ee..bd257c986f 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_ADNS3080.h @@ -77,7 +77,7 @@ #define ADNS3080_LED_MODE_WHEN_REQUIRED 0x01 #define ADNS3080_RESOLUTION_400 400 -#define ADNS3080_RESOLUTION_1200 1200 +#define ADNS3080_RESOLUTION_1600 1600 // Extended Configuration bits #define ADNS3080_SERIALNPU_OFF 0x02 @@ -118,8 +118,8 @@ class AP_OpticalFlow_ADNS3080 : public AP_OpticalFlow bool get_led_always_on(); // returns true if LED is always on, false if only on when required void set_led_always_on( bool alwaysOn ); // set parameter to true if you want LED always on, otherwise false for only when required - int get_resolution(); // returns resolution (either 400 or 1200 counts per inch) - void set_resolution(int resolution); // set parameter to 400 or 1200 counts per inch + int get_resolution(); // returns resolution (either 400 or 1600 counts per inch) + void set_resolution(int resolution); // set parameter to 400 or 1600 counts per inch bool get_frame_rate_auto(); // get_frame_rate_auto - return true if frame rate is set to "auto", false if manual void set_frame_rate_auto(bool auto_frame_rate); // set_frame_rate_auto(bool) - set frame rate to auto (true), or manual (false) diff --git a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde index 197845c31f..fa369532ce 100644 --- a/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde +++ b/libraries/AP_OpticalFlow/examples/AP_OpticalFlow_test/AP_OpticalFlow_test.pde @@ -201,7 +201,7 @@ void set_resolution() Serial.print("resolution: "); Serial.println(resolution); Serial.println("Choose new value:"); - Serial.println(" 1) 1200"); + Serial.println(" 1) 1600"); Serial.println(" 4) 400"); Serial.println(" x) exit"); Serial.println(); @@ -215,10 +215,9 @@ void set_resolution() // update resolution if( value == '1' ) { - flowSensor.set_resolution(ADNS3080_RESOLUTION_1200); + flowSensor.set_resolution(ADNS3080_RESOLUTION_1600); } if( value == '4' ) { - Serial.println("you want 1200!"); flowSensor.set_resolution(ADNS3080_RESOLUTION_400); } From 817e7442bdc5d3b467465c391141d70d4ca477f4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Jan 2012 19:50:45 +1100 Subject: [PATCH 09/18] baro: fixed an integer overflow issue at high altitudes the averaging array was using 16 bit numbers, but we are storing numbers with 19 significant bits. That caused overflow at high altitude, and some very interesting altitude graphs! Thanks to Michael Oborne for spotting this in a log --- libraries/AP_Baro/AP_Baro_BMP085.cpp | 19 ++++++++++--------- libraries/AP_Baro/AP_Baro_BMP085.h | 13 +++++-------- libraries/AP_Baro/AP_Baro_BMP085_hil.h | 2 -- 3 files changed, 15 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 611fe58885..eefd9c5dad 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -56,6 +56,8 @@ extern "C" { // is not available to the arduino digitalRead function. #define BMP_DATA_READY() (_apm2_hardware?(PINE&0x80):digitalRead(BMP085_EOC)) +// oversampling 3 gives highest resolution +#define OVERSAMPLING 3 // Public Methods ////////////////////////////////////////////////////////////// bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) @@ -64,7 +66,6 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler ) pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input - oss = 3; // Over Sampling setting 3 = High resolution BMP085_State = 0; // Initial state // We read the calibration data registers @@ -142,7 +143,7 @@ int32_t AP_Baro_BMP085::get_raw_temp() { // Send command to Read Pressure void AP_Baro_BMP085::Command_ReadPress() { - if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) { + if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(OVERSAMPLING << 6)) != 0) { healthy = false; } } @@ -163,14 +164,15 @@ void AP_Baro_BMP085::ReadPress() return; } - RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss); + RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING); - if(_offset_press == 0){ + if (_offset_press == 0){ _offset_press = RawPress; RawPress = 0; } else{ RawPress -= _offset_press; } + // filter _press_filter[_press_index++] = RawPress; @@ -186,7 +188,6 @@ void AP_Baro_BMP085::ReadPress() // grab result RawPress /= PRESS_FILTER_SIZE; - //RawPress >>= 3; RawPress += _offset_press; } @@ -261,16 +262,16 @@ void AP_Baro_BMP085::Calculate() x1 = (b2 * (b6 * b6 >> 12)) >> 11; x2 = ac2 * b6 >> 11; x3 = x1 + x2; - //b3 = (((int32_t) ac1 * 4 + x3)<> 2; // BAD - //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for oss=0 + //b3 = (((int32_t) ac1 * 4 + x3)<> 2; // BAD + //b3 = ((int32_t) ac1 * 4 + x3 + 2) >> 2; //OK for OVERSAMPLING=0 tmp = ac1; - tmp = (tmp*4 + x3)<> 13; x2 = (b1 * (b6 * b6 >> 12)) >> 16; x3 = ((x1 + x2) + 2) >> 2; b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15; - b7 = ((uint32_t) RawPress - b3) * (50000 >> oss); + b7 = ((uint32_t) RawPress - b3) * (50000 >> OVERSAMPLING); p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2; x1 = (p >> 8) * (p >> 8); diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index 8e08965996..06cd3ff1d1 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -28,14 +28,11 @@ class AP_Baro_BMP085 : public AP_Baro private: int32_t RawPress; - int32_t _offset_press; + int32_t _offset_press; int32_t RawTemp; int16_t Temp; - int32_t Press; - //int Altitude; - uint8_t oss; + uint32_t Press; bool _apm2_hardware; - //int32_t Press0; // Pressure at sea level // State machine @@ -44,9 +41,9 @@ class AP_Baro_BMP085 : public AP_Baro int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; uint16_t ac4, ac5, ac6; - int _temp_filter[TEMP_FILTER_SIZE]; - int _press_filter[PRESS_FILTER_SIZE]; - long _offset_temp; + int16_t _temp_filter[TEMP_FILTER_SIZE]; + int32_t _press_filter[PRESS_FILTER_SIZE]; + int32_t _offset_temp; uint8_t _temp_index; uint8_t _press_index; diff --git a/libraries/AP_Baro/AP_Baro_BMP085_hil.h b/libraries/AP_Baro/AP_Baro_BMP085_hil.h index d7dbfd7618..bf121f2f16 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085_hil.h +++ b/libraries/AP_Baro/AP_Baro_BMP085_hil.h @@ -17,7 +17,6 @@ public: //int Altitude; bool healthy; - uint8_t oss; bool init(AP_PeriodicProcess * scheduler); uint8_t read(); int32_t get_pressure(); @@ -26,7 +25,6 @@ public: int32_t get_raw_pressure(); int32_t get_raw_temp(); void setHIL(float Temp, float Press); - int32_t _offset_press; }; #endif // __AP_BARO_BMP085_HIL_H__ From 6d0329b08c599f33653edda4cc356ebfe73e1477 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 10:17:39 -0800 Subject: [PATCH 10/18] added notes, removed redundant save wp_total --- ArduCopter/control_modes.pde | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index d5560ecbfb..76f5c7f7a4 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -111,6 +111,7 @@ static void read_trim_switch() trim_flag = false; if(control_mode == AUTO){ + // reset the mission CH7_wp_index = 0; g.command_total.set_and_save(1); return; @@ -118,7 +119,7 @@ static void read_trim_switch() if(CH7_wp_index == 0){ // this is our first WP, let's save WP 1 as a takeoff - // increment index + // increment index to WP index of 1 (home is stored at 0) CH7_wp_index = 1; // set our location ID to 16, MAV_CMD_NAV_WAYPOINT @@ -134,7 +135,7 @@ static void read_trim_switch() // increment index CH7_wp_index++; - // set the next_WP, 0 is Home so we don't set that + // set the next_WP (home is stored at 0) // max out at 100 since I think we need to stay under the EEPROM limit CH7_wp_index = constrain(CH7_wp_index, 1, 100); @@ -149,8 +150,11 @@ static void read_trim_switch() // save command set_cmd_with_index(current_loc, CH7_wp_index); - // save the index - g.command_total.set_and_save(CH7_wp_index + 1); + // 0 = home + // 1 = takeoff + // 2 = WP 2 + // 3 = command total + } } } From e6ec80bb219295006e5b0e231e9f9d4b2e380b35 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 10:18:13 -0800 Subject: [PATCH 11/18] Clarified the Increment for WP total --- ArduCopter/commands.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 1561001d25..a5c3dd5f98 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -101,7 +101,7 @@ static void set_cmd_with_index(struct Location temp, int i) eeprom_write_dword((uint32_t *) mem, temp.lng); // Long is stored in decimal degrees * 10^7 // Make sure our WP_total - if(g.command_total <= i) + if(g.command_total < (i+1)) g.command_total.set_and_save(i+1); } From f25d741f6290a246c1dff4f5cb12671098077d77 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 10:18:41 -0800 Subject: [PATCH 12/18] temp removed alt D term until more testing --- ArduCopter/Attitude.pde | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index c52e782114..c99cdb0259 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -187,10 +187,10 @@ get_nav_throttle(int32_t z_error) 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 + //rate_d = ((rate_d + climb_rate)>>1) * .1; // replace with gain // slight adjustment to alt hold output - output -= constrain(rate_d, -25, 25); + //output -= constrain(rate_d, -25, 25); // light filter of output output = (old_output * 3 + output) / 4; From 11e1df497d6583ed261c258814930a26584f658b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 10:55:06 -0800 Subject: [PATCH 13/18] lowered Loiter I, commented out unused var --- ArduCopter/Attitude.pde | 2 +- ArduCopter/config.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index c99cdb0259..9515f60189 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -166,7 +166,7 @@ static int16_t get_nav_throttle(int32_t z_error) { static int16_t old_output = 0; - static int16_t rate_d = 0; + //static int16_t rate_d = 0; int16_t rate_error; int16_t output; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index c9276f957a..c662b0baa0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -614,7 +614,7 @@ # define LOITER_P 2.0 // was .25 in previous #endif #ifndef LOITER_I -# define LOITER_I 0.05 // Wind control +# define LOITER_I 0.04 // Wind control #endif #ifndef LOITER_IMAX # define LOITER_IMAX 30 // degrees° From e7ad08e2090ee3851566c9aa36010379fe06d58a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 11:20:33 -0800 Subject: [PATCH 14/18] adjusted GPS watch dog not to be so aggressive, removed unneeded flag clearing --- ArduCopter/ArduCopter.pde | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ee03ee2688..fc1e70692f 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1289,7 +1289,7 @@ static void update_GPS(void) //current_loc.lat = -1224318000; // Lat * 10 * *7 //current_loc.alt = 100; // alt * 10 * *7 //return; - if(gps_watchdog < 12){ + if(gps_watchdog < 30){ gps_watchdog++; }else{ // after 12 reads we guess we may have lost GPS signal, stop navigating @@ -1357,8 +1357,6 @@ static void update_GPS(void) update_altitude(); #endif - } else { - g_gps->new_data = false; } } From 6714ab49ad8834fa5ffe951d1f541c718dc65df4 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 11:21:50 -0800 Subject: [PATCH 15/18] Tweak to make circle mode scripting more accurate --- ArduCopter/commands_logic.pde | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 90ec232f7f..1ad8d66560 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -290,7 +290,7 @@ static void do_loiter_turns() wp_control = CIRCLE_MODE; // reset desired location - circle_angle = 0; + if(command_nav_queue.lat == 0){ // allow user to specify just the altitude @@ -305,6 +305,10 @@ static void do_loiter_turns() loiter_total = command_nav_queue.p1 * 360; loiter_sum = 0; old_target_bearing = target_bearing; + + circle_angle = target_bearing + 18000; + circle_angle = wrap_360(circle_angle); + circle_angle *= RADX100; } static void do_loiter_time() From cf51967bbb1f33e0e0b1e7b0cb65e3aaabd0a6e7 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 11:32:48 -0800 Subject: [PATCH 16/18] Fix for SIMPLE mode --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index fc1e70692f..c16fb988a9 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1460,7 +1460,7 @@ void update_roll_pitch_mode(void) // new radio frame is used to make sure we only call this at 50hz void update_simple_mode(void) { - float simple_sin_y=0, simple_cos_x=0; + static float simple_sin_y=0, simple_cos_x=0; // used to manage state machine // which improves speed of function From 3879e79898ce80ab6adb548e4cb036fb87ca065b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 11:37:00 -0800 Subject: [PATCH 17/18] b2 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c16fb988a9..0ce2deace4 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" +#define THISFIRMWARE "ArduCopter V2.2 b2" /* ArduCopter Version 2.2 Authors: Jason Short From 7e411f5fb6b63788c9aa7b4f3b0f9e6cc75496be Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 14 Jan 2012 11:43:52 -0800 Subject: [PATCH 18/18] This allows users to test the Auto_throttle hold or cruise value --- ArduCopter/APM_Config.h | 6 ++++++ ArduCopter/ArduCopter.pde | 12 +++++++++--- ArduCopter/motors.pde | 2 ++ ArduCopter/system.pde | 2 ++ 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index e57eab2127..f634fa101f 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -64,4 +64,10 @@ #define USERHOOK_VARIABLES "UserVariables.h" +// to enable, set to 1 +// to disable, set to 0 +#define AUTO_THROTTLE_HOLD 1 + + + //# define LOGGING_ENABLED DISABLED \ No newline at end of file diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0ce2deace4..9d86beab8b 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1494,6 +1494,10 @@ void update_throttle_mode(void) { int16_t throttle_out; + #if AUTO_THROTTLE_HOLD != 0 + static float throttle_avg = THROTTLE_CRUISE; + #endif + switch(throttle_mode){ case THROTTLE_MANUAL: if (g.rc_3.control_in > 0){ @@ -1508,11 +1512,13 @@ void update_throttle_mode(void) } #endif + #if AUTO_THROTTLE_HOLD != 0 // calc average throttle - if ((g.rc_3.control_in > MINIMUM_THROTTLE)){ - //throttle_avg = throttle_avg * .98 + rc_3.control_in * .02; - //g.throttle_cruise = throttle_avg; + if ((g.rc_3.control_in > MINIMUM_THROTTLE) && abs(climb_rate) < 60){ + throttle_avg = throttle_avg * .98 + (float)g.rc_3.control_in * .02; + g.throttle_cruise = throttle_avg; } + #endif // Code to manage the Copter state if ((millis() - takeoff_timer) > 5000){ diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 7c05c0b400..7b81974192 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -121,6 +121,8 @@ static void init_disarm_motors() motor_armed = false; compass.save_offsets(); + g.throttle_cruise.save(); + // we are not in the air takeoff_complete = false; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index f5ffa7241a..d783c5b5d7 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -616,12 +616,14 @@ static void update_throttle_cruise() static void init_throttle_cruise() { +#if AUTO_THROTTLE_HOLD == 0 // are we moving from manual throttle to auto_throttle? if((old_control_mode <= STABILIZE) && (g.rc_3.control_in > MINIMUM_THROTTLE)){ g.pi_throttle.reset_I(); g.pi_alt_hold.reset_I(); g.throttle_cruise.set_and_save(g.rc_3.control_in); } +#endif } #if CLI_SLIDER_ENABLED == ENABLED && CLI_ENABLED == ENABLED