From 711bd2ce8706f085b90346439d09ce50d5f70e15 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 20 Oct 2020 15:21:04 +0200
Subject: [PATCH] Commander: use helper to set home position

---
 src/modules/commander/Commander.cpp | 28 ++++++++++++++++------------
 src/modules/commander/Commander.hpp |  1 +
 2 files changed, 17 insertions(+), 12 deletions(-)

diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index ef8752da89..fdd90e9583 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -808,20 +808,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						home_position_s home{};
 						home.timestamp = hrt_absolute_time();
 
-						home.lat = lat;
-						home.lon = lon;
-						home.alt = alt;
+						fillGlobalHomePos(home, lat, lon, alt);
 
 						home.manual_home = true;
-						home.valid_alt = true;
-						home.valid_hpos = true;
 
 						// update local projection reference including altitude
 						struct map_projection_reference_s ref_pos;
 						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;
+						float home_x;
+						float home_y;
+						map_projection_project(&ref_pos, lat, lon, &home_x, &home_y);
+						const float home_z = -(alt - local_pos.ref_alt);
+						fillLocalHomePos(home, home_x, home_y, home_z, 0.f);
 
 						/* mark home position as set */
 						_status_flags.condition_home_position_valid = _home_pub.update(home);
@@ -1398,12 +1396,18 @@ Commander::isGPosGoodForInitializingHomePos(const vehicle_global_position_s &gpo
 void
 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;
+	fillLocalHomePos(home, lpos.x, lpos.y, lpos.z, lpos.heading);
+}
+
+void
+Commander::fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) const
+{
+	home.x = x;
+	home.y = y;
+	home.z = z;
 	home.valid_lpos = true;
 
-	home.yaw = lpos.heading;
+	home.yaw = heading;
 }
 
 void Commander::fillGlobalHomePos(home_position_s &home, const vehicle_global_position_s &gpos) const
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index be2ec95465..4858b75eec 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -160,6 +160,7 @@ private:
 	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 fillLocalHomePos(home_position_s &home, float x, float y, float z, float heading) 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();