mirror of https://github.com/ArduPilot/ardupilot
Rover: set_guided_WP accepts location
This commit is contained in:
parent
131e1bdef5
commit
d6b55b2d09
|
@ -705,14 +705,12 @@ bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
|||
// only accept position updates when in GUIDED mode
|
||||
return false;
|
||||
}
|
||||
rover.guided_WP = cmd.content.location;
|
||||
|
||||
// This method is only called when we are in Guided WP GUIDED mode
|
||||
rover.guided_mode = Guided_WP;
|
||||
|
||||
// make any new wp uploaded instant (in case we are already in Guided mode)
|
||||
rover.rtl_complete = false;
|
||||
rover.set_guided_WP();
|
||||
rover.set_guided_WP(cmd.content.location);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
@ -1290,9 +1288,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
break;
|
||||
}
|
||||
|
||||
rover.guided_WP = loc;
|
||||
rover.rtl_complete = false;
|
||||
rover.set_guided_WP();
|
||||
rover.set_guided_WP(loc);
|
||||
}
|
||||
|
||||
break;
|
||||
|
@ -1324,9 +1320,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
|
|||
Location loc = rover.current_loc;
|
||||
loc.lat = packet.lat_int;
|
||||
loc.lng = packet.lon_int;
|
||||
rover.guided_WP = loc;
|
||||
rover.rtl_complete = false;
|
||||
rover.set_guided_WP();
|
||||
rover.set_guided_WP(loc);
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
|
@ -338,8 +338,6 @@ private:
|
|||
struct Location prev_WP;
|
||||
// The location of the current/active waypoint. Used for track following
|
||||
struct Location next_WP;
|
||||
// The location of the active waypoint in Guided mode.
|
||||
struct Location guided_WP;
|
||||
|
||||
// IMU variables
|
||||
// The main loop execution time. Seconds
|
||||
|
@ -472,7 +470,7 @@ private:
|
|||
void calc_nav_steer();
|
||||
void set_servos(void);
|
||||
void set_next_WP(const struct Location& loc);
|
||||
void set_guided_WP(void);
|
||||
void set_guided_WP(const struct Location& loc);
|
||||
void init_home();
|
||||
void restart_nav();
|
||||
void exit_mission();
|
||||
|
|
|
@ -37,7 +37,7 @@ void Rover::set_next_WP(const struct Location& loc)
|
|||
wp_distance = wp_totalDistance;
|
||||
}
|
||||
|
||||
void Rover::set_guided_WP(void)
|
||||
void Rover::set_guided_WP(const struct Location& loc)
|
||||
{
|
||||
// copy the current location into the OldWP slot
|
||||
// ---------------------------------------
|
||||
|
@ -45,11 +45,13 @@ void Rover::set_guided_WP(void)
|
|||
|
||||
// Load the next_WP slot
|
||||
// ---------------------
|
||||
next_WP = guided_WP;
|
||||
next_WP = loc;
|
||||
|
||||
// this is handy for the groundstation
|
||||
wp_totalDistance = get_distance(current_loc, next_WP);
|
||||
wp_distance = wp_totalDistance;
|
||||
|
||||
rover.rtl_complete = false;
|
||||
}
|
||||
|
||||
// run this at setup on the ground
|
||||
|
@ -77,7 +79,7 @@ void Rover::init_home()
|
|||
|
||||
// Load home for a default guided_WP
|
||||
// -------------
|
||||
guided_WP = home;
|
||||
set_guided_WP(home);
|
||||
}
|
||||
|
||||
void Rover::restart_nav()
|
||||
|
|
|
@ -333,13 +333,11 @@ void Rover::set_mode(enum mode mode)
|
|||
|
||||
case GUIDED:
|
||||
auto_throttle_mode = true;
|
||||
rtl_complete = false;
|
||||
/*
|
||||
when entering guided mode we set the target as the current
|
||||
location. This matches the behaviour of the copter code.
|
||||
*/
|
||||
guided_WP = current_loc;
|
||||
set_guided_WP();
|
||||
set_guided_WP(current_loc);
|
||||
break;
|
||||
|
||||
default:
|
||||
|
|
Loading…
Reference in New Issue