diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 70462e4cdc..87be304c40 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -700,6 +700,14 @@ static int32_t home_distance; // distance between plane and next_WP in cm // is not static because AP_Camera uses it uint32_t wp_distance; +// wpinav variables +Vector2f wpinav_origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP) +Vector2f wpinav_destination; // target destination in cm from home (equivalent to next_WP) +Vector2f wpinav_target; // the intermediate target location in cm from home +Vector2f wpinav_pos_delta; // position difference between origin and destination +float wpinav_track_length; // distance in cm between origin and destination +float wpinav_track_desired; // the desired distance along the track in cm + //////////////////////////////////////////////////////////////////////////////// // 3D Location vectors @@ -1777,9 +1785,9 @@ void update_simple_mode(void) g.rc_2.control_in = _pitch; } -// update_super_simple_beading - adjusts simple bearing based on location +// update_super_simple_bearing - adjusts simple bearing based on location // should be called after home_bearing has been updated -void update_super_simple_beading() +void update_super_simple_bearing() { // are we in SIMPLE mode? if(ap.simple_mode && g.super_simple) { diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index fbb5b6a0ae..ca1e0364bc 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -161,8 +161,25 @@ static void set_next_WP(struct Location *wp) // to check if we have missed the WP // --------------------------------- original_wp_bearing = wp_bearing; + +#if NAV_WP_ACTIVE == NAV_WP_INAV + // set inertial navigation controller's target incase it's called + wpinav_set_origin_and_destination(prev_WP, next_WP); +#endif } +// 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) +{ + // Load the next_WP slot + next_WP.lat = lat; + next_WP.lng = lon; + +#if NAV_WP_ACTIVE == NAV_WP_INAV + // set inertial navigation controller's target incase it's called + wpinav_set_destination(next_WP); +#endif +} // run this at setup on the ground // ------------------------------- diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 923965d8ad..192c828764 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -226,7 +226,7 @@ static void do_RTL(void) set_throttle_mode(RTL_THR); // set navigation mode - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); // initial climb starts at current location set_next_WP(¤t_loc); @@ -242,7 +242,7 @@ static void do_RTL(void) // do_takeoff - initiate takeoff navigation command static void do_takeoff() { - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); // Start with current location Location temp = current_loc; @@ -263,7 +263,7 @@ static void do_takeoff() // do_nav_wp - initiate move to next waypoint static void do_nav_wp() { - set_nav_mode(NAV_WP); + set_nav_mode(NAV_WP_ACTIVE); set_next_WP(&command_nav_queue); @@ -292,7 +292,7 @@ static void do_land() { // hold at our current location set_next_WP(¤t_loc); - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); // hold current heading set_yaw_mode(YAW_HOLD); @@ -302,15 +302,15 @@ static void do_land() static void do_loiter_unlimited() { - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); //cliSerial->println("dloi "); if(command_nav_queue.lat == 0) { set_next_WP(¤t_loc); - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); }else{ set_next_WP(&command_nav_queue); - set_nav_mode(NAV_WP); + set_nav_mode(NAV_WP_ACTIVE); } } @@ -344,11 +344,12 @@ static void do_loiter_turns() static void do_loiter_time() { if(command_nav_queue.lat == 0) { - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); loiter_time = millis(); set_next_WP(¤t_loc); }else{ - set_nav_mode(NAV_WP); + set_nav_mode(NAV_WP_ACTIVE); + set_next_WP(&command_nav_queue); } @@ -403,12 +404,10 @@ static bool verify_nav_wp() // we have reached our goal // loiter at the WP - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); if ((millis() - loiter_time) > loiter_time_max) { wp_verify_byte |= NAV_DELAY; - //gcs_send_text_P(SEVERITY_LOW,PSTR("verify_must: LOITER time complete")); - //cliSerial->println("vlt done"); } } @@ -424,9 +423,9 @@ static bool verify_nav_wp() static bool verify_loiter_unlimited() { - if(nav_mode == NAV_WP && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) { + if(nav_mode == NAV_WP_ACTIVE && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) { // switch to position hold - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); } return false; } @@ -434,16 +433,16 @@ static bool verify_loiter_unlimited() // verify_loiter_time - check if we have loitered long enough static bool verify_loiter_time() { - if(nav_mode == NAV_LOITER) { + if(nav_mode == NAV_LOITER_ACTIVE) { if ((millis() - loiter_time) > loiter_time_max) { return true; } } - if(nav_mode == NAV_WP && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) { + if(nav_mode == NAV_WP_ACTIVE && wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0)) { // reset our loiter time loiter_time = millis(); // switch to position hold - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); } return false; } @@ -483,7 +482,7 @@ static bool verify_RTL() set_new_altitude(get_RTL_alt()); // set navigation mode - set_nav_mode(NAV_WP); + set_nav_mode(NAV_WP_ACTIVE); // set yaw mode set_yaw_mode(RTL_YAW); @@ -497,7 +496,7 @@ static bool verify_RTL() // if we've reached home initiate loiter if (wp_distance <= (uint32_t)max((g.waypoint_radius * 100),0) || check_missed_wp()) { rtl_state = RTL_STATE_LOITERING_AT_HOME; - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); // set loiter timer rtl_loiter_start_time = millis(); @@ -515,8 +514,7 @@ static bool verify_RTL() // land do_land(); // override landing location (do_land defaults to current location) - next_WP.lat = home.lat; - next_WP.lng = home.lng; + set_next_WP_latlon(home.lat, home.lng); // update RTL state rtl_state = RTL_STATE_LAND; }else{ diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 848bba535f..2d5e9569f7 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -607,6 +607,15 @@ #define EARTH_FRAME 0 #define BODY_FRAME 1 +// active NAV controller +#ifndef NAV_WP_ACTIVE + # define NAV_WP_ACTIVE NAV_WP +#endif +// active LOITER controller +#ifndef NAV_LOITER_ACTIVE + # define NAV_LOITER_ACTIVE NAV_LOITER +#endif + // Flight mode roll, pitch, yaw, throttle and navigation definitions // Acro Mode @@ -704,7 +713,7 @@ #endif #ifndef LOITER_NAV - # define LOITER_NAV NAV_LOITER + # define LOITER_NAV NAV_LOITER_ACTIVE #endif // POSITION Mode @@ -721,7 +730,7 @@ #endif #ifndef POSITION_NAV - # define POSITION_NAV NAV_LOITER + # define POSITION_NAV NAV_LOITER_ACTIVE #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 78916cef56..1c40be0c91 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -200,6 +200,7 @@ #define NAV_LOITER 2 #define NAV_WP 3 #define NAV_LOITER_INAV 4 +#define NAV_WP_INAV 5 // Yaw override behaviours - used for setting yaw_override_behaviour #define YAW_OVERRIDE_BEHAVIOUR_AT_NEXT_WAYPOINT 0 // auto pilot takes back yaw control at next waypoint diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 2f5f1ab5db..db792c962e 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -168,7 +168,7 @@ static void calc_distance_and_bearing() home_bearing = get_bearing_cd(¤t_loc, &home); // update super simple bearing (if required) because it relies on home_bearing - update_super_simple_beading(); + update_super_simple_bearing(); // bearing to target (used when yaw_mode = YAW_LOOK_AT_LOCATION) yaw_look_at_WP_bearing = get_bearing_cd(¤t_loc, &yaw_look_at_WP); @@ -203,7 +203,7 @@ static void run_autopilot() case GUIDED: // switch to loiter once we've reached the target location and altitude if(verify_nav_wp()) { - set_nav_mode(NAV_LOITER); + set_nav_mode(NAV_LOITER_ACTIVE); } case RTL: verify_RTL(); @@ -249,6 +249,10 @@ static bool set_nav_mode(uint8_t new_nav_mode) case NAV_LOITER_INAV: loiter_set_target(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff()); + nav_initialised = set_roll_pitch_mode(ROLL_PITCH_LOITER_INAV); + break; + + case NAV_WP_INAV: nav_initialised = true; break; } @@ -296,8 +300,10 @@ static void update_nav_mode() if (circle_angle > 6.28318531f) circle_angle -= 6.28318531f; - next_WP.lng = circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp); - next_WP.lat = circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle)); + // update target location + set_next_WP_latlon( + circle_WP.lat + (g.circle_radius * 100 * sinf(1.57f - circle_angle)), + circle_WP.lng + (g.circle_radius * 100 * cosf(1.57f - circle_angle) * scaleLongUp)); // use error as the desired rate towards the target // nav_lon, nav_lat is calculated @@ -326,8 +332,7 @@ static void update_nav_mode() if(g.rc_2.control_in == 0 && g.rc_1.control_in == 0) { ap.loiter_override = false; // reset LOITER to current position - next_WP.lat = current_loc.lat; - next_WP.lng = current_loc.lng; + set_next_WP_latlon(current_loc.lat, current_loc.lng); } // We bring copy over our Iterms for wind control, but we don't navigate nav_lon = g.pid_loiter_rate_lon.get_integrator(); @@ -354,6 +359,10 @@ static void update_nav_mode() case NAV_LOITER_INAV: get_loiter_pos_lat_lon(loiter_lat_from_home_cm, loiter_lon_from_home_cm, 0.1f); break; + + case NAV_WP_INAV: + get_wpinav_pos(0.1f); + break; } /* @@ -759,7 +768,7 @@ get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt) #define MAX_LOITER_POS_VELOCITY 750 // should be 1.5 ~ 2.0 times the pilot input's max velocity #define MAX_LOITER_POS_ACCEL 250 static void -get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon, float dt) +get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_home, float dt) { static float dist_error_lat; int32_t desired_vel_lat; @@ -778,8 +787,8 @@ get_loiter_pos_lat_lon(int32_t target_lat, int32_t target_lon, float dt) // 100hz sample rate, 2hz filter, alpha = 0.11164f // 20hz sample rate, 2hz filter, alpha = 0.38587f // 10hz sample rate, 2hz filter, alpha = 0.55686f - dist_error_lat = dist_error_lat + 0.55686f * ((target_lat - inertial_nav.get_latitude_diff()) - dist_error_lat); - dist_error_lon = dist_error_lon + 0.55686f * ((target_lon - inertial_nav.get_longitude_diff()) - dist_error_lon); + dist_error_lat = dist_error_lat + 0.55686f * ((target_lat_from_home - inertial_nav.get_latitude_diff()) - dist_error_lat); + dist_error_lon = dist_error_lon + 0.55686f * ((target_lon_from_home - inertial_nav.get_longitude_diff()) - dist_error_lon); linear_distance = MAX_LOITER_POS_ACCEL/(2*g.pi_loiter_lat.kP()*g.pi_loiter_lat.kP()); @@ -842,3 +851,91 @@ loiter_set_target(float lat_from_home_cm, float lon_from_home_cm) next_WP.lat = home.lat + loiter_lat_from_home_cm; next_WP.lng = home.lng + loiter_lat_from_home_cm * scaleLongUp; } + +////////////////////////////////////////////////////////// +// waypoint inertial navigation controller +////////////////////////////////////////////////////////// +// Waypoint navigation is accomplished by moving the target location up to a maximum of 10m from the current location + +// get_wpinav_pos - wpinav position controller with desired position held in wpinav_destination +static void +get_wpinav_pos(float dt) +{ + // reuse loiter position controller + get_loiter_pos_lat_lon(wpinav_target.x, wpinav_target.y, dt); +} + +// wpinav_set_destination - set destination using lat/lon coordinates +void wpinav_set_destination(Location& destination) +{ + wpinav_origin.x = inertial_nav.get_latitude_diff(); + wpinav_origin.y = inertial_nav.get_longitude_diff(); + wpinav_destination.x = (destination.lat-home.lat) * LATLON_TO_CM; + wpinav_destination.y = (destination.lng-home.lng) * LATLON_TO_CM * scaleLongDown; + wpinav_pos_delta = wpinav_destination - wpinav_origin; + wpinav_track_length = wpinav_pos_delta.length(); + wpinav_track_desired = 0; +} + +// wpinav_set_origin_and_destination - set origin and destination using lat/lon coordinates +void wpinav_set_origin_and_destination(Location& origin, Location& destination) +{ + wpinav_origin.x = (origin.lat-home.lat) * LATLON_TO_CM; + wpinav_origin.y = (origin.lng-home.lng) * LATLON_TO_CM * scaleLongDown; + wpinav_destination.x = (destination.lat-home.lat) * LATLON_TO_CM; + wpinav_destination.y = (destination.lng-home.lng) * LATLON_TO_CM * scaleLongDown; + wpinav_pos_delta = wpinav_destination - wpinav_origin; + wpinav_track_length = wpinav_pos_delta.length(); + + // reset the desired distance along the track to the closest point's distance + Vector2f curr(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff()); + float line_a = wpinav_pos_delta.y; + float line_b = -wpinav_pos_delta.x; + float line_c = wpinav_pos_delta.x * wpinav_origin.y - wpinav_pos_delta.y * wpinav_origin.x; + float line_m = line_a / line_b; + line_m = 1/line_m; + line_a = line_m; + line_b = -1; + line_c = curr.y - line_m * curr.x; + + // calculate the distance to the closest point along the track and it's distance from the origin + wpinav_track_desired = abs(line_a*wpinav_origin.x + line_b*wpinav_origin.y + line_c) / safe_sqrt(line_a*line_a+line_b*line_b); +} + +#define WPINAV_MAX_POS_ERROR 100.0f // maximum distance (in cm) that the desired track can stray from our current location. +void +wpinav_advance_track_desired(float velocity_cms, float dt) +{ + // get current location + Vector2f curr(inertial_nav.get_latitude_diff(), inertial_nav.get_longitude_diff()); + + float line_a = wpinav_pos_delta.y; + float line_b = -wpinav_pos_delta.x; + float line_c = wpinav_pos_delta.x * wpinav_origin.y - wpinav_pos_delta.y * wpinav_origin.x; + float line_m = line_a / line_b; + float cross_track_dist = abs(line_a * curr.x + line_b * curr.y + line_c ) / wpinav_track_length; + + line_m = 1/line_m; + line_a = line_m; + line_b = -1; + line_c = curr.y - line_m * curr.x; + + // calculate the distance to the closest point along the track and it's distance from the origin + float track_covered = abs(line_a*wpinav_origin.x + line_b*wpinav_origin.y + line_c) / safe_sqrt(line_a*line_a+line_b*line_b); + + // maximum distance along the track that we will allow (stops target point from getting too far from the current position) + float track_desired_max = track_covered + safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR-cross_track_dist*cross_track_dist); + + // advance the current target + wpinav_track_desired += velocity_cms * dt; + + // constrain the target from moving too far + if( wpinav_track_desired > track_desired_max ) { + wpinav_track_desired = track_desired_max; + } + + // recalculate the desired position + float track_length_pct = wpinav_track_desired/wpinav_track_length; + wpinav_target.x = wpinav_origin.x + wpinav_pos_delta.x * track_length_pct; + wpinav_target.y = wpinav_origin.y + wpinav_pos_delta.y * track_length_pct; +}