Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Rover: revert next_WP, prev_WP to location structures

This commit is contained in:
Randy Mackay 2014-03-17 16:44:25 +09:00
parent 96b2e88e36
commit 59555e8364
6 changed files with 29 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
APMrover2/APMrover2.pde
View File

@ -514,9 +514,9 @@ static const struct Location &home = ahrs.get_home();
// Flag for if we have g_gps lock and have set the home location
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 struct Location prev_WP;
// The location of the current/active waypoint. Used for track following
static struct AP_Mission::Mission_Command next_WP;
static struct Location next_WP;
// The location of the active waypoint in Guided mode.
static struct Location guided_WP;

4
APMrover2/Steering.pde
View File

@ -138,13 +138,13 @@ static void calc_lateral_acceleration()
{
switch (control_mode) {
case AUTO:
nav_controller->update_waypoint(prev_WP.content.location, next_WP.content.location);
nav_controller->update_waypoint(prev_WP, next_WP);
break;
case RTL:
case GUIDED:
case STEERING:
nav_controller->update_waypoint(current_loc, next_WP.content.location);
nav_controller->update_waypoint(current_loc, next_WP);
break;
default:
return;

25
APMrover2/commands.pde
View File

@ -10,10 +10,9 @@
/*
This function stores waypoint commands
It looks to see what the next command type is and finds the last command.
*/
static void set_next_WP(const AP_Mission::Mission_Command& cmd)
* set_next_WP - sets the target location the vehicle should fly to
*/
static void set_next_WP(const struct Location& loc)
{
// copy the current WP into the OldWP slot
// ---------------------------------------
@ -21,20 +20,20 @@ static void set_next_WP(const AP_Mission::Mission_Command& cmd)
// Load the next_WP slot
// ---------------------
next_WP = cmd;
next_WP = loc;
// are we already past the waypoint? This happens when we jump
// waypoints, and it can cause us to skip a waypoint. If we are
// 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, next_WP)) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Resetting prev_WP"));
prev_WP.content.location = current_loc;
prev_WP = current_loc;
}
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP.content.location);
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
}
@ -42,14 +41,14 @@ static void set_guided_WP(void)
{
// copy the current location into the OldWP slot
// ---------------------------------------
prev_WP.content.location = current_loc;
prev_WP = current_loc;
// Load the next_WP slot
// ---------------------
next_WP.content.location = guided_WP;
next_WP = guided_WP;
// this is handy for the groundstation
wp_totalDistance = get_distance(current_loc, next_WP.content.location);
wp_totalDistance = get_distance(current_loc, next_WP);
wp_distance = wp_totalDistance;
}
@ -72,7 +71,7 @@ void init_home()
// Save prev loc
// -------------
next_WP.content.location = prev_WP.content.location = home;
next_WP = prev_WP = home;
// Load home for a default guided_WP
// -------------
@ -82,6 +81,6 @@ void init_home()
static void restart_nav()
{
g.pidSpeedThrottle.reset_I();
prev_WP.content.location = current_loc;
prev_WP = current_loc;
mission.resume();
}

20
APMrover2/commands_logic.pde
View File

@ -183,19 +183,19 @@ static bool verify_command(const AP_Mission::Mission_Command& cmd)
static void do_RTL(void)
{
prev_WP.content.location = current_loc;
prev_WP = current_loc;
control_mode = RTL;
next_WP.content.location = home;
next_WP = home;
}
static void do_takeoff(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd);
set_next_WP(cmd.content.location);
}
static void do_nav_wp(const AP_Mission::Mission_Command& cmd)
{
set_next_WP(cmd);
set_next_WP(cmd.content.location);
}
/********************************************************************************/
@ -210,15 +210,15 @@ static bool verify_nav_wp(const AP_Mission::Mission_Command& cmd)
if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) {
gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"),
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP.content.location));
(unsigned)get_distance(current_loc, next_WP));
return true;
}
// have we gone 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, next_WP)) {
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
(unsigned)cmd.index,
(unsigned)get_distance(current_loc, next_WP.content.location));
(unsigned)get_distance(current_loc, next_WP));
return true;
}
@ -234,9 +234,9 @@ static bool verify_RTL()
}
// have we gone 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, next_WP)) {
gcs_send_text_fmt(PSTR("Reached Home dist %um"),
(unsigned)get_distance(current_loc, next_WP.content.location));
(unsigned)get_distance(current_loc, next_WP));
return true;
}
@ -258,7 +258,7 @@ static void do_change_alt(const AP_Mission::Mission_Command& cmd)
condition_rate = abs((int)cmd.content.location.lat);
condition_value = cmd.content.location.alt;
if(condition_value < current_loc.alt) condition_rate = -condition_rate;
next_WP.content.location.alt = condition_value; // For future nav calculations
next_WP.alt = condition_value; // For future nav calculations
}
static void do_within_distance(const AP_Mission::Mission_Command& cmd)

2
APMrover2/control_modes.pde
View File

@ -27,7 +27,7 @@ static void read_control_switch()
set_mode((enum mode)modes[switchPosition].get());
oldSwitchPosition = switchPosition;
prev_WP.content.location = current_loc;
prev_WP = current_loc;
// reset speed integrator
g.pidSpeedThrottle.reset_I();

4
APMrover2/navigation.pde
View File

@ -11,13 +11,13 @@ static void navigate()
return;
}
if ((next_WP.content.location.lat == 0)||(home_is_set==false)){
if ((next_WP.lat == 0)||(home_is_set==false)){
return;
}
// waypoint distance from rover
// ----------------------------
wp_distance = get_distance(current_loc, next_WP.content.location);
wp_distance = get_distance(current_loc, next_WP);
if (wp_distance < 0){
gcs_send_text_P(SEVERITY_HIGH,PSTR("<navigate> WP error - distance < 0"));