Commander: set home position in air

en/dis-able in-air home position via parameter COM_HOME_IN_AIR

msg/home_position.msg

@@ -14,5 +14,6 @@ float32 yaw				# Yaw angle in radians
 
 bool valid_alt		# true when the altitude has been set
 bool valid_hpos		# true when the latitude and longitude have been set
+bool valid_lpos		# true when the local position (xyz) has been set
 
 bool manual_home	# true when home position was set manually

src/modules/commander/Commander.cpp

@@ -821,6 +821,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						map_projection_init(&ref_pos, local_pos.ref_lat, local_pos.ref_lon);
 						map_projection_project(&ref_pos, lat, lon, &home.x, &home.y);
 						home.z = -(alt - local_pos.ref_alt);
+						home.valid_lpos = true;
 
 						/* mark home position as set */
 						_status_flags.condition_home_position_valid = _home_pub.update(home);
@@ -1316,49 +1317,118 @@ bool
 Commander::set_home_position()
 {
 	// Need global and local position fix to be able to set home
-	if (_status_flags.condition_global_position_valid && _status_flags.condition_local_position_valid) {
+	// but already set the home position in local coordinates if available
+	// in case the global position is only valid after takeoff
+	if (_status_flags.condition_local_position_valid) {
+
+		// Set home position in local coordinates
+		const vehicle_local_position_s &lpos = _local_position_sub.get();
+		_heading_reset_counter = lpos.heading_reset_counter;
+
+		home_position_s home{};
+		home.timestamp = hrt_absolute_time();
+		home.manual_home = false;
+		fillLocalHomePos(home, lpos);
+
+		if (_status_flags.condition_global_position_valid) {
+
+			const vehicle_global_position_s &gpos = _global_position_sub.get();
+
+			// Ensure that the GPS accuracy is good enough for intializing home
+			if (isGPosGoodForInitializingHomePos(gpos)) {
+				fillGlobalHomePos(home, gpos);
+				setHomePosValid();
+			}
+		}
+
+		_home_pub.update(home);
+	}
+
+	return _status_flags.condition_home_position_valid;
+}
+
+bool
+Commander::set_in_air_home_position()
+{
+	if (_status_flags.condition_local_position_valid
+	    && _status_flags.condition_global_position_valid) {
 
 		const vehicle_global_position_s &gpos = _global_position_sub.get();
+		home_position_s home{};
 
 		// Ensure that the GPS accuracy is good enough for intializing home
-		if ((gpos.eph <= _param_com_home_h_t.get()) && (gpos.epv <= _param_com_home_v_t.get())) {
+		if (isGPosGoodForInitializingHomePos(gpos)) {
-
+			home = _home_pub.get();
+			home.timestamp = hrt_absolute_time();
 			const vehicle_local_position_s &lpos = _local_position_sub.get();
 
-			// Set home position
+			if (_home_pub.get().valid_lpos) {
-			home_position_s home{};
+				// Back-compute lon, lat and alt of home position given the home
+				// and current positions in local frame
+				map_projection_reference_s ref_pos;
+				double home_lat;
+				double home_lon;
+				map_projection_init(&ref_pos, gpos.lat, gpos.lon);
+				map_projection_reproject(&ref_pos, home.x - lpos.x, home.y - lpos.y, &home_lat, &home_lon);
+				const float home_alt = gpos.alt + home.z;
+				fillGlobalHomePos(home, home_lat, home_lon, home_alt);
 
-			home.timestamp = hrt_absolute_time();
+			} else {
+				// Home position in local frame is unknowm, set
+				// home as current position
+				fillLocalHomePos(home, lpos);
+				fillGlobalHomePos(home, gpos);
+			}
 
-			home.lat = gpos.lat;
+			setHomePosValid();
-			home.lon = gpos.lon;
+			_home_pub.update(home);
-			home.valid_hpos = true;
+		}
+	}
 
-			home.alt = gpos.alt;
+	return _status_flags.condition_home_position_valid;
-			home.valid_alt = true;
+}
 
-			home.x = lpos.x;
+bool
-			home.y = lpos.y;
+Commander::isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const
-			home.z = lpos.z;
+{
+	return (gpos.eph <= _param_com_home_h_t.get())
+	       && (gpos.epv <= _param_com_home_v_t.get());
+}
 
-			home.yaw = lpos.heading;
+void
-			_heading_reset_counter = lpos.heading_reset_counter;
+Commander::fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const
+{
+	home.x = lpos.x;
+	home.y = lpos.y;
+	home.z = lpos.z;
+	home.valid_lpos = true;
 
-			home.manual_home = false;
+	home.yaw = lpos.heading;
+}
 
-			// play tune first time we initialize HOME
+void Commander::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const
-			if (!_status_flags.condition_home_position_valid) {
+{
-				tune_home_set(true);
+	fillGlobalHomePos(home, gpos.lat, gpos.lon, gpos.alt);
-			}
+}
 
-			// mark home position as set
+void Commander::fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const
-			_status_flags.condition_home_position_valid = _home_pub.update(home);
+{
+	home.lat = lat;
+	home.lon = lon;
+	home.valid_hpos = true;
+	home.alt = alt;
+	home.valid_alt = true;
+}
 
-			return _status_flags.condition_home_position_valid;
+void Commander::setHomePosValid()
-		}
+{
+	// play tune first time we initialize HOME
+	if (!_status_flags.condition_home_position_valid) {
+		tune_home_set(true);
 	}
 
-	return false;
+	// mark home position as set
+	_status_flags.condition_home_position_valid = true;
 }
 
 bool
@@ -1655,14 +1725,19 @@ Commander::run()
 					_gpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
 					_lpos_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
 					_lvel_probation_time_us = _param_com_pos_fs_prob.get() * 1_s;
+				}
 
