From 9732b7d2dcaee7d12c45e4a82217fe4219a35b5a Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 17 May 2012 10:57:00 -0700 Subject: [PATCH] navigation.pde: removed unneeded WP_Distance check remove unneeded return for Navigate removed unneeded rate_D calcs removed unused functions --- ArduCopter/navigation.pde | 171 +------------------------------------- 1 file changed, 2 insertions(+), 169 deletions(-) diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 3efaae5373..38f50ff218 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -3,18 +3,13 @@ //**************************************************************** // Function that will calculate the desired direction to fly and distance //**************************************************************** -static byte navigate() +static void navigate() { // waypoint distance from plane in cm // --------------------------------------- wp_distance = get_distance(¤t_loc, &next_WP); home_distance = get_distance(¤t_loc, &home); - if (wp_distance < 0){ - // something went very wrong - return 0; - } - // target_bearing is where we should be heading // -------------------------------------------- target_bearing = get_bearing(¤t_loc, &next_WP); @@ -23,8 +18,6 @@ static byte navigate() // nav_bearing will includes xtrac correction // ------------------------------------------ nav_bearing = target_bearing; - - return 1; } static bool check_missed_wp() @@ -36,8 +29,8 @@ static bool check_missed_wp() } // ------------------------------ - static void calc_XY_velocity(){ + // called after GPS read // offset calculation of GPS speed: // used for estimations below 1.5m/s // our GPS is about 1m per @@ -80,12 +73,6 @@ static void calc_XY_velocity(){ static void calc_location_error(struct Location *next_loc) { - static int16_t last_lon_error = 0; - static int16_t last_lat_error = 0; - - static int16_t last_lon_d = 0; - static int16_t last_lat_d = 0; - /* Becuase we are using lat and lon to do our distance errors here's a quick chart: 100 = 1m @@ -100,43 +87,6 @@ static void calc_location_error(struct Location *next_loc) // Y Error lat_error = next_loc->lat - current_loc.lat; // 500 - 0 = 500 Go North - - int16_t tmp; - - // ------------------------------------- - tmp = (long_error - last_lon_error); - if(abs(abs(tmp) -last_lon_d) > 20){ - tmp = x_rate_d; - }/* - if(long_error > 0){ - if(tmp < 0) tmp = 0; - }else{ - if(tmp > 0) tmp = 0; - }*/ - x_rate_d = lon_rate_d_filter.apply(tmp); - x_rate_d = constrain(x_rate_d, -800, 800); - last_lon_d = abs(tmp); - - // ------------------------------------- - tmp = (lat_error - last_lat_error); - if(abs(abs(tmp) -last_lat_d) > 20) - tmp = y_rate_d; - /*if(lat_error > 0){ - if(tmp < 0) tmp = 0; - }else{ - if(tmp > 0) tmp = 0; - }*/ - y_rate_d = lat_rate_d_filter.apply(tmp); - y_rate_d = constrain(y_rate_d, -800, 800); - last_lat_d = abs(tmp); - - // debug - //int16_t t22 = x_rate_d * (g.pid_loiter_rate_lon.kD() / dTnav); - //if(control_mode == LOITER) - // Serial.printf("XX, %d, %d, %d \n", long_error, t22, (int16_t)g.pid_loiter_rate_lon.get_integrator()); - - last_lon_error = long_error; - last_lat_error = lat_error; } #define NAV_ERR_MAX 600 @@ -212,54 +162,6 @@ static void calc_loiter(int x_error, int y_error) // copy over I term to Nav_Rate g.pid_nav_lon.set_integrator(g.pid_loiter_rate_lon.get_integrator()); g.pid_nav_lat.set_integrator(g.pid_loiter_rate_lat.get_integrator()); - - //Serial.printf("XX, %d, %d, %d\n", long_error, x_actual_speed, (int16_t)g.pid_loiter_rate_lon.get_integrator()); - - // Wind I term based on location error, - // limit windup - /* - int16_t x_iterm, y_iterm; - x_error = constrain(x_error, -NAV_ERR_MAX, NAV_ERR_MAX); - y_error = constrain(y_error, -NAV_ERR_MAX, NAV_ERR_MAX); - x_iterm = g.pi_loiter_lon.get_i(x_error, dTnav); - y_iterm = g.pi_loiter_lat.get_i(y_error, dTnav); - nav_lat = nav_lat + y_iterm; - nav_lon = nav_lon + x_iterm; - */ - - /* - int8_t ttt = 1.0/dTnav; - int16_t t2 = g.pi_nav_lat.get_integrator(); - - // 1 2 3 4 5 6 7 8 9 10 - Serial.printf("%d, %d, %d, %d, %d, %d, %d, %d, %d, %d\n", - wp_distance, //1 - y_error, //2 - y_GPS_speed, //3 - - y_actual_speed, //4 ; - y_target_speed, //5 - y_rate_error, //6 - nav_lat_p, //7 - nav_lat, //8 - y_iterm, //9 - t2); //10 - //*/ - - /* - 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 - x_error, //3 - x_GPS_speed, //4 - x_actual_speed, //5 - x_target_speed, //6 - x_rate_error, //7 - nav_lat, //8 - x_iterm, //9 - t1); //10 - //*/ } static void calc_nav_rate(int max_speed) @@ -286,77 +188,8 @@ static void calc_nav_rate(int max_speed) g.pid_loiter_rate_lon.set_integrator(g.pid_nav_lon.get_integrator()); g.pid_loiter_rate_lat.set_integrator(g.pid_nav_lat.get_integrator()); - //int16_t x_iterm = g.pi_loiter_lon.get_i(x_rate_error, dTnav); - //int16_t y_iterm = g.pi_loiter_lat.get_i(y_rate_error, dTnav); - - //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 = constrain(nav_lon, -3000, 3000); -} - -static void calc_nav_lat(int rate) -{ - nav_lat = g.pid_nav_lat.get_pid(rate, dTnav); - 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 - - - // 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 static void calc_loiter_pitch_roll()