commander: fix status init & some status update changes

src/modules/commander/Commander.cpp

@@ -3769,9 +3769,6 @@ Commander *Commander::instantiate(int argc, char *argv[])
 {
 	Commander *instance = new Commander();
 
-	// XXX remove this once this is a class member
-	status = {};
-
 	if (instance) {
 		if (argc >= 2 && !strcmp(argv[1], "--hil")) {
 			instance->enable_hil();
@@ -3885,7 +3882,10 @@ void Commander::data_link_check(bool &status_changed)
 					if (telemetry.heartbeat_time > _datalink_last_heartbeat_gcs) {
 						status.data_link_lost = false;
 						status_changed = true;
-						mavlink_log_info(&mavlink_log_pub, "Data link regained");
+
+						if (_datalink_last_heartbeat_gcs != 0) {
+							mavlink_log_info(&mavlink_log_pub, "Data link regained");
+						}
 					}
 				}
 
@@ -3899,6 +3899,7 @@ void Commander::data_link_check(bool &status_changed)
 					if (telemetry.heartbeat_time > _datalink_last_heartbeat_onboard_controller) {
 						mavlink_log_info(&mavlink_log_pub, "Onboard controller regained");
 						_onboard_controller_lost = false;
+						status_changed = true;
 					}
 
 				}
@@ -3915,9 +3916,9 @@ void Commander::data_link_check(bool &status_changed)
 
 					if (_avoidance_system_lost) {
 						mavlink_log_info(&mavlink_log_pub, "Avoidance system regained");
+						status_changed = true;
+						_avoidance_system_lost = false;
 					}
-
-					_avoidance_system_lost = false;
 				}
 
 				break;
@@ -3947,6 +3948,7 @@ void Commander::data_link_check(bool &status_changed)
 
 		mavlink_log_critical(&mavlink_log_pub, "Onboard controller lost");
 		_onboard_controller_lost = true;
+		status_changed = true;
 	}
 
 	// AVOIDANCE SYSTEM state check (only if it is enabled)

src/modules/commander/Commander.hpp

@@ -167,10 +167,15 @@ private:
 
 	void mission_init();
 
+	void estimator_check(bool *status_changed);
+
+	void battery_status_check();
+
 	/**
 	 * Checks the status of all available data links and handles switching between different system telemetry states.
 	 */
 	void		data_link_check(bool &status_changed);
+
 	int		_telemetry_status_sub{-1};
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
@@ -190,14 +195,10 @@ private:
 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
 
-	void estimator_check(bool *status_changed);
-
 	int _battery_sub{-1};
 	uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	float _battery_current{0.0f};
 
-	void battery_status_check();
-
 	systemlib::Hysteresis	_auto_disarm_landed{false};
 	systemlib::Hysteresis	_auto_disarm_killed{false};