From 4bf140aeac50c3976d19ada7ecf279064b22dd4e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 16 Mar 2014 15:53:10 +1100 Subject: [PATCH] Plane: make next_WP and prev_WP be locations having a command ID in them doesn't really make sense Pair-Programmed-With: Randy Mackay --- ArduPlane/ArduPlane.pde | 8 +++--- ArduPlane/GCS_Mavlink.pde | 8 +++--- ArduPlane/commands.pde | 36 +++++++++++++------------- ArduPlane/commands_logic.pde | 37 +++++++++++++-------------- ArduPlane/control_modes.pde | 4 +-- ArduPlane/geofence.pde | 16 ++++++------ ArduPlane/navigation.pde | 49 +++++++++++++++++++----------------- ArduPlane/rally.pde | 13 +++++----- ArduPlane/system.pde | 8 +++--- 9 files changed, 90 insertions(+), 89 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 14a8a6730f..973b4221b4 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -640,15 +640,15 @@ static const struct Location &home = ahrs.get_home(); // Flag for if we have g_gps lock and have set the home location in AHRS static bool home_is_set; // The location of the previous waypoint. Used for track following and altitude ramp calculations -static struct AP_Mission::Mission_Command prev_WP; +static Location prev_WP_loc; // The plane's current location static struct Location current_loc; // The location of the current/active waypoint. Used for altitude ramp, track following and loiter calculations. -static struct AP_Mission::Mission_Command next_WP; +static Location next_WP_loc; // The location of the active waypoint in Guided mode. -static struct Location guided_WP; +static struct Location guided_WP_loc; // special purpose command used only after mission completed to return vehicle to home or rally point -static struct AP_Mission::Mission_Command auto_rtl_command; +static struct AP_Mission::Mission_Command auto_rtl_command; //////////////////////////////////////////////////////////////////////////////// // Altitude / Climb rate control diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index ded04f83e4..0d99aa5506 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1695,11 +1695,11 @@ mission_item_send_failed: } if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = cmd.content.location; + guided_WP_loc = cmd.content.location; // add home alt if needed - if (guided_WP.flags.relative_alt) { - guided_WP.alt += home.alt; + if (guided_WP_loc.flags.relative_alt) { + guided_WP_loc.alt += home.alt; } set_mode(GUIDED); @@ -1721,7 +1721,7 @@ mission_item_send_failed: cmd.content.location.alt += home.alt; } - next_WP.content.location.alt = cmd.content.location.alt; + next_WP_loc.alt = cmd.content.location.alt; // verify we recevied the command mavlink_msg_mission_ack_send( diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index f43e40c365..353f1e0e16 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -20,29 +20,29 @@ static void set_next_WP(const AP_Mission::Mission_Command& cmd) { // copy the current WP into the OldWP slot // --------------------------------------- - prev_WP = next_WP; + prev_WP_loc = next_WP_loc; // Load the next_WP slot // --------------------- - next_WP = cmd; + next_WP_loc = cmd.content.location; // if lat and lon is zero, then use current lat/lon // this allows a mission to contain a "loiter on the spot" // command - if (next_WP.content.location.lat == 0 && next_WP.content.location.lng == 0) { - next_WP.content.location.lat = current_loc.lat; - next_WP.content.location.lng = current_loc.lng; + if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) { + next_WP_loc.lat = current_loc.lat; + next_WP_loc.lng = current_loc.lng; // additionally treat zero altitude as current altitude - if (next_WP.content.location.alt == 0) { - next_WP.content.location.alt = current_loc.alt; - next_WP.content.location.flags.relative_alt = false; + if (next_WP_loc.alt == 0) { + next_WP_loc.alt = current_loc.alt; + next_WP_loc.flags.relative_alt = false; } } // convert relative alt to absolute alt - if (next_WP.content.location.flags.relative_alt) { - next_WP.content.location.flags.relative_alt = false; - next_WP.content.location.alt += home.alt; + if (next_WP_loc.flags.relative_alt) { + next_WP_loc.flags.relative_alt = false; + next_WP_loc.alt += home.alt; } // are we already past the waypoint? This happens when we jump @@ -50,9 +50,9 @@ static void set_next_WP(const AP_Mission::Mission_Command& cmd) // past the waypoint when we start on a leg, then use the current // location as the previous waypoint, to prevent immediately // considering the waypoint complete - if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) { + if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP")); - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; } // used to control FBW and limit the rate of climb @@ -77,11 +77,11 @@ static void set_guided_WP(void) // copy the current location into the OldWP slot // --------------------------------------- - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; // Load the next_WP slot // --------------------- - next_WP.content.location = guided_WP; + next_WP_loc = guided_WP_loc; // used to control FBW and limit the rate of climb // ----------------------------------------------- @@ -118,12 +118,12 @@ static void init_home() // Save prev loc // ------------- - next_WP.content.location = prev_WP.content.location = home; + next_WP_loc = prev_WP_loc = home; // Load home for a default guided_WP // ------------- - guided_WP = home; - guided_WP.alt += g.RTL_altitude_cm; + guided_WP_loc = home; + guided_WP_loc.alt += g.RTL_altitude_cm; } /* diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 54862aa3da..16d51c3cb5 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -12,8 +12,6 @@ static void do_within_distance(const AP_Mission::Mission_Command& cmd); static void do_change_alt(const AP_Mission::Mission_Command& cmd); static void do_change_speed(const AP_Mission::Mission_Command& cmd); static void do_set_home(const AP_Mission::Mission_Command& cmd); -static AP_Mission::Mission_Command rally_find_best_cmd(const Location &myloc, const Location &homeloc); - /********************************************************************************/ // Command Event Handlers @@ -242,8 +240,8 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd) // Ret static void do_RTL(void) { control_mode = RTL; - prev_WP.content.location = current_loc; - next_WP = rally_find_best_cmd(current_loc, home); + prev_WP_loc = current_loc; + next_WP_loc = rally_find_best_location(current_loc, home); if (g.loiter_radius < 0) { loiter.direction = -1; @@ -262,9 +260,9 @@ static void do_takeoff(const AP_Mission::Mission_Command& cmd) set_next_WP(cmd); // pitch in deg, airspeed m/s, throttle %, track WP 1 or 0 takeoff_pitch_cd = (int)cmd.p1 * 100; - takeoff_altitude_cm = next_WP.content.location.alt; - next_WP.content.location.lat = home.lat + 10; - next_WP.content.location.lng = home.lng + 10; + takeoff_altitude_cm = next_WP_loc.alt; + next_WP_loc.lat = home.lat + 10; + next_WP_loc.lng = home.lng + 10; takeoff_complete = false; // set flag to use gps ground course during TO. IMU will be doing yaw drift correction // Flag also used to override "on the ground" throttle disable } @@ -334,7 +332,7 @@ static bool verify_takeoff() if (adjusted_altitude_cm() > takeoff_altitude_cm) { steer_state.hold_course_cd = -1; takeoff_complete = true; - next_WP.content.location = prev_WP.content.location = current_loc; + next_WP_loc = prev_WP_loc = current_loc; return true; } else { return false; @@ -351,7 +349,7 @@ static bool verify_land() // Set land_complete if we are within 2 seconds distance or within // 3 meters altitude of the landing point if ((wp_distance <= (g.land_flare_sec*g_gps->ground_speed_cm*0.01f)) - || (adjusted_altitude_cm() <= next_WP.content.location.alt + g.land_flare_alt*100)) { + || (adjusted_altitude_cm() <= next_WP_loc.alt + g.land_flare_alt*100)) { land_complete = true; @@ -384,7 +382,7 @@ static bool verify_land() // recalc bearing error with hold_course; nav_controller->update_heading_hold(steer_state.hold_course_cd); } else { - nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location); + nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); } return false; } @@ -393,13 +391,13 @@ static bool verify_nav_wp() { steer_state.hold_course_cd = -1; - nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location); + nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); // see if the user has specified a maximum distance to waypoint if (g.waypoint_max_radius > 0 && wp_distance > (uint16_t)g.waypoint_max_radius) { - if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) { + if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { // this is needed to ensure completion of the waypoint - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; } return false; } @@ -407,15 +405,15 @@ static bool verify_nav_wp() if (wp_distance <= nav_controller->turn_distance(g.waypoint_radius)) { gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"), (unsigned)mission.get_current_nav_cmd().index, - (unsigned)get_distance(current_loc, next_WP.content.location)); + (unsigned)get_distance(current_loc, next_WP_loc)); return true; } // have we flown past the waypoint? - if (location_passed_point(current_loc, prev_WP.content.location, next_WP.content.location)) { + if (location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) { gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"), (unsigned)mission.get_current_nav_cmd().index, - (unsigned)get_distance(current_loc, next_WP.content.location)); + (unsigned)get_distance(current_loc, next_WP_loc)); return true; } @@ -485,7 +483,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd) condition_rate = -condition_rate; } target_altitude_cm = adjusted_altitude_cm() + (condition_rate / 10); // Divide by ten for 10Hz update - next_WP.content.location.alt = condition_value; // For future nav calculations + next_WP_loc.alt = condition_value; // For future nav calculations offset_altitude_cm = 0; // For future nav calculations } @@ -538,7 +536,7 @@ static void do_loiter_at_location() } else { loiter.direction = 1; } - next_WP.content.location = current_loc; + next_WP_loc = current_loc; } static void do_change_speed(const AP_Mission::Mission_Command& cmd) @@ -596,7 +594,8 @@ static void exit_mission_callback() { if (control_mode == AUTO) { gcs_send_text_fmt(PSTR("Returning to Home")); - auto_rtl_command = rally_find_best_cmd(current_loc, home); + memset(&auto_rtl_command, 0, sizeof(auto_rtl_command)); + auto_rtl_command.content.location = rally_find_best_location(current_loc, home); auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM; setup_glide_slope(); start_command(auto_rtl_command); diff --git a/ArduPlane/control_modes.pde b/ArduPlane/control_modes.pde index 64785c1049..61251761b9 100644 --- a/ArduPlane/control_modes.pde +++ b/ArduPlane/control_modes.pde @@ -43,13 +43,13 @@ static void read_control_switch() set_mode((enum FlightMode)(flight_modes[switchPosition].get())); oldSwitchPosition = switchPosition; - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; } if (g.reset_mission_chan != 0 && hal.rcin->read(g.reset_mission_chan-1) > RESET_SWITCH_CHAN_PWM) { mission.start(); - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; } switch_debouncer = false; diff --git a/ArduPlane/geofence.pde b/ArduPlane/geofence.pde index 098875b956..9a376ae322 100644 --- a/ArduPlane/geofence.pde +++ b/ArduPlane/geofence.pde @@ -187,8 +187,8 @@ static void geofence_check(bool altitude_check_only) g.fence_total >= 5 && geofence_state->boundary_uptodate && geofence_state->old_switch_position == oldSwitchPosition && - guided_WP.lat == geofence_state->boundary[0].x && - guided_WP.lng == geofence_state->boundary[0].y) { + guided_WP_loc.lat == geofence_state->boundary[0].x && + guided_WP_loc.lng == geofence_state->boundary[0].y) { geofence_state->old_switch_position = 254; reset_control_switch(); } @@ -268,18 +268,18 @@ static void geofence_check(bool altitude_check_only) case FENCE_ACTION_GUIDED_THR_PASS: if (g.fence_retalt > 0) { //fly to the return point using fence_retalt - guided_WP.alt = home.alt + 100.0*g.fence_retalt; + guided_WP_loc.alt = home.alt + 100.0f*g.fence_retalt; } else if (g.fence_minalt >= g.fence_maxalt) { // invalid min/max, use RTL_altitude - guided_WP.alt = home.alt + g.RTL_altitude_cm; + guided_WP_loc.alt = home.alt + g.RTL_altitude_cm; } else { // fly to the return point, with an altitude half way between // min and max - guided_WP.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2; + guided_WP_loc.alt = home.alt + 100.0*(g.fence_minalt + g.fence_maxalt)/2; } - guided_WP.options = 0; - guided_WP.lat = geofence_state->boundary[0].x; - guided_WP.lng = geofence_state->boundary[0].y; + guided_WP_loc.options = 0; + guided_WP_loc.lat = geofence_state->boundary[0].x; + guided_WP_loc.lng = geofence_state->boundary[0].y; geofence_state->old_switch_position = oldSwitchPosition; diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 29ebc65b44..df104eb143 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -54,13 +54,13 @@ static void navigate() return; } - if (next_WP.content.location.lat == 0) { + if (next_WP_loc.lat == 0) { return; } // waypoint distance from plane // ---------------------------- - wp_distance = get_distance(current_loc, next_WP.content.location); + wp_distance = get_distance(current_loc, next_WP_loc); if (wp_distance < 0) { gcs_send_text_P(SEVERITY_HIGH,PSTR("WP error - distance < 0")); @@ -135,19 +135,19 @@ static void calc_altitude_error() if (nav_controller->reached_loiter_target()) { // once we reach a loiter target then lock to the final // altitude target - target_altitude_cm = next_WP.content.location.alt; + target_altitude_cm = next_WP_loc.alt; } else if (offset_altitude_cm != 0) { // control climb/descent rate - target_altitude_cm = next_WP.content.location.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30))); + target_altitude_cm = next_WP_loc.alt - (offset_altitude_cm*((float)(wp_distance-30) / (float)(wp_totalDistance-30))); - // stay within a certain range - if (prev_WP.content.location.alt > next_WP.content.location.alt) { - target_altitude_cm = constrain_int32(target_altitude_cm, next_WP.content.location.alt, prev_WP.content.location.alt); - }else{ - target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP.content.location.alt, next_WP.content.location.alt); + // stay within the range of the start and end locations in altitude + if (prev_WP_loc.alt > next_WP_loc.alt) { + target_altitude_cm = constrain_int32(target_altitude_cm, next_WP_loc.alt, prev_WP_loc.alt); + } else { + target_altitude_cm = constrain_int32(target_altitude_cm, prev_WP_loc.alt, next_WP_loc.alt); } } else if (mission.get_current_do_cmd().id != MAV_CMD_CONDITION_CHANGE_ALT) { - target_altitude_cm = next_WP.content.location.alt; + target_altitude_cm = next_WP_loc.alt; } altitude_error_cm = target_altitude_cm - adjusted_altitude_cm(); @@ -155,7 +155,7 @@ static void calc_altitude_error() static void update_loiter() { - nav_controller->update_loiter(next_WP.content.location, abs(g.loiter_radius), loiter.direction); + nav_controller->update_loiter(next_WP_loc, abs(g.loiter_radius), loiter.direction); } /* @@ -180,15 +180,15 @@ static void update_cruise() cruise_state.locked_heading = true; cruise_state.lock_timer_ms = 0; cruise_state.locked_heading_cd = g_gps->ground_course_cd; - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; } if (cruise_state.locked_heading) { - next_WP = prev_WP; + next_WP_loc = prev_WP_loc; // always look 1km ahead - location_update(next_WP.content.location, + location_update(next_WP_loc, cruise_state.locked_heading_cd*0.01f, - get_distance(prev_WP.content.location, current_loc) + 1000); - nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location); + get_distance(prev_WP_loc, current_loc) + 1000); + nav_controller->update_waypoint(prev_WP_loc, next_WP_loc); } } @@ -232,7 +232,7 @@ static void setup_glide_slope(void) { // establish the distance we are travelling to the next waypoint, // for calculating out rate of change of altitude - wp_totalDistance = get_distance(current_loc, next_WP.content.location); + wp_totalDistance = get_distance(current_loc, next_WP_loc); wp_distance = wp_totalDistance; /* @@ -246,18 +246,21 @@ static void setup_glide_slope(void) rapidly if below it. See https://github.com/diydrones/ardupilot/issues/39 */ - if (current_loc.alt > next_WP.content.location.alt) { - offset_altitude_cm = next_WP.content.location.alt - current_loc.alt; + if (current_loc.alt > next_WP_loc.alt) { + offset_altitude_cm = next_WP_loc.alt - current_loc.alt; } else { offset_altitude_cm = 0; } break; case AUTO: - if (prev_WP.id != MAV_CMD_NAV_TAKEOFF && - prev_WP.content.location.alt != home.alt && - (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) { - offset_altitude_cm = next_WP.content.location.alt - prev_WP.content.location.alt; + // we only do glide slide handling in AUTO when above 40m or + // when descending. The 40 meter threshold is arbitrary, and + // is basically to prevent situations where we try to slowly + // gain height at low altitudes, potentially hitting + // obstacles. + if (relative_altitude() > 40 || next_WP_loc.alt < prev_WP_loc.alt) { + offset_altitude_cm = next_WP_loc.alt - prev_WP_loc.alt; } else { offset_altitude_cm = 0; } diff --git a/ArduPlane/rally.pde b/ArduPlane/rally.pde index 078448d558..e7e1358d9a 100644 --- a/ArduPlane/rally.pde +++ b/ArduPlane/rally.pde @@ -82,20 +82,19 @@ static Location rally_location_to_location(const RallyLocation &r_loc, const Loc } // return best RTL location from current position -static AP_Mission::Mission_Command rally_find_best_cmd(const Location &myloc, const Location &homeloc) +static Location rally_find_best_location(const Location &myloc, const Location &homeloc) { RallyLocation ral_loc = {}; - AP_Mission::Mission_Command ret = {}; - ret.id = MAV_CMD_NAV_LOITER_UNLIM; + Location ret = {}; if (find_best_rally_point(myloc, home, ral_loc)) { //we have setup Rally points: use them instead of Home for RTL - ret.content.location = rally_location_to_location(ral_loc, home); + ret = rally_location_to_location(ral_loc, home); } else { - ret.content.location = homeloc; + ret = homeloc; // Altitude to hold over home - ret.content.location.alt = read_alt_to_hold(); + ret.alt = read_alt_to_hold(); // read_alt_to_hold returns an absolute altitude - ret.content.location.flags.relative_alt = false; + ret.flags.relative_alt = false; } return ret; } diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 2364cd7976..b3a99f4467 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -340,11 +340,11 @@ static void set_mode(enum FlightMode mode) case CIRCLE: // the altitude to circle at is taken from the current altitude - next_WP.content.location.alt = current_loc.alt; + next_WP_loc.alt = current_loc.alt; break; case AUTO: - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; // start the mission. Note that we use resume(), not start(), // as the correct behaviour for plane when entering auto is to // continue the mission. If the pilot wants to restart the @@ -354,7 +354,7 @@ static void set_mode(enum FlightMode mode) break; case RTL: - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; do_RTL(); break; @@ -368,7 +368,7 @@ static void set_mode(enum FlightMode mode) break; default: - prev_WP.content.location = current_loc; + prev_WP_loc = current_loc; do_RTL(); break; }