Offboard Attitude Control using Simulink

Hello Everyone

I’m trying to understand offboard control using the setpoint_raw/attitude mavros messages.

I’ve created a small simulink model that publishes this roll angle (phi) reference:

This reference is then converted to a quaternion and published using the following code:

I’ve tested this with a SITL simulation in Gazebo and the tailsitter is (kind of) reacting as expected, i.e. it’s periodically changing the roll angle, but the displayed roll angle setpoint and thrust setpoints in the logs seem incorrect:

Is it possible that the publishing frequency is too low (It’s about 30 Hz)? When listening to the vehicle_attitude_setpoint topic I get this:

TOPIC: vehicle_attitude_setpoint instance 0 #2
vehicle_attitude_setpoint_s
timestamp: 66664000 (1.488000 seconds ago)
roll_body: -0.0169
pitch_body: -0.0246
yaw_body: 1.5140
yaw_sp_move_rate: 0.0000
q_d: [0.7269, 0.0023, -0.0148, 0.6866]
thrust_body: [0.0000, 0.0000, -0.5721]
q_d_valid: True
roll_reset_integral: False
pitch_reset_integral: False
yaw_reset_integral: False
fw_control_yaw: False
apply_flaps: 0

TOPIC: vehicle_attitude_setpoint instance 0 #3
vehicle_attitude_setpoint_s
timestamp: 66664000 (1.496000 seconds ago)
roll_body: -0.0169
pitch_body: -0.0246
yaw_body: 1.5140
yaw_sp_move_rate: 0.0000
q_d: [0.7269, 0.0023, -0.0148, 0.6866]
thrust_body: [0.0000, 0.0000, -0.5721]
q_d_valid: True
roll_reset_integral: False
pitch_reset_integral: False
yaw_reset_integral: False
fw_control_yaw: False
apply_flaps: 0

TOPIC: vehicle_attitude_setpoint instance 0 #4
vehicle_attitude_setpoint_s
timestamp: 68164000 (0.004000 seconds ago)
roll_body: -0.0000
pitch_body: -0.0000
yaw_body: 1.5708
yaw_sp_move_rate: 0.0000
q_d: [-0.7071, 0.0000, 0.0000, -0.7071]
thrust_body: [0.0000, 0.0000, -0.7000]
q_d_valid: True
roll_reset_integral: False
pitch_reset_integral: False
yaw_reset_integral: False
fw_control_yaw: False
apply_flaps: 0

TOPIC: vehicle_attitude_setpoint instance 0 #5
vehicle_attitude_setpoint_s
timestamp: 66664000 (1.508000 seconds ago)
roll_body: -0.0169
pitch_body: -0.0246
yaw_body: 1.5140
yaw_sp_move_rate: 0.0000
q_d: [0.7269, 0.0023, -0.0148, 0.6866]
thrust_body: [0.0000, 0.0000, -0.5721]
q_d_valid: True
roll_reset_integral: False
pitch_reset_integral: False
yaw_reset_integral: False
fw_control_yaw: False
apply_flaps: 0

Here it seems that only one message is actually from the setpoint_raw/attitude publication (the second last one with thrust_body: [0.0000, 0.0000, -0.7000])…

The other messages seem to be from the onboard position control… Why is that?

Also the timestamp seems to not be synchronized…

Finally I also get this error: TM : Time jump detected. Resetting time synchroniser.
I heard that this can come from a inadequate computer?

Any help would be greatly appreciated!