simulation: HIL_STATE_QUATERNION add angular acceleration and protect from initial lat/lon 0

src/modules/simulation/simulator_mavlink/SimulatorMavlink.cpp

@@ -531,6 +531,12 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 	mavlink_hil_state_quaternion_t hil_state;
 	mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
+	if ((hil_state.lat == 0) && (hil_state.lon == 0)) {
+		_time_us_prev = hil_state.time_usec;
+		return;
+	}
+
+	float dt_s = (hil_state.time_usec - _time_us_prev) * 1e-6f;
 
 	uint64_t timestamp_sample = hrt_absolute_time(); // TODO: HIL_STATE_QUATERNION.time_us
 
@@ -543,14 +549,14 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 	{
 		vehicle_angular_velocity_s angular_velocity{};
 		angular_velocity.timestamp_sample = timestamp_sample;
+
 		angular_velocity.xyz[0] = hil_state.rollspeed;
 		angular_velocity.xyz[1] = hil_state.pitchspeed;
 		angular_velocity.xyz[2] = hil_state.yawspeed;
 
-		// TODO: add derivative fields
+		angular_velocity.xyz_derivative[0] = (hil_state.rollspeed - _rollspeed_prev) / dt_s;
-		// angular_velocity.xyz_derivative[0] = hil_state.rollspeed;
+		angular_velocity.xyz_derivative[1] = (hil_state.pitchspeed - _pitchspeed_prev) / dt_s;
-		// angular_velocity.xyz_derivative[1] = hil_state.pitchspeed;
+		angular_velocity.xyz_derivative[2] = (hil_state.yawspeed - _yawspeed_prev) / dt_s;
-		// angular_velocity.xyz_derivative[2] = hil_state.yawspeed;
 
 		angular_velocity.timestamp = hrt_absolute_time();
 		_vehicle_angular_velocity_pub.publish(angular_velocity);
@@ -578,11 +584,14 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 		double lat = hil_state.lat * 1e-7; // degE7 -> deg
 		double lon = hil_state.lon * 1e-7; // degE7 -> deg
 
-		if (!_global_local_proj_ref.isInitialized()) {
+		if (!_global_local_proj_ref.isInitialized() && (hil_state.lon != 0)) {
 			_global_local_proj_ref.initReference(lat, lon);
 			_global_local_alt0 = hil_state.alt / 1000.f; // mm -> m
 		}
 
+		if (_global_local_proj_ref.isInitialized()) {
+
+
 			local_position.xy_valid = true;
 			local_position.z_valid = true;
 			local_position.v_xy_valid = true;
@@ -625,6 +634,7 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 			local_position.timestamp = hrt_absolute_time();
 			_vehicle_local_position_pub.publish(local_position);
 		}
+	}
 
 
 	// global position
@@ -647,6 +657,11 @@ void SimulatorMavlink::handle_message_hil_state_quaternion(const mavlink_message
 	}
 
 
+	_time_us_prev = hil_state.time_usec;
+	_rollspeed_prev = hil_state.rollspeed;
+	_pitchspeed_prev = hil_state.pitchspeed;
+	_yawspeed_prev = hil_state.yawspeed;
+
 #if 0
 
 	uint64_t timestamp = hrt_absolute_time();

src/modules/simulation/simulator_mavlink/SimulatorMavlink.hpp

@@ -260,6 +260,10 @@ private:
 	uORB::Publication<vehicle_local_position_s>   _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
 	uORB::Publication<vehicle_odometry_s>         _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
 
+	uint64_t _time_us_prev{0};
+	float _rollspeed_prev{0.f};
+	float _pitchspeed_prev{0.f};
+	float _yawspeed_prev{0.f};
 
 
 	//rpm