aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: use AR_WPNav get stopping location

Browse Source
This commit is contained in:
Peter Hall 2019-09-26 20:45:53 +01:00 committed by Randy Mackay
parent 62331cf8a0
commit e28d56f63a
3 changed files with 3 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
APMrover2/mode.cpp
View File

@ -451,31 +451,6 @@ void Mode::calc_steering_to_heading(float desired_heading_cd, float rate_max_deg
set_steering(steering_out * 4500.0f);
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
void Mode::calc_stopping_location(Location& stopping_loc)
{
// default stopping location
stopping_loc = rover.current_loc;
// get current velocity vector and speed
const Vector2f velocity = ahrs.groundspeed_vector();
const float speed = velocity.length();
// avoid divide by zero
if (!is_positive(speed)) {
stopping_loc = rover.current_loc;
return;
}
// get stopping distance in meters
const float stopping_dist = attitude_control.get_stopping_distance(speed);
// calculate stopping position from current location in meters
const Vector2f stopping_offset = velocity.normalized() * stopping_dist;
stopping_loc.offset(stopping_offset.x, stopping_offset.y);
}
void Mode::set_steering(float steering_value)
{
if (allows_stick_mixing() && g2.stick_mixing > 0) {

3
APMrover2/mode.h
View File

@ -186,9 +186,6 @@ protected:
// reversed should be true if the vehicle is intentionally backing up which allows the pilot to increase the backing up speed by pulling the throttle stick down
float calc_speed_nudge(float target_speed, bool reversed);
// calculate vehicle stopping location using current location, velocity and maximum acceleration
void calc_stopping_location(Location& stopping_loc);
protected:
// decode pilot steering and throttle inputs and return in steer_out and throttle_out arguments

4
APMrover2/mode_loiter.cpp
View File

@ -4,7 +4,9 @@
bool ModeLoiter::_enter()
{
// set _destination to reasonable stopping point
calc_stopping_location(_destination);
if (!g2.wp_nav.get_stopping_location(_destination)) {
return false;
}
// initialise desired speed to current speed
if (!attitude_control.get_forward_speed(_desired_speed)) {