From f80b4f8732ce94c70faeee2a8e3bb59a5352a498 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 12 Oct 2016 14:06:14 +0200 Subject: [PATCH] fw_pos_ctrl_l1: added wrapping of lat lon coordinate after addition of delta reset Signed-off-by: Roman --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a457a6ae37..da6c95c00c 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -2299,10 +2299,22 @@ FixedwingPositionControl::task_main() // adjust navigation waypoints in position control mode if (_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_velocity_enabled && _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 = 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 = 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 = 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 = 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; } }