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

Copter: zigzag uses desired xy instead of actual

This commit is contained in:
Randy Mackay 2024-10-03 12:13:04 +09:00
parent e06c9048e0
commit be1c87f3d1
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/mode_zigzag.cpp
View File

@ -161,7 +161,7 @@ void ModeZigZag::run()
void ModeZigZag::save_or_move_to_destination(Destination ab_dest) void ModeZigZag::save_or_move_to_destination(Destination ab_dest)
{ {
// get current position as an offset from EKF origin // get current position as an offset from EKF origin
const Vector2f curr_pos {inertial_nav.get_position_xy_cm()}; const Vector2f curr_pos = pos_control->get_pos_desired_cm().xy().tofloat();
// handle state machine changes // handle state machine changes
switch (stage) { switch (stage) {
@ -431,7 +431,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve
} }
// get distance from vehicle to start_pos // get distance from vehicle to start_pos
const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()}; const Vector2f curr_pos2d = pos_control->get_pos_desired_cm().xy().tofloat();
Vector2f veh_to_start_pos = curr_pos2d - start_pos; Vector2f veh_to_start_pos = curr_pos2d - start_pos;
// lengthen AB_diff so that it is at least as long as vehicle is from start point // lengthen AB_diff so that it is at least as long as vehicle is from start point
@ -499,7 +499,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con
float scalar = constrain_float(_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared()); float scalar = constrain_float(_side_dist, 0.1f, 100.0f) * 100 / safe_sqrt(AB_side.length_squared());
// get distance from vehicle to start_pos // get distance from vehicle to start_pos
const Vector2f curr_pos2d {inertial_nav.get_position_xy_cm()}; const Vector2f curr_pos2d = pos_control->get_pos_desired_cm().xy().tofloat();
next_dest.xy() = curr_pos2d + (AB_side * scalar); next_dest.xy() = curr_pos2d + (AB_side * scalar);
// if we have a downward facing range finder then use terrain altitude targets // if we have a downward facing range finder then use terrain altitude targets