From 8155044b40b80adbbfcb10bea1add4e0446c6449 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 15 Oct 2016 11:21:30 +0200 Subject: [PATCH] fw_pos_control_l1: put comments for lat/lon wrapping Signed-off-by: Roman --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 +++ 1 file changed, 3 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 da6c95c00c..70541d0411 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 @@ -2301,6 +2301,9 @@ FixedwingPositionControl::task_main() && _global_pos.lat_lon_reset_counter != _pos_reset_counter) { // add position reset delta to previous waypoint coordinate + // wrap latitude value by applying an arcsinus to the sine of the latitude value + // this makes sure that latitude is always in the correct range of [-pi/2, pi/2] + // wrap longitude to the range [-pi, pi] _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));