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

4 files changed, 27 insertions, 36 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3fc1d2c19..442f16a58 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1048,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (!home_position_set && gps_position.fix_type >= 3 &&
 			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&	// XXX note that vdop is 0 for mtk
-			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) {
+			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
+			    && global_position.valid) {
 				/* copy position data to uORB home message, store it locally as well */
-				// TODO use global position estimate
-				home.lat = gps_position.lat;
-				home.lon = gps_position.lon;
-				home.alt = gps_position.alt;
 
-				home.eph_m = gps_position.eph_m;
-				home.epv_m = gps_position.epv_m;
 
-				home.s_variance_m_s = gps_position.s_variance_m_s;
-				home.p_variance_m = gps_position.p_variance_m;
+				home.lat = (double)global_position.lat / 1e7d;
+				home.lon = (double)global_position.lon / 1e7d;
+				home.altitude = (float)global_position.alt / 1e3f;
 
-				double home_lat_d = home.lat * 1e-7;
-				double home_lon_d = home.lon * 1e-7;
-				warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d);
+				warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude);
+				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude);
 
 				/* announce new home position */
 				if (home_pub > 0) {
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index de902f3da..96888f06a 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -657,7 +657,7 @@ l_home(const struct listener *l)
 
 	orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home);
 
-	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
+	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f);
 }
 
 void
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
 	}
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 7e1b82a0f..1cf37e29c 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,9 +35,10 @@
 
 /**
  * @file home_position.h
- * Definition of the GPS home position uORB topic.
+ * Definition of the home position uORB topic.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef TOPIC_HOME_POSITION_H_
@@ -55,16 +57,12 @@
  */
 struct home_position_s
 {
-	uint64_t timestamp;             /**< Timestamp (microseconds since system boot)   */
-	uint64_t time_gps_usec;         /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module   */
-	
-	int32_t lat;                    /**< Latitude in 1E7 degrees */
-	int32_t lon;                    /**< Longitude in 1E7 degrees */
-	int32_t alt;                    /**< Altitude in 1E3 meters (millimeters) above MSL */
-	float eph_m;                   /**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;                   /**< GPS VDOP horizontal dilution of position in m */
-	float s_variance_m_s;               /**< speed accuracy estimate m/s */
-	float p_variance_m;               /**< position accuracy estimate m */
+	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
+
+	bool altitude_is_relative;
+	double lat;				/**< Latitude in degrees 			*/
+	double lon;				/**< Longitude in degrees			*/
+	float altitude;				/**< Altitude in meters				*/
 };
 
 /**