fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 06:10:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Convert L1 controller to local coordinates

Browse Source
This commit is contained in:
Jaeyoung Lim
2022-03-17 21:04:02 +01:00
committed by JaeyoungLim
parent 897506267d
commit 28d34bf095
4 changed files with 59 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_pos_control/RoverPositionControl.cpp
+5 -1
View File
@@ -281,7 +281,11 @@ RoverPositionControl::control_position(const matrix::Vector2d &current_position,
_pos_ctrl_state = STOPPING; // We are closer than loiter radius to waypoint, stop.
} else {
_gnd_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
prev_wp(1));
_gnd_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed_2d);
_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = mission_throttle;