-					// automatically set or update home position
+				// automatically set or update home position
-					if (!_home_pub.get().manual_home) {
+				if (!_home_pub.get().manual_home) {
-						// set the home position when taking off, but only if we were previously disarmed
+					// set the home position when taking off, but only if we were previously disarmed
-						// and at least 500 ms from commander start spent to avoid setting home on in-air restart
+					// and at least 500 ms from commander start spent to avoid setting home on in-air restart
-						if (_should_set_home_on_takeoff && (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
+					if (_should_set_home_on_takeoff && !_land_detector.landed &&
-							_should_set_home_on_takeoff = false;
+					    (hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
-							set_home_position();
+						if (was_landed) {
+							_should_set_home_on_takeoff = !set_home_position();
+
+						} else if (_param_com_home_in_air.get()) {
+							_should_set_home_on_takeoff = !set_in_air_home_position();
 						}
 					}
 				}
@@ -2373,7 +2448,7 @@ Commander::run()
 			const vehicle_local_position_s &local_position = _local_position_sub.get();
 
 			if (!_armed.armed) {
-				if (_status_flags.condition_home_position_valid) {
+				if (_home_pub.get().valid_lpos) {
 					if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
 						/* distance from home */
 						float home_dist_xy = -1.0f;

src/modules/commander/Commander.hpp

@@ -157,6 +157,12 @@ private:
 
 	bool set_home_position();
 	bool set_home_position_alt_only();
+	bool set_in_air_home_position();
+	bool isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpos) const;
+	void fillLocalHomePos(home_position_s &home, const vehicle_local_position_s &lpos) const;
+	void fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const;
+	void fillGlobalHomePos(home_position_s &home, double lat, double lon, float alt) const;
+	void setHomePosValid();
 	void updateHomePositionYaw(float yaw);
 
 	void update_control_mode();
@@ -187,6 +193,7 @@ private:
 
 		(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
 		(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,
+		(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
 
 		(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 		(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, 	/*Not realy used for now*/

src/modules/commander/commander_params.c

@@ -213,6 +213,18 @@ PARAM_DEFINE_FLOAT(COM_HOME_H_T, 5.0f);
  */
 PARAM_DEFINE_FLOAT(COM_HOME_V_T, 10.0f);
 
+/**
+ * Allows setting the home position after takeoff
+ *
+ * If set to true, the autopilot is allowed to set its home position after takeoff
+ * The true home position is back-computed if a local position is estimate if available.
+ * If no local position is available, home is set to the current position.
+ *
+ * @boolean
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_HOME_IN_AIR, 0);
+
 /**
  * RC control input mode
  *