Fix

F

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -562,6 +562,8 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing");
 			_state = state::fail;
 
+			_state_start_time = now;
+
 		} else if (timeout) {
 			// Skip to next axis
 			mavlink_log_critical(&mavlink_log_pub, "Autotune axis timeout, skipping to next axis");
@@ -586,9 +588,11 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 				_state = state::fail;         // safety fallback
 				break;
 			}
+
+			_state_start_time = now;
 		}
 
-		_state_start_time = now;
+
 	}
 }
 

src/modules/mavlink/mavlink_main.cpp

@@ -1435,9 +1435,13 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
 		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
+#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
 		configure_stream_local("GLOBAL_POSITION", 5.0f);
+#endif
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
+#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
 		configure_stream_local("GNSS_INTEGRITY", 1.0f);
+#endif
 		configure_stream_local("GPS2_RAW", 1.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", 5.0f);
@@ -1510,7 +1514,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
 		configure_stream_local("GLOBAL_POSITION_INT", 50.0f);
+#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
 		configure_stream_local("GNSS_INTEGRITY", 1.0f);
+#endif
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
@@ -1671,7 +1677,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ESTIMATOR_STATUS", 5.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 2.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
+#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
 		configure_stream_local("GNSS_INTEGRITY", 1.0f);
+#endif
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
@@ -1770,10 +1778,14 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("CURRENT_MODE", 0.5f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
+#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
 		configure_stream_local("GLOBAL_POSITION", 10.0f);
+#endif
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
+#if defined(MAVLINK_MSG_ID_GNSS_INTEGRITY)
 		configure_stream_local("GNSS_INTEGRITY", 1.0f);
+#endif
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("HOME_POSITION", 0.5f);
@@ -1834,7 +1846,9 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 0.5f);
 		configure_stream_local("GLOBAL_POSITION_INT", 2.0f);
+#if defined(MAVLINK_MSG_ID_GLOBAL_POSITION)
 		configure_stream_local("GLOBAL_POSITION", 2.0f);
+#endif
 		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS2_RAW", 2.0f);
 		configure_stream_local("GPS_RAW_INT", 2.0f);

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -89,6 +89,8 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	_camera_pub = _node.Advertise<gz::msgs::GUICamera>("/gui/move_to/pose");
+
 	// OPTIONAL:
 	if (_sim_gz_en_gps.get()) {
 		if (!subscribeNavsat(false)) {
@@ -506,6 +508,21 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg)
 			const double dt = math::constrain((timestamp - _timestamp_prev) * 1e-6, 0.001, 0.1);
 			_timestamp_prev = timestamp;
 
+			gz::math::Pose3d model_pose = gz::msgs::Convert(msg.pose(p));
+			gz::math::Vector3d offset = gz::math::Vector3d(5.0, 5.0, 5.0);
+			double offset_yaw = 0.1;
+			gz::math::Vector3d offset_world = model_pose.Rot().RotateVector(offset);
+			gz::math::Vector3d camera_position = model_pose.Pos() + offset_world;
+			double model_yaw = model_pose.Rot().Yaw();
+			// Camera looks at the model with specified pitch and yaw offset
+			gz::math::Quaterniond camera_orientation(0.0, -0.1, model_yaw + offset_yaw);
+
+			gz::msgs::GUICamera camera_msg;
+			gz::msgs::Set(camera_msg.mutable_pose()->mutable_position(), camera_position);
+			gz::msgs::Set(camera_msg.mutable_pose()->mutable_orientation(), camera_orientation);
+			// camera_msg.set_duration(0.1);
+			_camera_pub.Publish(camera_msg);
+
 			gz::msgs::Vector3d pose_position = msg.pose(p).position();
 			gz::msgs::Quaternion pose_orientation = msg.pose(p).orientation();
 

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -178,6 +178,7 @@ private:
 
 	bool _realtime_clock_set{false};
 	gz::transport::Node _node;
+	gz::transport::Node::Publisher _camera_pub;
 
 	// GPS noise model
 	float _gps_pos_noise_n = 0.0f;