Merge pull request #1657 from Zefz/commander-home-pos-cleanup

Commander home pos cleanup

1 files changed, 84 insertions, 74 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bae1ed2e4..aac6b2002 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -241,6 +241,13 @@ transition_result_t check_navigation_state_machine(struct vehicle_status_s *stat
 transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);
 
 /**
+* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
+*		 time the vehicle is armed with a good GPS fix.
+**/
+static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
+					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition);
+
+/**
  * Loop that runs at a lower rate and priority for calibration and parameter tasks.
  */
 void *commander_low_prio_loop(void *arg);
@@ -560,7 +567,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-	/* Flight termination */
+		/* Flight termination */
 	case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
 			if (cmd->param1 > 0.5f) {
 				//XXX update state machine?
@@ -716,6 +723,53 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 	return true;
 }
 
+/**
+* @brief This function initializes the home position of the vehicle. This happens first time we get a good GPS fix and each
+*		 time the vehicle is armed with a good GPS fix.
+**/
+static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home,
+					const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition)
+{
+	//Need global position fix to be able to set home
+	if (!status.condition_global_position_valid) {
+		return;
+	}
+
+	//Ensure that the GPS accuracy is good enough for intializing home
+	if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
+		return;
+	}
+
+	//Set home position
+	home.timestamp = hrt_absolute_time();
+	home.lat = globalPosition.lat;
+	home.lon = globalPosition.lon;
+	home.alt = globalPosition.alt;
+
+	home.x = localPosition.x;
+	home.y = localPosition.y;
+	home.z = localPosition.z;
+
+	warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
+	mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+
+	/* announce new home position */
+	if (homePub > 0) {
+		orb_publish(ORB_ID(home_position), homePub, &home);
+
+	} else {
+		homePub = orb_advertise(ORB_ID(home_position), &home);
+	}
+
+	//Play tune first time we initialize HOME
+	if (!status.condition_home_position_valid) {
+		tune_positive(true);
+	}
+
+	/* mark home position as set */
+	status.condition_home_position_valid = true;
+}
+
 int commander_thread_main(int argc, char *argv[])
 {
 	/* not yet initialized */
@@ -904,7 +958,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool critical_battery_voltage_actions_done = false;
 
 	hrt_abstime last_idle_time = 0;
-	hrt_abstime start_time = 0;
 
 	bool status_changed = true;
 	bool param_init_forced = true;
@@ -1035,7 +1088,7 @@ int commander_thread_main(int argc, char *argv[])
 	commander_initialized = true;
 	thread_running = true;
 
-	start_time = hrt_absolute_time();
+	const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
 
 	transition_result_t arming_ret;
 
@@ -1096,8 +1149,8 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* set vehicle_status.is_vtol flag */
-				status.is_vtol =  (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
-				    (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
+				status.is_vtol = (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR) ||
+						 (status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR);
 
 				/* check and update system / component ID */
 				param_get(_param_system_id, &(status.system_id));
@@ -1259,12 +1312,12 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			//Notify the user if the status of the safety switch changes
-			if(safety.safety_switch_available && previous_safety_off != safety.safety_off) {
+			if (safety.safety_switch_available && previous_safety_off != safety.safety_off) {
 
-				if(safety.safety_off) {
+				if (safety.safety_off) {
 					set_tune(TONE_NOTIFY_POSITIVE_TUNE);
-				}
-				else {
+
+				} else {
 					tune_neutral(true);
 				}
 
@@ -1279,6 +1332,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* vtol status changed */
 			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
 			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
+
 			/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
 			if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
 				status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
@@ -1325,34 +1379,6 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid),
 			    &status_changed);
 
-		/* update home position */
-		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
-		    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
-
-			home.lat = global_position.lat;
-			home.lon = global_position.lon;
-			home.alt = global_position.alt;
-
-			home.x = local_position.x;
-			home.y = local_position.y;
-			home.z = local_position.z;
-
-			warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
-			mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
-
-			/* announce new home position */
-			if (home_pub > 0) {
-				orb_publish(ORB_ID(home_position), home_pub, &home);
-
-			} else {
-				home_pub = orb_advertise(ORB_ID(home_position), &home);
-			}
-
-			/* mark home position as set */
-			status.condition_home_position_valid = true;
-			tune_positive(true);
-		}
-
 		/* update condition_local_position_valid and condition_local_altitude_valid */
 		/* hysteresis for EPH */
 		bool local_eph_good;
@@ -1401,7 +1427,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (hrt_absolute_time() > commander_boot_timestamp + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
@@ -1609,7 +1635,8 @@ int commander_thread_main(int argc, char *argv[])
 
 			} else {
 				if (status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",(hrt_absolute_time()-status.rc_signal_lost_timestamp)/1000);
+					mavlink_log_critical(mavlink_fd, "RC SIGNAL REGAINED after %llums",
+							     (hrt_absolute_time() - status.rc_signal_lost_timestamp) / 1000);
 					status_changed = true;
 				}
 			}
@@ -1710,9 +1737,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		} else {
 			if (!status.rc_signal_lost) {
-				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)",hrt_absolute_time()/1000);
+				mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST (at t=%llums)", hrt_absolute_time() / 1000);
 				status.rc_signal_lost = true;
-				status.rc_signal_lost_timestamp=sp_man.timestamp;
+				status.rc_signal_lost_timestamp = sp_man.timestamp;
 				status_changed = true;
 			}
 		}
@@ -1859,44 +1886,23 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-
 		//Get current timestamp
 		const hrt_abstime now = hrt_absolute_time();
 
+		//First time home position update
+		if (!status.condition_home_position_valid) {
+			commander_set_home_position(home_pub, home, local_position, global_position);
+		}
+
+		/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
+			commander_set_home_position(home_pub, home, local_position, global_position);
+		}
+
 		/* print new state */
 		if (arming_state_changed) {
 			status_changed = true;
 			mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]);
-
-			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
-			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
-			    (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
-
-				// TODO remove code duplication (setting home is also done somewhere else in this file)
-				home.timestamp = now;
-				home.lat = global_position.lat;
-				home.lon = global_position.lon;
-				home.alt = global_position.alt;
-
-				home.x = local_position.x;
-				home.y = local_position.y;
-				home.z = local_position.z;
-
-				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
-				mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
-
-				/* announce new home position */
-				if (home_pub > 0) {
-					orb_publish(ORB_ID(home_position), home_pub, &home);
-
-				} else {
-					home_pub = orb_advertise(ORB_ID(home_position), &home);
-				}
-
-				/* mark home position as set */
-				status.condition_home_position_valid = true;
-			}
-
 			arming_state_changed = false;
 		}
 
@@ -1917,11 +1923,14 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
+
 			if (status.failsafe) {
 				mavlink_log_critical(mavlink_fd, "failsafe mode on");
+
 			} else {
 				mavlink_log_critical(mavlink_fd, "failsafe mode off");
 			}
+
 			failsafe_old = status.failsafe;
 		}
 
@@ -1967,7 +1976,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 
 			//Notify the user that it is safe to approach the vehicle
-			if(arm_tune_played) {
+			if (arm_tune_played) {
 				tune_neutral(true);
 			}
 
@@ -2434,6 +2443,7 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 		}
+
 		break;
 
 	default:
@@ -2472,7 +2482,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-		tune_positive(true);
+			tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED: