Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-27 11:08:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: make next_WP and prev_WP be locations

Browse Source 
having a command ID in them doesn't really make sense

Pair-Programmed-With: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Andrew Tridgell 2014-03-16 15:53:10 +11:00 committed by Randy Mackay
parent 48329c1bba
commit 4bf140aeac
9 changed files with 90 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduPlane/ArduPlane.pde
View File

@ -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

8
ArduPlane/GCS_Mavlink.pde
View File

@ -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(

36
ArduPlane/commands.pde
View File

@ -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;
}
/*

37
ArduPlane/commands_logic.pde
View File

@ -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);

4
ArduPlane/control_modes.pde
View File

@ -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;

16
ArduPlane/geofence.pde
View File

@ -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;

49
ArduPlane/navigation.pde
View File

@ -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;
}

13
ArduPlane/rally.pde
View File

@ -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;
}

8
ArduPlane/system.pde
View File

@ -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;
}