Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Copter: ModeFollow: fix YAW_BEHAVE_FACE_LEAD_VEHICLE

Browse Source
This commit is contained in:
Leonard Hall
2026-07-08 10:35:23 +09:00
committed by Randy Mackay
parent b8f83fc3a5
commit f6c0aa9cf9
1 changed files with 5 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ArduCopter/mode_follow.cpp
+5 -3
View File
@@ -99,9 +99,11 @@ void ModeFollow::run()
Vector3p pos_ned_m; // vector to lead vehicle
Vector3f vel_ned_ms; // velocity of lead vehicle
Vector3f accel_ned_mss; // accel of lead vehicle
if (g2.follow.get_target_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, accel_ned_mss))
if (pos_ned_m.xy().length_squared() > 1.0) {
yaw_rad = (pos_ned_m.xy() - pos_control->get_pos_target_NED_m().xy()).tofloat().angle();
if (g2.follow.get_target_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, accel_ned_mss)) {
const Vector2p vec_to_lead_ne_m = pos_ned_m.xy() - pos_control->get_pos_target_NED_m().xy();
if (vec_to_lead_ne_m.length_squared() > 1.0) {
yaw_rad = vec_to_lead_ne_m.angle();
}
}
break;
}