From fffa1af3b35600c4baafc2d62fb69c616f2a638d Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sat, 21 Jan 2012 22:12:57 -0800 Subject: [PATCH] Fixed speed governor which was letting speed get to 0. added Loiter_d to replace Nav_P for rate control wp_distance calc now returns CM --- ArduCopter/navigation.pde | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index c6ca18db00..02eabe763d 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -167,7 +167,8 @@ 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 = g.pi_nav_lon.get_p(x_rate_error); + 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); @@ -177,7 +178,8 @@ static void calc_loiter(int x_error, int y_error) 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 = g.pi_nav_lat.get_p(y_rate_error); + 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); @@ -270,21 +272,20 @@ static int16_t calc_desired_speed(int16_t max_speed) */ // max_speed is default 600 or 6m/s - // (wp_distance * 50) = 1/2 of the distance converted to speed + // (wp_distance * .5) = 1/2 of the distance converted to speed // wp_distance is always in m/s and not cm/s - I know it's stupid that way // for example 4m from target = 200cm/s speed // we choose the lowest speed based on disance - max_speed = min(max_speed, (wp_distance * 100)); + max_speed = min(max_speed, wp_distance); + + // go at least 100cm/s + max_speed = max(max_speed, WAYPOINT_SPEED_MIN); // limit the ramp up of the speed // waypoint_speed_gov is reset to 0 at each new WP command - if(waypoint_speed_gov < max_speed){ + if(max_speed > waypoint_speed_gov){ waypoint_speed_gov += (int)(100.0 * dTnav); // increase at .5/ms - - // go at least 100cm/s - max_speed = max(100, waypoint_speed_gov); - // limit with governer - max_speed = min(max_speed, waypoint_speed_gov); + max_speed = waypoint_speed_gov; } return max_speed; @@ -296,7 +297,7 @@ static void calc_nav_rate(int max_speed) update_crosstrack(); // nav_bearing includes crosstrack - float temp = (9000 - nav_bearing) * RADX100; + 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); @@ -398,7 +399,6 @@ static void set_new_altitude(int32_t _new_alt) alt_change_flag = REACHED_ALT; //Serial.printf("reached alt\n"); } - //Serial.printf("new alt: %d Org alt: %d\n", target_altitude, original_altitude); } @@ -490,16 +490,14 @@ static int32_t wrap_180(int32_t error) return current_loc.alt - home.alt; } */ + // distance is returned in meters static int32_t get_distance(struct Location *loc1, struct Location *loc2) { - //if(loc1->lat == 0 || loc1->lng == 0) - // return -1; - //if(loc2->lat == 0 || loc2->lng == 0) - // return -1; float dlat = (float)(loc2->lat - loc1->lat); float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown; - return sqrt(sq(dlat) + sq(dlong)) * .01113195; + dlong = sqrt(sq(dlat) + sq(dlong)) * 1.113195; + return dlong; } /* //static int32_t get_alt_distance(struct Location *loc1, struct Location *loc2)