From f2e33f830345ae05b87a7e66f4a9a5f74aa82745 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:27:56 -0700 Subject: [PATCH 1/7] removed nav_bearing - not used --- ArduCopter/commands.pde | 1 - 1 file changed, 1 deletion(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 10d5fb31f3..7984e7c6c7 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -190,7 +190,6 @@ static void set_next_WP(struct Location *wp) wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; target_bearing = get_bearing(¤t_loc, &next_WP); - nav_bearing = target_bearing; // to check if we have missed the WP // ---------------------------- From 689bfb803ceb6e3b9e6f57f13777a11112999461 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:28:43 -0700 Subject: [PATCH 2/7] increased imax --- ArduCopter/config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 29514f99c1..144c8f650d 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -482,10 +482,10 @@ # define NAV_P 2.0 // for 4.5 ms error = 13.5 pitch #endif #ifndef NAV_I -# define NAV_I 0.1 // this +# define NAV_I 0.12 // this #endif #ifndef NAV_IMAX -# define NAV_IMAX 16 // degrees +# define NAV_IMAX 20 // degrees #endif /* From 1ae63e2a4ce31aa217266d212a6c11d517e2dedc Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:32:58 -0700 Subject: [PATCH 3/7] removed Nav_bearing --- ArduCopter/Log.pde | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index d67fabf2a8..9f1ef3a578 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -537,9 +537,8 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteInt((int)wp_distance); // 1 DataFlash.WriteByte(wp_verify_byte); // 2 DataFlash.WriteInt((int)(target_bearing/100)); // 3 - DataFlash.WriteInt((int)(nav_bearing/100)); // 4 - DataFlash.WriteInt((int)long_error); // 5 - DataFlash.WriteInt((int)lat_error); // 6 + DataFlash.WriteInt((int)long_error); // 4 + DataFlash.WriteInt((int)lat_error); // 5 /* @@ -568,7 +567,6 @@ static void Log_Read_Nav_Tuning() DataFlash.ReadInt(), // distance DataFlash.ReadByte(), // wp_verify_byte DataFlash.ReadInt(), // target_bearing - DataFlash.ReadInt(), // nav_bearing DataFlash.ReadInt(), // long_error DataFlash.ReadInt()); // lat_error From efeb1555ba8160f0e15e7207a92b09e1e55f66f1 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:33:29 -0700 Subject: [PATCH 4/7] removed nav_bearing --- ArduCopter/Mavlink_Common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/Mavlink_Common.h b/ArduCopter/Mavlink_Common.h index 7d54f4ef80..a04ffecd4c 100644 --- a/ArduCopter/Mavlink_Common.h +++ b/ArduCopter/Mavlink_Common.h @@ -142,7 +142,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_ chan, nav_roll / 1.0e2, nav_pitch / 1.0e2, - nav_bearing / 1.0e2, + target_bearing / 1.0e2, target_bearing / 1.0e2, wp_distance, altitude_error / 1.0e2, From 7d57dfa3ecd9338dca429b73399c5b61011389c2 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:36:20 -0700 Subject: [PATCH 5/7] removed Xtrack and increased rate error limit --- ArduCopter/navigation.pde | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 6cddf96010..3b78dadf90 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -34,11 +34,12 @@ static void navigate() // nav_bearing will include xtrac correction // ----------------------------------------- //xtrack_enabled = false; - if(xtrack_enabled){ - nav_bearing = wrap_360(target_bearing + get_crosstrack_correction()); - }else{ - nav_bearing = target_bearing; - } + + //if(xtrack_enabled){ + // crosstrack_correction = get_crosstrack_correction(); + //}else { + // crosstrack_correction = 0; + //} } static bool check_missed_wp() @@ -101,18 +102,20 @@ static void calc_nav_rate(int x_error, int y_error, int max_speed, int min_speed } // find the rates: + float temp = radians((float)g_gps->ground_course/100.0); + // calc the cos of the error to tell how fast we are moving towards the target in cm - y_actual_speed = (float)g_gps->ground_speed * cos(radians((float)g_gps->ground_course/100.0)); + y_actual_speed = (float)g_gps->ground_speed * cos(temp); y_rate_error = y_target_speed - y_actual_speed; // 413 - y_rate_error = constrain(y_rate_error, -250, 250); // added a rate error limit to keep pitching down to a minimum + y_rate_error = constrain(y_rate_error, -600, 600); // added a rate error limit to keep pitching down to a minimum nav_lat = constrain(g.pi_nav_lat.get_pi(y_rate_error, dTnav), -3500, 3500); //Serial.printf("yr: %d, nav_lat: %d, int:%d \n",y_rate_error, nav_lat, g.pi_nav_lat.get_integrator()); // calc the sin of the error to tell how fast we are moving laterally to the target in cm - x_actual_speed = (float)g_gps->ground_speed * sin(radians((float)g_gps->ground_course/100.0)); + x_actual_speed = (float)g_gps->ground_speed * sin(temp); x_rate_error = x_target_speed - x_actual_speed; - x_rate_error = constrain(x_rate_error, -250, 250); + x_rate_error = constrain(x_rate_error, -600, 600); nav_lon = constrain(g.pi_nav_lon.get_pi(x_rate_error, dTnav), -3500, 3500); } @@ -128,11 +131,11 @@ static void calc_nav_pitch_roll() } // ------------------------------ -static void calc_bearing_error() +/*static void calc_bearing_error() { bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = wrap_180(bearing_error); -} +}*/ static long get_altitude_error() { @@ -189,6 +192,7 @@ static long wrap_180(long error) return error; } +/* static long get_crosstrack_correction(void) { // Crosstrack Error @@ -206,7 +210,7 @@ static long get_crosstrack_correction(void) } return 0; } - +*/ static long cross_track_test() { From 7e7961212511da7dc48b112369e771a5c32cf041 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 11 Sep 2011 20:40:05 -0700 Subject: [PATCH 6/7] Removed last of Xtrack --- ArduCopter/ArduCopter.pde | 18 ++++++++++-------- ArduCopter/HIL_Xplane.pde | 2 +- ArduCopter/commands.pde | 2 +- ArduCopter/navigation.pde | 7 ++++--- ArduCopter/system.pde | 10 +++++----- 5 files changed, 21 insertions(+), 18 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 541a1ec586..d377ee26bf 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -316,11 +316,13 @@ static bool did_ground_start = false; // have we ground started after first ar // --------------------- static const float radius_of_earth = 6378100; // meters static const float gravity = 9.81; // meters/ sec^2 -static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate +//static long nav_bearing; // deg * 100 : 0 to 360 current desired bearing to navigate static long target_bearing; // deg * 100 : 0 to 360 location of the plane to the target -static bool xtrack_enabled = false; -static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +//static bool xtrack_enabled = false; +//static long crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target +//static long crosstrack_correction; // deg * 100 : 0 to 360 desired angle of plane to target + static int climb_rate; // m/s * 100 - For future implementation of controlled ascent/descent by rate static long circle_angle = 0; static byte wp_control; // used to control - navgation or loiter @@ -349,7 +351,7 @@ static int airspeed; // m/s * 100 // Location Errors // --------------- -static long bearing_error; // deg * 100 : 0 to 36000 +//static long bearing_error; // deg * 100 : 0 to 36000 static long altitude_error; // meters * 100 we are off in altitude static long old_altitude; static long yaw_error; // how off are we pointed @@ -674,8 +676,8 @@ static void medium_loop() // ------------------------------------------------------ navigate(); - // control mode specific updates to nav_bearing - // -------------------------------------------- + // control mode specific updates + // ----------------------------- update_navigation(); if (g.log_bitmask & MASK_LOG_NTUN) @@ -799,7 +801,7 @@ static void fifty_hz_loop() #endif // use Yaw to find our bearing error - calc_bearing_error(); + //calc_bearing_error(); //if (throttle_slew < 0) // throttle_slew++; @@ -1246,7 +1248,7 @@ static void update_navigation() wp_control = WP_MODE; }else{ set_mode(LOITER); - xtrack_enabled = false; + //xtrack_enabled = false; } diff --git a/ArduCopter/HIL_Xplane.pde b/ArduCopter/HIL_Xplane.pde index 8f9a748e74..311b9da649 100644 --- a/ArduCopter/HIL_Xplane.pde +++ b/ArduCopter/HIL_Xplane.pde @@ -12,7 +12,7 @@ void HIL_XPLANE::output_HIL(void) output_int((int)(g.rc_3.servo_out)); // 2 bytes 4, 5 output_int((int)(g.rc_4.servo_out)); // 3 bytes 6, 7 output_int((int)wp_distance); // 4 bytes 8,9 - output_int((int)bearing_error); // 5 bytes 10,11 + //output_int((int)bearing_error); // 5 bytes 10,11 output_int((int)altitude_error); // 6 bytes 12, 13 output_int((int)energy_error); // 7 bytes 14,15 output_byte((int)g.waypoint_index); // 8 bytes 16 diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 7984e7c6c7..427fc04b6c 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -197,7 +197,7 @@ static void set_next_WP(struct Location *wp) // set a new crosstrack bearing // ---------------------------- - crosstrack_bearing = target_bearing; // Used for track following + //crosstrack_bearing = target_bearing; // Used for track following gcs.print_current_waypoints(); } diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 3b78dadf90..a40ef79f27 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -211,18 +211,19 @@ static long get_crosstrack_correction(void) return 0; } */ - +/* static long cross_track_test() { long temp = wrap_180(target_bearing - crosstrack_bearing); return abs(temp); } - +*/ +/* static void reset_crosstrack() { crosstrack_bearing = get_bearing(¤t_loc, &next_WP); // Used for track following } - +*/ static long get_altitude_above_home(void) { // This is the altitude above the home location diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 70daa8e178..f9ce432993 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -41,7 +41,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = { }; // Create the top-level menu object. -MENU(main_menu, "ArduCopter 2.0.42 Beta", main_menu_commands); +MENU(main_menu, "ArduCopter 2.0.43 Beta", main_menu_commands); #endif // CLI_ENABLED @@ -362,7 +362,7 @@ static void set_mode(byte mode) led_mode = NORMAL_LEDS; // most modes do not calculate crosstrack correction - xtrack_enabled = false; + //xtrack_enabled = false; reset_nav_I(); switch(control_mode) @@ -411,7 +411,7 @@ static void set_mode(byte mode) // do crosstrack correction // XXX move to flight commands - xtrack_enabled = true; + //xtrack_enabled = true; break; case CIRCLE: @@ -437,7 +437,7 @@ static void set_mode(byte mode) roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_AUTO; - xtrack_enabled = true; + //xtrack_enabled = true; init_throttle_cruise(); next_WP = current_loc; break; @@ -447,7 +447,7 @@ static void set_mode(byte mode) roll_pitch_mode = RTL_RP; throttle_mode = RTL_THR; - xtrack_enabled = true; + //xtrack_enabled = true; init_throttle_cruise(); do_RTL(); break; From ae77029c9892dc9c4686b0183ad816caa926406d Mon Sep 17 00:00:00 2001 From: Mike Smith Date: Sun, 11 Sep 2011 22:20:54 -0700 Subject: [PATCH 7/7] Reinstate a Cygwin workround that was backed out by the previous changes. Not all platforms support (or require) the -r argument to sed. --- config.mk | 2 ++ libraries/AP_Common/Arduino.mk | 7 ++++++- 2 files changed, 8 insertions(+), 1 deletion(-) create mode 100644 config.mk diff --git a/config.mk b/config.mk new file mode 100644 index 0000000000..8375e45334 --- /dev/null +++ b/config.mk @@ -0,0 +1,2 @@ +BOARD=mega +PORT=/dev/null diff --git a/libraries/AP_Common/Arduino.mk b/libraries/AP_Common/Arduino.mk index ae3f3e572e..287684a060 100644 --- a/libraries/AP_Common/Arduino.mk +++ b/libraries/AP_Common/Arduino.mk @@ -259,7 +259,12 @@ SKETCHCPP_SRC := $(SKETCHPDE) $(sort $(filter-out $(SKETCHPDE),$(SKETCHPDESRCS) # make. # SEXPR = 's/^[[:space:]]*\#include[[:space:]][<\"]([^>\"./]+).*$$/\1/p' -LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) +ifneq ($(findstring CYGWIN, $(SYSTYPE)),) + # Workaround a cygwin issue + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nre $(SEXPR))) +else + LIBTOKENS := $(sort $(shell cat $(SKETCHPDESRCS) $(SKETCHSRCS) | sed -nEe $(SEXPR))) +endif # # Find sketchbook libraries referenced by the sketch.