From 76815c3b2af2ad7161fc34cb6e8174274da032a3 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sat, 16 Apr 2011 20:44:23 +0000 Subject: [PATCH] moved some math around for Loiter code. Added more notes git-svn-id: https://arducopter.googlecode.com/svn/trunk@1897 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/navigation.pde | 45 ++++++++++++++++++++++++++++------- 1 file changed, 36 insertions(+), 9 deletions(-) diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 746e489edb..f01e1798e2 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -54,14 +54,17 @@ void calc_loiter_nav() Becuase we are using lat and lon to do our distance errors here's a quick chart: 100 = 1m 1000 = 11m - 1800 = 1980m = 60 feet + 1800 = 19.80m = 60 feet 3000 = 33m 10000 = 111m pitch_max = 22° (2200) */ + // X ROLL + long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 500 - 0 = 500 roll EAST + + // Y PITCH + lat_error = current_loc.lat - next_WP.lat; // 0 - 500 = -500 pitch NORTH - long_error = (float)(next_WP.lng - current_loc.lng) * scaleLongDown; // 50 - 30 = 20 pitch right - lat_error = next_WP.lat - current_loc.lat; // 50 - 30 = 20 pitch up long_error = constrain(long_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error lat_error = constrain(lat_error, -DIST_ERROR_MAX, DIST_ERROR_MAX); // +- 20m max error @@ -72,11 +75,38 @@ void calc_loiter_nav() // PITCH Y //nav_lat = lat_error * g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° - nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); + nav_lat = g.pid_nav_lat.get_pid(lat_error, dTnav2, 1.0); // invert lat (for pitch) + + + // nav_lat = -1000 Y Pitch + // nav_lon = 1000 X Roll // rotate the vector - nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; - nav_pitch = -((float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y); + nav_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * -cos_yaw_x; + // BAD + //NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left + //WEST -1000 * 0 - 1000 * -1 = 1000 // roll right - Backwards + //EAST -1000 * 0 - 1000 * 1 = -1000 // roll left - Backwards + //SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right + + // GOOD + //NORTH -1000 * 1 - 1000 * 0 = -1000 // roll left + //WEST -1000 * 0 - 1000 * 1 = -1000 // roll right + //EAST -1000 * 0 - 1000 * -1 = 1000 // roll left + //SOUTH -1000 * -1 - 1000 * 0 = 1000 // roll right + + nav_pitch = ((float)nav_lon * -cos_yaw_x + (float)nav_lat * sin_yaw_y); + // BAD + //NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back + //WEST -1000 * -1 + 1000 * 0 = 1000 // pitch back - Backwards + //EAST -1000 * 1 + 1000 * 0 = -1000 // pitch forward - Backwards + //SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward + + // GOOD + //NORTH -1000 * 0 + 1000 * 1 = 1000 // pitch back + //WEST -1000 * 1 + 1000 * 0 = -1000 // pitch forward + //EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back + //SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward long pmax = g.pitch_max.get(); @@ -87,8 +117,6 @@ void calc_loiter_nav() void calc_waypoint_nav() { nav_lat = constrain((wp_distance * 100), -1800, 1800); // +- 20m max error - //nav_lat = max(wp_distance, -DIST_ERROR_MAX); - //nav_lat = min(wp_distance, DIST_ERROR_MAX); // Scale response by kP nav_lat *= g.pid_nav_lat.kP(); // 1800 * 2 = 3600 or 36° @@ -101,7 +129,6 @@ void calc_waypoint_nav() nav_roll = (float)nav_lat * cos_nav_x; nav_pitch = -(float)nav_lat * sin_nav_y; - long pmax = g.pitch_max.get(); nav_roll = constrain(nav_roll, -pmax, pmax); nav_pitch = constrain(nav_pitch, -pmax, pmax);