diff options
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
| -rw-r--r-- | src/modules/navigator/navigator_main.cpp | 21 |
1 files changed, 10 insertions, 11 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7ac885b4..c6fe93e9e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -953,8 +953,7 @@ Navigator::start_loiter() get_loiter_item(&_mission_item_triplet.current); - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + float global_min_alt = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { @@ -1080,9 +1079,9 @@ Navigator::start_rtl() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; @@ -1104,9 +1103,9 @@ Navigator::start_rtl_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; get_loiter_item(&_mission_item_triplet.current); @@ -1319,9 +1318,9 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) { if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } |