From b7ce6e036eb71b57d6f6e9906e140b668f111e8f Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 8 Aug 2011 16:54:56 +0000 Subject: [PATCH] 2.0.39 Change the way Rate nav works. we now take into account both forward *and lateral* rates. The signs may be backward so be careful! Let me know if the quad shoots off to one side and I'll flip the signs. git-svn-id: https://arducopter.googlecode.com/svn/trunk@3049 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/APM_Config.h | 8 ++++---- ArduCopterMega/ArduCopterMega.pde | 29 ++++++++++++++++++++--------- ArduCopterMega/navigation.pde | 29 ++++++++++++++++++++--------- 3 files changed, 44 insertions(+), 22 deletions(-) diff --git a/ArduCopterMega/APM_Config.h b/ArduCopterMega/APM_Config.h index dcb06b7b89..97a846f2e7 100644 --- a/ArduCopterMega/APM_Config.h +++ b/ArduCopterMega/APM_Config.h @@ -9,7 +9,7 @@ //#define BROKEN_SLIDER 0 // 1 = yes (use Yaw to enter CLI mode) -//#define FRAME_CONFIG QUAD_FRAME +#define FRAME_CONFIG QUAD_FRAME /* options: QUAD_FRAME @@ -20,7 +20,7 @@ HELI_FRAME */ -//#define FRAME_ORIENTATION X_FRAME +#define FRAME_ORIENTATION X_FRAME /* PLUS_FRAME X_FRAME @@ -28,7 +28,7 @@ */ -//#define CHANNEL_6_TUNING CH6_NONE +#define CHANNEL_6_TUNING CH6_NONE /* CH6_NONE CH6_STABILIZE_KP @@ -43,7 +43,7 @@ CH6_YAW_RATE_KI CH6_TOP_BOTTOM_RATIO CH6_PMAX - CH6_RELAY + CH6_RELAY */ // experimental!! diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 88312c2e25..d177b26def 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -1336,24 +1336,36 @@ static void update_navigation() case GUIDED: case RTL: - if(wp_distance > 20){ + if(wp_distance > 5){ // calculates desired Yaw // XXX this is an experiment #if FRAME_CONFIG == HELI_FRAME update_nav_yaw(); #endif - }else{ - // Don't Yaw anymore - // hack to elmininate crosstrack effect - crosstrack_bearing = target_bearing; } + //if(wp_distance < 4){ + // clears crosstrack + crosstrack_bearing = target_bearing; + //wp_control = WP_MODE; + //}else{ + //wp_control = LOITER_MODE; + //} + + wp_control = WP_MODE; + // are we Traversing or Loitering? - wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE; + //wp_control = (wp_distance < 4 ) ? LOITER_MODE : WP_MODE; // calculates the desired Roll and Pitch - update_nav_wp(); + //update_nav_wp(); + + // calc a rate dampened pitch to the target + calc_rate_nav(g.waypoint_speed_max.get()); + + // rotate that pitch to the copter frame of reference + calc_nav_output(); break; // switch passthrough to LOITER @@ -1378,11 +1390,10 @@ static void update_navigation() update_loiter(); // calc a rate dampened pitch to the target - calc_rate_nav(200); + calc_rate_nav(400); // rotate that pitch to the copter frame of reference calc_nav_output(); - break; } diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index b9acde6f25..f4a5139a79 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -133,20 +133,25 @@ static void calc_nav_output() float cos_nav_x = cos(radians((float)(bearing_error - 9000) / 100)); // rotate the vector - nav_roll = (float)nav_lat * cos_nav_x; - nav_pitch = -(float)nav_lat * sin_nav_y; + //nav_roll = (float)nav_lat * cos_nav_x; + //nav_pitch = -(float)nav_lat * sin_nav_y; + nav_roll = (float)nav_lon * sin_nav_y - (float)nav_lat * -cos_nav_x; + nav_pitch = (float)nav_lon * cos_nav_x - (float)nav_lat * sin_nav_y; } // called after we get GPS read static void calc_rate_nav(int speed) { // which direction are we moving? - long target_error = nav_bearing - g_gps->ground_course; - target_error = wrap_180(target_error); + long heading_error = nav_bearing - g_gps->ground_course; + heading_error = wrap_180(heading_error); // calc the cos of the error to tell how fast we are moving towards the target in cm - int groundspeed = (float)g_gps->ground_speed * cos(radians((float)target_error/100)); - groundspeed = max(groundspeed, 0); + int targetspeed = (float)g_gps->ground_speed * cos(radians((float)heading_error/100)); + + // calc the sin of the error to tell how fast we are moving laterally to the target in cm + int lateralspeed = (float)g_gps->ground_speed * sin(radians((float)heading_error/100)); + //targetspeed = max(targetspeed, 0); // Reduce speed on RTL if(control_mode == RTL){ @@ -160,12 +165,15 @@ static void calc_rate_nav(int speed) //waypoint_speed = g.waypoint_speed_max.get(); } - int error = constrain(waypoint_speed - groundspeed, -1000, 1000); + int error = constrain(waypoint_speed - targetspeed, -1000, 1000); // Scale response by kP nav_lat += g.pid_nav_wp.get_pid(error, dTnav, 1.0); nav_lat >>= 1; // divide by two for smooting - //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, groundspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); + nav_lon += lateralspeed * 2; // 2 is our fake PID gain + nav_lon >>= 1; + + //Serial.printf("dTnav: %ld, gs: %d, err: %d, int: %d, pitch: %ld", dTnav, targetspeed, error, (int)g.pid_nav_wp.get_integrator(), (long)nav_lat); // limit our output nav_lat = constrain(nav_lat, -3500, 3500); // +- max error @@ -239,7 +247,10 @@ static void update_crosstrack(void) // Crosstrack Error // ---------------- if (cross_track_test() < 9000) { // If we are too far off or too close we don't do track following - crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line + // Meters we are off track line + crosstrack_error = sin(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; + + // take meters * 100 to get adjustment to nav_bearing long xtrack = g.pid_crosstrack.get_pid(crosstrack_error, dTnav, 1.0) * 100; nav_bearing += constrain(xtrack, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get()); nav_bearing = wrap_360(nav_bearing);