From 048532ae98577841a47c6fdbb007014e18704d6a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 6 Jan 2012 21:43:37 -0800 Subject: [PATCH 01/20] scaled to 0-1023 --- ArduCopter/defines.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index ff145e2e5b..cd0efba163 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -283,8 +283,8 @@ enum gcs_severity { #define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from -#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*VOLT_DIV_RATIO -#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT +#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1023.0))*VOLT_DIV_RATIO +#define CURRENT_AMPS(x) ((x*(g.input_voltage/1023.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT //#define BARO_FILTER_SIZE 8 /* ************************************************************** */ From b1341011a0f82dcde166c2fdbd55682d36ec60ef Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:20:25 -0800 Subject: [PATCH 02/20] updated throttle to keep altitude --- Tools/autotest/arducopter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index ae1d53eca8..d4a92de701 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -50,7 +50,7 @@ def takeoff(mavproxy, mav, alt_min = 30): '''takeoff get to 30m altitude''' mavproxy.send('switch 6\n') # stabilize mode wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1500\n') + mavproxy.send('rc 3 1510\n') m = mav.recv_match(type='VFR_HUD', blocking=True) if (m.alt < alt_min): wait_altitude(mav, alt_min, (alt_min + 5)) @@ -208,7 +208,7 @@ def fly_simple(mavproxy, mav, side=60, timeout=120): '''fly Simple, flying N then E''' mavproxy.send('switch 6\n') wait_mode(mav, 'STABILIZE') - mavproxy.send('rc 3 1430\n') + mavproxy.send('rc 3 1450\n') tstart = time.time() failed = False From b8bcd81b3985b74105b3bd11b41fd5ce18e38b4a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:20:45 -0800 Subject: [PATCH 03/20] Cleanup --- ArduCopter/APM_Config.h | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index df0a080170..8007fd2a4d 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -25,7 +25,7 @@ V_FRAME */ -# define CH7_OPTION CH7_SAVE_WP +//# define CH7_OPTION CH7_SAVE_WP /* CH7_DO_NOTHING CH7_SET_HOVER @@ -39,9 +39,6 @@ #define ACCEL_ALT_HOLD 0 // disabled by default, work in progress -// lets use Manual throttle during Loiter -//#define LOITER_THR THROTTLE_MANUAL -# define RTL_YAW YAW_HOLD //#define RATE_ROLL_I 0.18 //#define RATE_PITCH_I 0.18 @@ -68,6 +65,4 @@ // #define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 // #define APM2_BETA_HARDWARE // for developers who received an early beta board with the older BMP085 - -// This is experimental!!, be caureful, effects stable mode -#define WIND_COMP_STAB 0 \ No newline at end of file +//# define LOGGING_ENABLED DISABLED \ No newline at end of file From ca80dc549cc5797be19b8d710d7bade2025a868c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:21:31 -0800 Subject: [PATCH 04/20] Made RTL hold position until it reaches altitude --- ArduCopter/ArduCopter.pde | 46 +++++++++++++++++++++++++-------------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 72acb99733..c60b2aad70 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1390,15 +1390,17 @@ void update_roll_pitch_mode(void) if(do_simple && new_radio_frame){ update_simple_mode(); } + #if WIND_COMP_STAB == 1 - // in this mode, nav_roll and nav_pitch = the iterm - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll); - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch); + // in this mode, nav_roll and nav_pitch = the iterm + g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in + nav_roll); + g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in + nav_pitch); #else - // in this mode, nav_roll and nav_pitch = the iterm - g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); + // in this mode, nav_roll and nav_pitch = the iterm + g.rc_1.servo_out = get_stabilize_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_stabilize_pitch(g.rc_2.control_in); #endif + break; case ROLL_PITCH_AUTO: @@ -1612,6 +1614,10 @@ static void update_navigation() set_mode(LOITER); auto_land_timer = millis(); + }else if(current_loc.alt < (next_WP.alt - 300)){ + // don't navigate if we are below our target alt + wp_control = LOITER_MODE; + }else{ // calculates desired Yaw #if FRAME_CONFIG == HELI_FRAME @@ -1655,10 +1661,13 @@ static void update_navigation() case LAND: wp_control = LOITER_MODE; - if (current_loc.alt < 250){ - wp_control = NO_NAV_MODE; - next_WP.alt = -200; // force us down + if(verify_land()) { // JLN fix for auto land in RTL + set_mode(STABILIZE); + } else { + // calculates the desired Roll and Pitch + update_nav_wp(); } + // calculates the desired Roll and Pitch update_nav_wp(); break; @@ -2012,14 +2021,19 @@ static void update_nav_wp() }else if(wp_control == NO_NAV_MODE){ // calc the Iterms for Loiter based on velocity - //if ((x_actual_speed + y_actual_speed) == 0) - if (g_gps->ground_speed < 50) - calc_wind_compensation(); - else - reduce_wind_compensation(); + #if WIND_COMP_STAB == 1 + if (g_gps->ground_speed < 50) + calc_wind_compensation(); + else + reduce_wind_compensation(); - // rotate nav_lat, nav_lon to roll and pitch - calc_loiter_pitch_roll(); + // rotate nav_lat, nav_lon to roll and pitch + calc_loiter_pitch_roll(); + #else + // clear out our nav so we can do things like land straight + nav_pitch = 0; + nav_roll = 0; + #endif } } From 250545f31deddb15fccec4c3d38190a0f24b9221 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:22:57 -0800 Subject: [PATCH 05/20] Auto-land updates - removed sonar option - not needed updates from JLN --- ArduCopter/commands_logic.pde | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 631f54d931..e81fae7958 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -357,15 +357,17 @@ static bool verify_land() velocity_land = 2000; - if ((current_loc.alt - home.alt) < 500){ + if ((current_loc.alt - home.alt) < 300){ // 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) + (old_alt - current_loc.alt)) / 8; } + + // remenber altitude for climb_rate old_alt = current_loc.alt; - if ((current_loc.alt - home.alt) < 300){ - + if ((current_loc.alt - home.alt) < 200){ + // don't bank to hold position wp_control = NO_NAV_MODE; // Update by JLN for a safe AUTO landing @@ -375,20 +377,14 @@ static bool verify_land() g.pi_throttle.reset_I(); } - if(g.sonar_enabled){ - // decide which sensor we're using - if(sonar_alt < 40){ - land_complete = true; - //Serial.println("Y"); - return true; - } - } - - if((current_loc.alt - home.alt) < 300 && velocity_land <= 100){ + if((current_loc.alt - home.alt) < 100 && velocity_land <= 100){ land_complete = true; + // reset manual_boost hack + manual_boost = 0; + // reset old_alt old_alt = 0; - init_disarm_motors(); + //init_disarm_motors(); return true; } //Serial.printf("N, %d\n", velocity_land); From 7e96e0f7fa30745631a84fcd507ca675f49c45b2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:23:44 -0800 Subject: [PATCH 06/20] Quad frame - X is default added wind comp stability option to config - off by default --- ArduCopter/config.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index bee8cb8a66..3abb31d789 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -65,10 +65,10 @@ // FRAME_CONFIG // #ifndef FRAME_CONFIG -# define FRAME_CONFIG QUAD_FRAME +# define FRAME_CONFIG QUAD_FRAME #endif #ifndef FRAME_ORIENTATION -# define FRAME_ORIENTATION PLUS_FRAME +# define FRAME_ORIENTATION X_FRAME #endif ////////////////////////////////////////////////////////////////////////////// @@ -468,6 +468,11 @@ #endif +// experimental feature for +#ifndef WIND_COMP_STAB +# define WIND_COMP_STAB 0 +#endif + ////////////////////////////////////////////////////////////////////////////// From 12493d6431c0b3e47961132dd30951002a3c7e63 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:24:35 -0800 Subject: [PATCH 07/20] Moved wind comp into a define --- ArduCopter/navigation.pde | 30 +++++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 8834570fcc..a5ba2e3790 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -195,7 +195,7 @@ static void calc_loiter_pitch_roll() nav_pitch = -nav_pitch; } -// what's the update rate? 10hz GPS? +#if WIND_COMP_STAB == 1 static void calc_wind_compensation() { // this idea is a function that converts user input into I term for position hold @@ -216,8 +216,25 @@ static void calc_wind_compensation() // filter the input and apply it to out integrator value // nav_lon and nav_lat will be applied to normal flight - nav_lon = ((int32_t)g.pi_loiter_lon.get_integrator() * 15 + roll_tmp) / 16; - nav_lat = ((int32_t)g.pi_loiter_lat.get_integrator() * 15 + pitch_tmp) / 16; + + // This filter is far too fast!!! + //nav_lon = ((int32_t)g.pi_loiter_lon.get_integrator() * 15 + roll_tmp) / 16; + //nav_lat = ((int32_t)g.pi_loiter_lat.get_integrator() * 15 + pitch_tmp) / 16; + + nav_lon = g.pi_loiter_lon.get_integrator(); + nav_lat = g.pi_loiter_lat.get_integrator(); + + // Maybe a slower filter would work? + if(g.pi_loiter_lon.get_integrator() > roll_tmp){ + g.pi_loiter_lon.set_integrator(nav_lon - 5); + }else if(g.pi_loiter_lon.get_integrator() < roll_tmp){ + g.pi_loiter_lon.set_integrator(nav_lon + 5); + } + if(g.pi_loiter_lat.get_integrator() > pitch_tmp){ + g.pi_loiter_lat.set_integrator(nav_lat - 5); + }else if(g.pi_loiter_lat.get_integrator() < pitch_tmp){ + g.pi_loiter_lat.set_integrator(nav_lat + 5); + } // save smoothed input to integrator g.pi_loiter_lon.set_integrator(nav_lon); // X @@ -243,16 +260,15 @@ static void reduce_wind_compensation() tmp *= .98; g.pi_loiter_lat.set_integrator(tmp); // Y -#if 0 // debug - int16_t t1 = g.pi_loiter_lon.get_integrator(); - int16_t t2 = g.pi_loiter_lon.get_integrator(); + //int16_t t1 = g.pi_loiter_lon.get_integrator(); + //int16_t t2 = g.pi_loiter_lon.get_integrator(); //Serial.printf("reduce wind iterm X:%d Y:%d \n", // t1, // t2); -#endif } +#endif static int16_t calc_desired_speed(int16_t max_speed) { From cf5e0b3a1baf5f0eb6de1c037f126c4abc6c954b Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:26:56 -0800 Subject: [PATCH 08/20] converted auto land to use the mission planner version Removed gate that looked for already set control_mode. Wasn't compatible with failsafe --- ArduCopter/system.pde | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 96d6179b64..a57413b9eb 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -426,10 +426,11 @@ static void startup_ground(void) static void set_mode(byte mode) { - if(control_mode == mode){ + //if(control_mode == mode){ // don't switch modes if we are already in the correct mode. - return; - } + // Serial.print("Double mode\n"); + //return; + //} // if we don't have GPS lock if(home_is_set == false){ @@ -458,6 +459,9 @@ static void set_mode(byte mode) // do not auto_land if we are leaving RTL auto_land_timer = 0; + // if we change modes, we must clear landed flag + land_complete = false; + // debug to Serial terminal Serial.println(flight_mode_strings[control_mode]); @@ -531,12 +535,10 @@ static void set_mode(byte mode) break; case LAND: - land_complete = false; - yaw_mode = YAW_HOLD; - roll_pitch_mode = ROLL_PITCH_AUTO; + yaw_mode = LOITER_YAW; + roll_pitch_mode = LOITER_RP; throttle_mode = THROTTLE_AUTO; - next_WP = current_loc; - next_WP.alt = 0; + do_land(); break; case RTL: From 0a7378856d8162a82b8619ecb79b8bcb977267b2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:27:16 -0800 Subject: [PATCH 09/20] Added finish to missions - auto-land or stabilize --- ArduCopter/commands_process.pde | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 3073891416..4777178db2 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -58,7 +58,14 @@ static void update_commands() command_nav_queue.id = NO_COMMAND; } }else{ + // we are out of commands g.command_index = command_nav_index = 255; + // if we are on the ground, enter stabilize, else Land + if (land_complete == true){ + set_mode(STABILIZE); + } else { + set_mode(LAND); + } } } From a8574e813d8248644557d50a4a41afda4e59244c Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 7 Jan 2012 22:28:03 -0800 Subject: [PATCH 10/20] R8 --- ArduCopter/ArduCopter.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index c60b2aad70..05045a2fc3 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.1.1r7 alpha" +#define THISFIRMWARE "ArduCopter V2.1.1r8 alpha" /* ArduCopter Version 2.0 Beta Authors: Jason Short From 598593e1fc46a67cb1047756c88e4ba3337db76c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 8 Jan 2012 17:48:30 +0900 Subject: [PATCH 11/20] ArduCopter - changed Sonar to use A1 (was AN4) --- ArduCopter/config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3abb31d789..0ebd2b0c36 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -170,7 +170,7 @@ # endif #elif CONFIG_SONAR_SOURCE == SONAR_SOURCE_ANALOG_PIN # ifndef CONFIG_SONAR_SOURCE_ANALOG_PIN -# define CONFIG_SONAR_SOURCE_ANALOG_PIN AN4 +# define CONFIG_SONAR_SOURCE_ANALOG_PIN A1 # endif #else # warning Invalid value for CONFIG_SONAR_SOURCE, disabling sonar From fda12c8902807b5da2e9b233b7813a012b91d570 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 8 Jan 2012 14:52:03 -0800 Subject: [PATCH 12/20] Added Acro to Rate_P --- ArduCopter/ArduCopter.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 05045a2fc3..f7a015efe4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1886,6 +1886,8 @@ static void tuning(){ 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); break; case CH6_RATE_KI: From 0fb40825813c79e51f02e612da75c9eda79de598 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 8 Jan 2012 18:24:55 -0700 Subject: [PATCH 13/20] Bug fix for issue 438. The rate in a change alt command does not need to be signed. The code will determine if the rate needs to be positive or negative --- ArduPlane/commands_logic.pde | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 52ab2661e7..b1ae83b171 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -413,8 +413,9 @@ static void do_wait_delay() static void do_change_alt() { - condition_rate = next_nonnav_command.lat; + condition_rate = abs((int)next_nonnav_command.lat); condition_value = next_nonnav_command.alt; + if(condition_value < current_loc.alt) condition_rate = -condition_rate; target_altitude = current_loc.alt + (condition_rate / 10); // Divide by ten for 10Hz update next_WP.alt = condition_value; // For future nav calculations offset_altitude = 0; // For future nav calculations From 8b68da31057cc63c8b8d9709ac1573d6ea1b49f7 Mon Sep 17 00:00:00 2001 From: analoguedevices Date: Mon, 9 Jan 2012 01:59:55 +0000 Subject: [PATCH 14/20] Put in commented-out APM2 #defines, to save people the trouble of having to type them in themselves --- ArduCopter/APM_Config.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 8007fd2a4d..ed7fe5ce65 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -2,6 +2,9 @@ // Example config file. Take a look at config.h. Any term define there can be overridden by defining it here. +// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 +// # define APM2_BETA_HARDWARE + // GPS is auto-selected //#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD From 47197891e34760454e8277bee8a755efd446018f Mon Sep 17 00:00:00 2001 From: analoguedevices Date: Mon, 9 Jan 2012 02:00:46 +0000 Subject: [PATCH 15/20] Put in commented-out APM2 # defines, to save people the trouble of having to type them in themselves. --- ArduPlane/APM_Config.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index d7abbcc277..0b40f0e887 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -6,6 +6,9 @@ // For example if you wanted the Port 3 baud rate to be 38400 you would add a statement like the one below (uncommented) //#define SERIAL3_BAUD 38400 +// # define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 +// # define APM2_BETA_HARDWARE + // You may also put an include statement here to point at another configuration file. This is convenient if you maintain // different configuration files for different aircraft or HIL simulation. See the examples below From 22d2db53d858a7e71b10bda54e66593a61177cee Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Sun, 8 Jan 2012 21:26:56 -0700 Subject: [PATCH 16/20] Repair dataflash test --- .../examples/DataFlash_test/DataFlash_test.pde | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde b/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde index f3930f6a12..7cea253c3e 100644 --- a/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde +++ b/libraries/DataFlash/examples/DataFlash_test/DataFlash_test.pde @@ -10,9 +10,14 @@ #define HEAD_BYTE1 0xA3 #define HEAD_BYTE2 0x95 +// NOTE: You must uncomment one of the following two lines +DataFlash_APM2 DataFlash; // Uncomment this line if using APM2 hardware +//DataFlash_APM1 DataFlash; // Uncomment this line if using APM1 hardware + + void setup() { - Serial.begin(38400); + Serial.begin(115200); DataFlash.Init(); // DataFlash initialization Serial.println("Dataflash Log Test 1.0"); @@ -24,14 +29,14 @@ void setup() Serial.print("Manufacturer:"); Serial.print(int(DataFlash.df_manufacturer)); Serial.print(","); - Serial.print(int(DataFlash.df_device_0)); - Serial.print(","); - Serial.print(int(DataFlash.df_device_1)); + Serial.print(DataFlash.df_device); Serial.println(); // We start to write some info (sequentialy) starting from page 1 // This is similar to what we will do... DataFlash.StartWrite(1); + Serial.println("After testing perform erase before using DataFlash for logging!"); + Serial.println(""); Serial.println("Writing to flash... wait..."); for (int i = 0; i < 1000; i++){ // Write 1000 packets... // We write packets of binary data... (without worry about nothing more) @@ -96,5 +101,8 @@ void loop() } Serial.println(); } - delay(10000); + Serial.println(""); + Serial.println("Test complete. Test will repeat in 20 seconds"); + Serial.println(""); + delay(20000); } \ No newline at end of file From 7b9701fdd538fad20643b79f080a9ccc32cc87e7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Jan 2012 13:53:54 +0900 Subject: [PATCH 17/20] ArduCopter - added ROLL_PITCH_STABLE_OF (i.e. Stabilised Roll/Pitch + adjustments based on optical flow) Removed optical flow from regular loiter for now until it's tested. --- ArduCopter/ArduCopter.pde | 75 ++++++++++++++++++++++++++++++--------- ArduCopter/Attitude.pde | 70 +++++++++++++++++++++++++++++++++++- ArduCopter/Log.pde | 20 ++++++++--- ArduCopter/Parameters.h | 8 +++++ ArduCopter/config.h | 17 +++++++++ ArduCopter/defines.h | 5 +++ ArduCopter/system.pde | 2 ++ 7 files changed, 175 insertions(+), 22 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f7a015efe4..9368f051ae 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -665,6 +665,11 @@ static int16_t nav_lon; static int16_t nav_lat_p; static int16_t nav_lon_p; +// The Commanded ROll from the autopilot based on optical flow sensor. +static int32_t of_roll = 0; +// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. +static int32_t of_pitch = 0; + //////////////////////////////////////////////////////////////////////////////// // Navigation Throttle control @@ -926,12 +931,6 @@ static void medium_loop() update_GPS(); } - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled){ - update_optical_flow(); - } - #endif - #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled){ if (compass.read()) { @@ -975,13 +974,13 @@ static void medium_loop() // If we have optFlow enabled we can grab a more accurate speed // here and override the speed from the GPS // ---------------------------------------- - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled && current_loc.alt < 500){ - // optflow wont be enabled on 1280's - x_GPS_speed = optflow.x_cm; - y_GPS_speed = optflow.y_cm; - } - #endif + //#ifdef OPTFLOW_ENABLED + //if(g.optflow_enabled && current_loc.alt < 500){ + // // optflow wont be enabled on 1280's + // x_GPS_speed = optflow.x_cm; + // y_GPS_speed = optflow.y_cm; + //} + //#endif // control mode specific updates // ----------------------------- @@ -1096,6 +1095,13 @@ static void fifty_hz_loop() } #endif + // syncronise optical flow reads with altitude reads + #ifdef OPTFLOW_ENABLED + if(g.optflow_enabled){ + update_optical_flow(); + } + #endif + // agmatthews - USERHOOKS #ifdef USERHOOK_50HZLOOP USERHOOK_50HZLOOP @@ -1222,15 +1228,21 @@ static void super_slow_loop() #ifdef OPTFLOW_ENABLED static void update_optical_flow(void) { + static int log_counter = 0; + optflow.update(); optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow // write to log - if (g.log_bitmask & MASK_LOG_OPTFLOW){ - Log_Write_Optflow(); + log_counter++; + if( log_counter >= 5 ) { + log_counter = 0; + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } } - if(g.optflow_enabled && current_loc.alt < 500){ + /*if(g.optflow_enabled && current_loc.alt < 500){ if(GPS_enabled){ // if we have a GPS, we add some detail to the GPS // XXX this may not ne right @@ -1247,7 +1259,7 @@ static void update_optical_flow(void) } // OK to run the nav routines nav_ok = true; - } + }*/ } #endif @@ -1414,6 +1426,22 @@ void update_roll_pitch_mode(void) g.rc_1.servo_out = get_stabilize_roll(control_roll); g.rc_2.servo_out = get_stabilize_pitch(control_pitch); break; + + case ROLL_PITCH_STABLE_OF: + // apply SIMPLE mode transform + if(do_simple && new_radio_frame){ + update_simple_mode(); + } + + // in this mode, nav_roll and nav_pitch = the iterm + #if WIND_COMP_STAB == 1 + g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in + nav_roll)); + g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in + nav_pitch)); + #else + g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in)); + g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in)); + #endif + break; } // clear new radio frame info @@ -1950,6 +1978,19 @@ static void tuning(){ g.rc_6.set_range(0,1000); // 0 to 1 g.pi_alt_hold.kP(tuning_value); break; + + case CH6_OPTFLOW_KP: + g.rc_6.set_range(0,10000); // 0 to 10 + g.pi_optflow_roll.kP(tuning_value); + g.pi_optflow_pitch.kP(tuning_value); + break; + + case CH6_OPTFLOW_KI: + g.rc_6.set_range(0,100); // 0 to 0.1 + g.pi_optflow_roll.kI(tuning_value); + g.pi_optflow_pitch.kI(tuning_value); + break; + } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index cdf552d099..8c075f3305 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -258,6 +258,8 @@ static void reset_rate_I() g.pi_rate_pitch.reset_I(); g.pi_acro_roll.reset_I(); g.pi_acro_pitch.reset_I(); + g.pi_optflow_roll.reset_I(); + g.pi_optflow_pitch.reset_I(); } @@ -429,4 +431,70 @@ static int get_z_damping() static void init_z_damper() { } -#endif \ No newline at end of file +#endif + +// calculate modified roll/pitch depending upon optical flow values +static int32_t +get_of_roll(int32_t control_roll) +{ +#ifdef OPTFLOW_ENABLED + //static int32_t of_roll = 0; // we use global variable to make logging easier + static unsigned long last_of_roll_update = 0; + static float prev_value = 0; + float x_cm; + + // check if new optflow data available + if( optflow.last_update != last_of_roll_update) { + last_of_roll_update = optflow.last_update; + + // filter movement + x_cm = (optflow.x_cm + prev_value) / 2.0 * 50.0; + + // only stop roll if caller isn't modifying roll + if( control_roll == 0 && current_loc.alt < 1500) { + of_roll = g.pi_optflow_roll.get_pi(-x_cm, 1.0); // we could use the last update time to calculate the time change + }else{ + g.pi_optflow_roll.reset_I(); + prev_value = 0; + } + } + // limit maximum angle + of_roll = constrain(of_roll, -1000, 1000); + + return control_roll+of_roll; +#else + return control_roll; +#endif +} + +static int32_t +get_of_pitch(int32_t control_pitch) +{ +#ifdef OPTFLOW_ENABLED + //static int32_t of_pitch = 0; // we use global variable to make logging easier + static unsigned long last_of_pitch_update = 0; + static float prev_value = 0; + float y_cm; + + // check if new optflow data available + if( optflow.last_update != last_of_pitch_update ) { + last_of_pitch_update = optflow.last_update; + + // filter movement + y_cm = (optflow.y_cm + prev_value) / 2.0 * 50.0; + + // only stop roll if caller isn't modifying roll + if( control_pitch == 0 && current_loc.alt < 1500 ) { + of_pitch = g.pi_optflow_pitch.get_pi(y_cm, 1.0); // we could use the last update time to calculate the time change + }else{ + g.pi_optflow_pitch.reset_I(); + prev_value = 0; + } + } + // limit maximum angle + of_pitch = constrain(of_pitch, -1000, 1000); + return control_pitch+of_pitch; +#else + return control_pitch; +#endif +} diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index cfc71a7454..61197d4186 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -482,8 +482,12 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); + DataFlash.WriteInt((int)optflow.x_cm); + DataFlash.WriteInt((int)optflow.y_cm); DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(of_roll); + DataFlash.WriteLong(of_pitch); DataFlash.WriteByte(END_BYTE); #endif } @@ -495,15 +499,23 @@ static void Log_Read_Optflow() int16_t temp1 = DataFlash.ReadInt(); // 1 int16_t temp2 = DataFlash.ReadInt(); // 2 int16_t temp3 = DataFlash.ReadInt(); // 3 - float temp4 = DataFlash.ReadLong(); // 4 - float temp5 = DataFlash.ReadLong(); // 5 + int16_t temp4 = DataFlash.ReadInt(); // 4 + int16_t temp5 = DataFlash.ReadInt(); // 5 + float temp6 = DataFlash.ReadLong(); // 6 + float temp7 = DataFlash.ReadLong(); // 7 + int32_t temp8 = DataFlash.ReadLong(); // 8 + int32_t temp9 = DataFlash.ReadLong(); // 9 - Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), + Serial.printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %d, %d\n"), temp1, temp2, temp3, temp4, - temp5); + temp5, + temp6, + temp7, + temp8, + temp9); #endif } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e5e68693ec..4895a39b52 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -178,6 +178,8 @@ public: 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 // 254,255: reserved @@ -298,6 +300,9 @@ public: APM_PI pi_acro_roll; APM_PI pi_acro_pitch; + APM_PI pi_optflow_roll; + APM_PI pi_optflow_pitch; + uint8_t junk; // Note: keep initializers here in the same order as they are declared above. @@ -417,6 +422,9 @@ public: 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_IMAX * 100), + pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100), + junk(0) // XXX just so that we can add things without worrying about the trailing comma { } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 0ebd2b0c36..02b3548c79 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -310,6 +310,23 @@ #ifndef OPTFLOW_FOV # define OPTFLOW_FOV AP_OPTICALFLOW_ADNS3080_08_FOV #endif +// optical flow based loiter PI values +#ifndef OPTFLOW_ROLL_P + #define OPTFLOW_ROLL_P 6.4 +#endif +#ifndef OPTFLOW_ROLL_I + #define OPTFLOW_ROLL_I 0.068 +#endif +#ifndef OPTFLOW_PITCH_P + #define OPTFLOW_PITCH_P 6.4 +#endif +#ifndef OPTFLOW_PITCH_I + #define OPTFLOW_PITCH_I 0.068 +#endif +#ifndef OPTFLOW_IMAX + #define OPTFLOW_IMAX 4 +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index cd0efba163..efa8fe9f70 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -18,6 +18,7 @@ #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 #define ROLL_PITCH_AUTO 2 +#define ROLL_PITCH_STABLE_OF 3 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 @@ -155,6 +156,10 @@ #define CH6_Z_GAIN 15 #define CH6_DAMP 16 +// optical flow controller +#define CH6_OPTFLOW_KP 17 +#define CH6_OPTFLOW_KI 18 + // nav byte mask // ------------- diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a57413b9eb..6c610d7137 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -380,6 +380,8 @@ static void reset_I_all(void) g.pi_throttle.reset_I(); g.pi_acro_roll.reset_I(); g.pi_acro_pitch.reset_I(); + g.pi_optflow_roll.reset_I(); + g.pi_optflow_pitch.reset_I(); } From ba72a9834577250a7ef12a6ef82bbb528b4dac14 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 8 Jan 2012 23:01:22 -0800 Subject: [PATCH 18/20] Slowed Auto Descent Don't return true in Landing code to prevent flyways in Stabilize --- ArduCopter/commands_logic.pde | 5 ++--- ArduCopter/commands_process.pde | 2 ++ 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index e81fae7958..a480c2210c 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -348,7 +348,7 @@ static bool verify_land() static int16_t velocity_land = -1; // land at .62 meter per second - next_WP.alt = original_alt - ((millis() - land_start) / 16); // condition_value = our initial + next_WP.alt = original_alt - ((millis() - land_start) / 32); // condition_value = our initial if (old_alt == 0) old_alt = current_loc.alt; @@ -384,8 +384,7 @@ static bool verify_land() // reset old_alt old_alt = 0; - //init_disarm_motors(); - return true; + return false; } //Serial.printf("N, %d\n", velocity_land); //Serial.printf("N_alt, %ld\n", next_WP.alt); diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index 4777178db2..76ad7cde6b 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -63,6 +63,8 @@ static void update_commands() // if we are on the ground, enter stabilize, else Land if (land_complete == true){ set_mode(STABILIZE); + // disarm motors + //init_disarm_motors(); } else { set_mode(LAND); } From 8af708b165990070ff9d13f16b5c71600d1defaa Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jan 2012 14:27:27 +1100 Subject: [PATCH 19/20] autotest: make history pages use the css style --- Tools/autotest/web/css/main.css | 4 ++-- Tools/autotest/web/index.html | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Tools/autotest/web/css/main.css b/Tools/autotest/web/css/main.css index a9f5f3c735..17a7f918f8 100644 --- a/Tools/autotest/web/css/main.css +++ b/Tools/autotest/web/css/main.css @@ -11,11 +11,11 @@ body { margin:0px; padding:0px; background-color: #fff; - background-image: url(images/bg.png); + background-image: url(/images/bg.png); } #logo { - background-image:url(images/logo.png); + background-image:url(/images/logo.png); background-repeat:no-repeat; height: 120px; width: 420px; diff --git a/Tools/autotest/web/index.html b/Tools/autotest/web/index.html index 5dd5e28d27..ed6ed1d211 100644 --- a/Tools/autotest/web/index.html +++ b/Tools/autotest/web/index.html @@ -4,7 +4,7 @@ ArduPilot Automatic Testing - + From 6245ce935d7a6e0d0ff193400b8d64652afd2456 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Jan 2012 18:07:51 +1100 Subject: [PATCH 20/20] autotest: cope with startup messages in a different order --- Tools/autotest/arducopter.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index d4a92de701..71ac9a8b79 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -374,8 +374,9 @@ def fly_ArduCopter(viewerip=None): os.unlink(buildlog) os.link(logfile, buildlog) - mavproxy.expect('Received [0-9]+ parameters') - mavproxy.expect("Ready to FLY") + # the received parameters can come before or after the ready to fly message + mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) + mavproxy.expect(['Received [0-9]+ parameters', 'Ready to FLY']) util.expect_setup_callback(mavproxy, expect_callback)