A bug for HIL mode in hil_state_quaternion message acquisition

[RflySim]

The definition of message mavlink_hil_state_quaternion_t is
int16_t xacc; /< X acceleration (mg)/
int16_t yacc; /< Y acceleration (mg)/
int16_t zacc; /< Z acceleration (mg)/

in "Firmware\src\modules\mavlink\mavlink_receiver.cpp", Line 2413 of the function "handle_message_hil_state_quaternion(mavlink_message_t *msg)"
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
accel.timestamp = timestamp;
accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
accel.y_raw = hil_state.yacc / CONSTANTS_ONE_G * 1e3f;
accel.z_raw = hil_state.zacc / CONSTANTS_ONE_G * 1e3f;
accel.x = hil_state.xacc;
accel.y = hil_state.yacc;
accel.z = hil_state.zacc;
accel.temperature = 25.0f;

if (_accel_pub == nullptr) {
_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);

} else {
orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
}

It is obviously wrong, because hil_state.xacc is integer (with unit 10^-3*9.8 m/s^2) while accel.x is a float (with unit m/s^2) .

The correct code should be
accel.x_raw = hil_state.xacc;
accel.y_raw = hil_state.yacc;
accel.z_raw = hil_state.zacc;
accel.x = hil_state.xacc / 1000.0f * CONSTANTS_ONE_G ;
accel.y = hil_state.xacc / 1000.0f * CONSTANTS_ONE_G;
accel.z = hil_state.xacc / 1000.0f * CONSTANTS_ONE_G;

Additional context
I am working on an HIL simulator covering all types of aircraft and vehicles. It works fine before the PX4 version 1.7. However, since version 1.9.0, I am not able to obtain the correct mavlink_hil_actuator_controls_t message (all zeros after being armed) to control my vehicle unless I send mavlink_hil_state_quaternion_t. Before version 1.8.0, the mavlink_hil_state_quaternion_t message is not necessary for the HIL Simulation.

In version 1.9.0, I can send mavlink_hil_state_quaternion_t message to obtain motor control signals, but it will cause "No local position" error in Position Mode due to the above bug.

I delete the code
"orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);"
then, it will work properly.

There is one more problem, why mavlink_hil_state_quaternion_t is required in 1.9.0 compared with the previous versions. I think mavlink_hil_gps_t and mavlink_hil_sensor_t should be enough, because most information of mavlink_hil_state_quaternion_t has been included in the previous two message.

[rligocki]

So after deleting "orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);" everything works? Or do I need to create mavlink_hil_state_quaternion_t message?

[RflySim]

1. In 1.9.*, the HIL_ACTUATOR_CONTROLS message cannot be received only if the simulator program sends mavlink_hil_state_quaternion_t message. Even if HIL_ACTUATOR_CONTROLS is successfully received by the simulator, the vehicle cannot fly normally due to the two sensor uORB publishing sources (HIL_SENSOR and HIL QUATERNION) are conflicting.
2. After deleting "orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel)" and sending mavlink_hil_state_quaternion_t message to PX4, everything works for me.

In the HIL mode of 1.9.*, it seems that PX4 requires the uORB messages (vehicle_global_position and vehicle_local_position) published in handle_message_hil_state_quaternion() of mavlink_receiver.cpp to generate MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS.

[rligocki]

How do you send mavlink_hil_state_quaternion_t message? I removed orb_publish and normal plane and rover start to work, but multicopter and VTOL don't.

[RflySim]

How do you send mavlink_hil_state_quaternion_t message? I removed orb_publish and normal plane and rover start to work, but multicopter and VTOL don't.

I have developed a HIL simulator by myself, the mavlink_hil_state_quaternion_t message is generated according to the vehicle simulated state and the mavlink_hil_state_quaternion_t MAVLink message definition. I have tried to send to mavlink_hil_state_quaternion_t message along with HIL_SENSOR message (250HZ) or HIL_GPS (5HZ~10Hz) message through serial port, both of them works.

[julianoes]

Have you head a look at docs about lockstep?
https://dev.px4.io/master/en/simulation/#lockstep-simulation

[rligocki]

@XunhuaDai Is your api for PX4 comm available? I would be very grateful for any help.

[RflySim]

Have you head a look at docs about lockstep?
https://dev.px4.io/master/en/simulation/#lockstep-simulation

Lockstep is for SITL, and my problem is about HITL. I have checked the code in Firmware\src\modules\simulator\simulator_mavlink.cpp for SITL, and the ORB_ID(sensor_accel) message is not published in MAVLINK_MSG_ID_HIL_STATE_QUATERNION handling function. Therefore, the bug is not exist in SITL mode.

[RflySim]

@XunhuaDai Is your api for PX4 comm available? I would be very grateful for any help.

My comm API is normal serial communication in QT/C++ environment, and my model and sensor data are from MATLAB/Simulink with automatic code generation technique. My simulator works well for different types of vehicles and different PX4 versions (<=1.8), so i'm confident about my model and interface data. The problem is that mavlink_receiver.cpp handles the MAVLINK_MSG_ID_HIL_STATE_QUATERNION in a wrong way, and publishes wrong sensor data (ORB_ID(sensor_accel)) conflicting with MAVLINK_MSG_ID_HIL_SENSOR message.

Remove it or correct it will both solve my problem.

[stale]

This issue has been automatically marked as stale because it has not had recent activity. Thank you for your contributions.