Rover: wp nav uses stopping point or prev wp for origin

This commit is contained in:
Randy Mackay 2018-03-13 21:06:13 +09:00
parent 784c60f58f
commit 75487c1e02
2 changed files with 39 additions and 2 deletions

View File

@ -99,8 +99,13 @@ void Mode::get_pilot_desired_steering_and_throttle(float &steering_out, float &t
// set desired location // set desired location
void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd) void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
{ {
// record targets // set origin to last destination if waypoint controller active
_origin = rover.current_loc; if ((AP_HAL::millis() - last_steer_to_wp_ms < 100) && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
calc_stopping_location(_origin);
}
_destination = destination; _destination = destination;
// initialise distance // initialise distance
@ -304,6 +309,9 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
// this function updates the _yaw_error_cd value // this function updates the _yaw_error_cd value
void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed) void Mode::calc_steering_to_waypoint(const struct Location &origin, const struct Location &destination, bool reversed)
{ {
// record system time of call
last_steer_to_wp_ms = AP_HAL::millis();
// Calculate the required turn of the wheels // Calculate the required turn of the wheels
// negative error = left turn // negative error = left turn
// positive error = right turn // positive error = right turn
@ -353,3 +361,28 @@ void Mode::calc_steering_to_heading(float desired_heading_cd, bool reversed)
const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed); const float steering_out = attitude_control.get_steering_out_angle_error(yaw_error, g2.motors.have_skid_steering(), g2.motors.limit.steer_left, g2.motors.limit.steer_right, reversed);
g2.motors.set_steering(steering_out * 4500.0f); g2.motors.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;
location_offset(stopping_loc, stopping_offset.x, stopping_offset.y);
}

View File

@ -136,6 +136,9 @@ protected:
// relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination // relies on these internal members being updated: lateral_acceleration, _yaw_error_cd, _distance_to_destination
float calc_reduced_speed_for_turn_or_distance(float desired_speed); float calc_reduced_speed_for_turn_or_distance(float desired_speed);
// calculate vehicle stopping location using current location, velocity and maximum acceleration
void calc_stopping_location(Location& stopping_loc);
// references to avoid code churn: // references to avoid code churn:
class AP_AHRS &ahrs; class AP_AHRS &ahrs;
class Parameters &g; class Parameters &g;
@ -156,6 +159,7 @@ protected:
float _desired_speed; // desired speed in m/s float _desired_speed; // desired speed in m/s
float _desired_speed_final; // desired speed in m/s when we reach the destination float _desired_speed_final; // desired speed in m/s when we reach the destination
float _speed_error; // ground speed error in m/s float _speed_error; // ground speed error in m/s
uint32_t last_steer_to_wp_ms; // system time of last call to calc_steering_to_waypoint
}; };