From 1410063a14eef3a47d7d8d2bab25fc9a97b7f3fe Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Feb 2013 17:51:56 +0900 Subject: [PATCH] Copter: new ROLL_PITCH_WP_INAV roll-pitch mode small corrections to allow waypoints to work with new inertial nav wp controller --- ArduCopter/ArduCopter.pde | 10 ++++++++++ ArduCopter/commands.pde | 17 +++++++++++------ ArduCopter/defines.h | 1 + ArduCopter/navigation.pde | 7 ++++--- 4 files changed, 26 insertions(+), 9 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 88814aaf60..11191932ee 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1614,6 +1614,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) break; case ROLL_PITCH_LOITER_INAV: + case ROLL_PITCH_WP_INAV: // require gps lock if( ap.home_is_set ) { roll_pitch_initialised = true; @@ -1733,6 +1734,15 @@ void update_roll_pitch_mode(void) get_stabilize_roll(nav_roll); get_stabilize_pitch(nav_pitch); break; + + case ROLL_PITCH_WP_INAV: + // To-Do: allow pilot to take control of target location + // copy latest output from nav controller to stabilize controller + nav_roll = auto_roll; + nav_pitch = auto_pitch; + get_stabilize_roll(nav_roll); + get_stabilize_pitch(nav_pitch); + break; } #if FRAME_CONFIG != HELI_FRAME diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 61b3d5dcd3..64941538d4 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -150,25 +150,30 @@ static void set_next_WP(struct Location *wp) // To-Do: remove the restriction on negative altitude targets (after testing) set_new_altitude(max(wp->alt,100)); - // this is handy for the groundstation - // ----------------------------------- + // recalculate waypoint distance and bearing wp_distance = get_distance_cm(¤t_loc, &next_WP); wp_bearing = get_bearing_cd(&prev_WP, &next_WP); + original_wp_bearing = wp_bearing; // to check if we have missed the WP // calc the location error: calc_location_error(&next_WP); - - // to check if we have missed the WP - // --------------------------------- - original_wp_bearing = wp_bearing; } // set_next_WP_latlon - update just lat and lon targets for nav controllers static void set_next_WP_latlon(uint32_t lat, uint32_t lon) { + // save current location as previous location + prev_WP = current_loc; + // Load the next_WP slot next_WP.lat = lat; next_WP.lng = lon; + + // recalculate waypoint distance and bearing + // To-Do: these functions are too expensive to be called every time we update the next_WP + wp_distance = get_distance_cm(¤t_loc, &next_WP); + wp_bearing = get_bearing_cd(&prev_WP, &next_WP); + original_wp_bearing = wp_bearing; // to check if we have missed the WP } // run this at setup on the ground diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1c40be0c91..ad663ea43a 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -31,6 +31,7 @@ #define ROLL_PITCH_STABLE_OF 3 #define ROLL_PITCH_TOY 4 // THOR This is the Roll and Pitch mode #define ROLL_PITCH_LOITER_INAV 5 // pilot inputs the desired horizontal velocities +#define ROLL_PITCH_WP_INAV 6 // pilot inputs the desired horizontal velocities which temporarily interrupt the autopilot #define THROTTLE_MANUAL 0 // manual throttle mode - pilot input goes directly to motors #define THROTTLE_MANUAL_TILT_COMPENSATED 1 // mostly manual throttle but with some tilt compensation diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 9b25c2207e..3035af79aa 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -264,7 +264,8 @@ static bool set_nav_mode(uint8_t new_nav_mode) break; case NAV_WP_INAV: - nav_initialised = true; + // To-Do: below allows user to move around set point but do we want to allow this when in Auto flight mode? + nav_initialised = set_roll_pitch_mode(ROLL_PITCH_WP_INAV); break; } @@ -397,7 +398,7 @@ static bool check_missed_wp() int32_t temp; temp = wp_bearing - original_wp_bearing; temp = wrap_180(temp); - return (labs(temp) > 9000); // we passed the waypoint by 100 degrees + return (labs(temp) > 9000); // we passed the waypoint by 90 degrees } //////////////////////////////////////////////////////////////// @@ -538,7 +539,7 @@ static void calc_nav_rate(int16_t max_speed) static void calc_nav_pitch_roll() { // To-Do: remove this hack dependent upon nav_mode - if( nav_mode != NAV_LOITER_INAV ) { + if( nav_mode != NAV_LOITER_INAV && nav_mode != NAV_WP_INAV ) { // rotate the vector auto_roll = (float)nav_lon * sin_yaw_y - (float)nav_lat * cos_yaw_x; auto_pitch = (float)nav_lon * cos_yaw_x + (float)nav_lat * sin_yaw_y;