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

Home position: use double for lat/lon and float for altitude, set home position to global position instead of GPS position once we have a fix

Browse Source
This commit is contained in:
Julian Oes
2013-12-27 11:07:45 +01:00
parent 677150388f
commit 32c7aea2a6
5 changed files with 29 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/navigator_main.cpp
+10 -11
View File
@@ -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
}