Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Circle: integrate pos vel accel offsets

Browse Source 
Co-authored-by: Leonard Hall <leonardthall@gmail.com>
This commit is contained in:
Randy Mackay 2024-09-02 14:07:05 +09:00
parent 9185b82b7a
commit 9ca47cf465
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -86,7 +86,7 @@ void AC_Circle::init()
_pos_control.init_z_controller_stopping_point();
// get stopping point
const Vector3p& stopping_point = _pos_control.get_pos_target_cm();
const Vector3p& stopping_point = _pos_control.get_pos_desired_cm();
// set circle center to circle_radius ahead of stopping point
_center = stopping_point;
@ -185,7 +185,7 @@ bool AC_Circle::update(float climb_rate_cms)
if (_terrain_alt) {
target_z_cm = _center.z + terr_offset;
} else {
target_z_cm = _pos_control.get_pos_target_z_cm();
target_z_cm = _pos_control.get_pos_desired_z_cm();
}
// if the circle_radius is zero we are doing panorama so no need to update loiter target
@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms)
target.y += - _radius * sinf(-_angle);
// heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position_xy_cm(), _center.tofloat().xy());
_yaw = get_bearing_cd(_pos_control.get_pos_desired_cm().xy().tofloat(), _center.tofloat().xy());
if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
_yaw += is_positive(_rate)?-9000.0f:9000.0f;
@ -313,12 +313,13 @@ void AC_Circle::init_start_angle(bool use_heading)
_angle = wrap_PI(_ahrs.yaw-M_PI);
} else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f &curr_pos = _inav.get_position_neu_cm();
if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) {
// curr_pos_desired is the position before we add offsets and terrain
const Vector3f &curr_pos_desired= _pos_control.get_pos_desired_cm().tofloat();
if (is_equal(curr_pos_desired.x,float(_center.x)) && is_equal(curr_pos_desired.y,float(_center.y))) {
_angle = wrap_PI(_ahrs.yaw-M_PI);
} else {
// get bearing from circle center to vehicle in radians
float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
float bearing_rad = atan2f(curr_pos_desired.y-_center.y, curr_pos_desired.x-_center.x);
_angle = wrap_PI(bearing_rad);
}
}