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

fw_pos_ctrl_l1: added wrapping of lat lon coordinate after addition of

Browse Source 
delta reset

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-11-21 07:29:21 +01:00
parent b3f9114b1c
commit f80b4f8732
1 changed files with 12 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+12
View File
@@ -2299,10 +2299,22 @@ FixedwingPositionControl::task_main()
// adjust navigation waypoints in position control mode // adjust navigation waypoints in position control mode
if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled
&& _global_pos.lat_lon_reset_counter != _pos_reset_counter) { && _global_pos.lat_lon_reset_counter != _pos_reset_counter) {
// add position reset delta to previous waypoint coordinate
_hdg_hold_prev_wp.lat += _global_pos.delta_lat_lon[0]; _hdg_hold_prev_wp.lat += _global_pos.delta_lat_lon[0];
_hdg_hold_prev_wp.lat = M_RAD_TO_DEG * asin(sin(_hdg_hold_prev_wp.lat * M_DEG_TO_RAD));
_hdg_hold_prev_wp.lon += _global_pos.delta_lat_lon[1]; _hdg_hold_prev_wp.lon += _global_pos.delta_lat_lon[1];
_hdg_hold_prev_wp.lon = M_RAD_TO_DEG * matrix::wrap_pi(_hdg_hold_prev_wp.lon * M_DEG_TO_RAD);
// add position reset delta to current waypoint coordinate
_hdg_hold_curr_wp.lat += _global_pos.delta_lat_lon[0]; _hdg_hold_curr_wp.lat += _global_pos.delta_lat_lon[0];
_hdg_hold_curr_wp.lat = M_RAD_TO_DEG * asin(sin(_hdg_hold_curr_wp.lat * M_DEG_TO_RAD));
_hdg_hold_curr_wp.lon += _global_pos.delta_lat_lon[1]; _hdg_hold_curr_wp.lon += _global_pos.delta_lat_lon[1];
_hdg_hold_curr_wp.lon = M_RAD_TO_DEG * matrix::wrap_pi(_hdg_hold_curr_wp.lon * M_DEG_TO_RAD);
// update reset counter
_pos_reset_counter = _global_pos.lat_lon_reset_counter; _pos_reset_counter = _global_pos.lat_lon_reset_counter;
} }
} }