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

Rover: Correct circle mode nav outputs

This commit is contained in:
Stephen Dade 2024-08-30 17:04:57 +10:00 committed by Peter Barker
parent 9beca76f44
commit e0e79a6287
1 changed files with 8 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Rover/mode_circle.cpp
View File

@ -70,7 +70,7 @@ bool ModeCircle::set_center(const Location& center_loc, float radius_m, bool dir
return true; return true;
} }
// initialize dock mode // initialize circle mode from current position
bool ModeCircle::_enter() bool ModeCircle::_enter()
{ {
// capture starting point and yaw // capture starting point and yaw
@ -85,6 +85,9 @@ bool ModeCircle::_enter()
target.yaw_rad = AP::ahrs().get_yaw(); target.yaw_rad = AP::ahrs().get_yaw();
target.speed = 0; target.speed = 0;
// record center as location (only used for reporting)
config.center_loc = rover.current_loc;
// check speed around circle does not lead to excessive lateral acceleration // check speed around circle does not lead to excessive lateral acceleration
check_config_speed(); check_config_speed();
@ -146,8 +149,8 @@ void ModeCircle::update()
// Update distance to destination and distance to edge // Update distance to destination and distance to edge
const Vector2f center_to_veh = curr_pos - config.center_pos; const Vector2f center_to_veh = curr_pos - config.center_pos;
_distance_to_destination = center_to_veh.length(); _distance_to_destination = (target.pos.tofloat() - curr_pos).length();
dist_to_edge_m = fabsf(_distance_to_destination - config.radius); dist_to_edge_m = fabsf(center_to_veh.length() - config.radius);
// Update depending on stage // Update depending on stage
if (!reached_edge) { if (!reached_edge) {
@ -221,7 +224,7 @@ float ModeCircle::wp_bearing() const
return 0; return 0;
} }
// calc vector from circle center to vehicle // calc vector from circle center to vehicle
Vector2f veh_to_center = (config.center_pos - curr_pos_NE); Vector2f veh_to_center = (target.pos.tofloat() - curr_pos_NE);
if (veh_to_center.is_zero()) { if (veh_to_center.is_zero()) {
return 0; return 0;
} }
@ -266,6 +269,7 @@ bool ModeCircle::set_desired_speed(float speed_ms)
bool ModeCircle::get_desired_location(Location& destination) const bool ModeCircle::get_desired_location(Location& destination) const
{ {
destination = config.center_loc; destination = config.center_loc;
destination.offset_bearing(degrees(target.yaw_rad), config.radius);
return true; return true;
} }