Telemetry.set_rate_attitude() seems to have no effect

[fas2037]

'm trying to set the rate of the attitude messages. I tried setting it to a lower hertz rate, for example 10 Hz. But the messages still seem to get published in a very high rate (100 Hz)

I subscribe to Telemetry.subscribe_attitude_quaternion()

The function should set the rate of this MAVLINK Message.

MAVSDK/src/plugins/telemetry/telemetry_impl.cpp

Lines 215 to 219 in ee84bd9

	Telemetry::Result TelemetryImpl::set_rate_attitude(double rate_hz)
	{
	return telemetry_result_from_command_result(
	_parent->set_msg_rate(MAVLINK_MSG_ID_ATTITUDE_QUATERNION, rate_hz));
	}

So I took a look at this:

MAVSDK/src/plugins/telemetry/telemetry_impl.cpp

Lines 640 to 720 in ee84bd9

	void TelemetryImpl::process_attitude(const mavlink_message_t& message)
	{
	mavlink_attitude_t attitude;
	mavlink_msg_attitude_decode(&message, &attitude);
	Telemetry::EulerAngle euler_angle;
	euler_angle.roll_deg = to_deg_from_rad(attitude.roll);
	euler_angle.pitch_deg = to_deg_from_rad(attitude.pitch);
	euler_angle.yaw_deg = to_deg_from_rad(attitude.yaw);
	euler_angle.timestamp_us = static_cast<uint64_t>(attitude.time_boot_ms) * 1000;
	Telemetry::AngularVelocityBody angular_velocity_body;
	angular_velocity_body.roll_rad_s = attitude.rollspeed;
	angular_velocity_body.pitch_rad_s = attitude.pitchspeed;
	angular_velocity_body.yaw_rad_s = attitude.yawspeed;
	set_attitude_angular_velocity_body(angular_velocity_body);
	auto quaternion = mavsdk::to_quaternion_from_euler_angle(euler_angle);
	set_attitude_quaternion(quaternion);
	std::lock_guard<std::mutex> lock(_subscription_mutex);
	if (_attitude_quaternion_angle_subscription) {
	auto callback = _attitude_quaternion_angle_subscription;
	auto arg = attitude_quaternion();
	_parent->call_user_callback([callback, arg]() { callback(arg); });
	}
	if (_attitude_euler_angle_subscription) {
	auto callback = _attitude_euler_angle_subscription;
	auto arg = attitude_euler();
	_parent->call_user_callback([callback, arg]() { callback(arg); });
	}
	if (_attitude_angular_velocity_body_subscription) {
	auto callback = _attitude_angular_velocity_body_subscription;
	auto arg = attitude_angular_velocity_body();
	_parent->call_user_callback([callback, arg]() { callback(arg); });
	}
	}
	void TelemetryImpl::process_attitude_quaternion(const mavlink_message_t& message)
	{
	mavlink_attitude_quaternion_t mavlink_attitude_quaternion;
	mavlink_msg_attitude_quaternion_decode(&message, &mavlink_attitude_quaternion);
	Telemetry::Quaternion quaternion;
	quaternion.w = mavlink_attitude_quaternion.q1;
	quaternion.x = mavlink_attitude_quaternion.q2;
	quaternion.y = mavlink_attitude_quaternion.q3;
	quaternion.z = mavlink_attitude_quaternion.q4;
	quaternion.timestamp_us =
	static_cast<uint64_t>(mavlink_attitude_quaternion.time_boot_ms) * 1000;
	Telemetry::AngularVelocityBody angular_velocity_body;
	angular_velocity_body.roll_rad_s = mavlink_attitude_quaternion.rollspeed;
	angular_velocity_body.pitch_rad_s = mavlink_attitude_quaternion.pitchspeed;
	angular_velocity_body.yaw_rad_s = mavlink_attitude_quaternion.yawspeed;
	set_attitude_quaternion(quaternion);
	set_attitude_angular_velocity_body(angular_velocity_body);
	std::lock_guard<std::mutex> lock(_subscription_mutex);
	if (_attitude_quaternion_angle_subscription) {
	auto callback = _attitude_quaternion_angle_subscription;
	auto arg = attitude_quaternion();
	_parent->call_user_callback([callback, arg]() { callback(arg); });
	}
	if (_attitude_euler_angle_subscription) {
	auto callback = _attitude_euler_angle_subscription;
	auto arg = attitude_euler();
	_parent->call_user_callback([callback, arg]() { callback(arg); });
	}
	if (_attitude_angular_velocity_body_subscription) {
	auto callback = _attitude_angular_velocity_body_subscription;
	auto arg = attitude_angular_velocity_body();
	_parent->call_user_callback([callback, arg]() { callback(arg); });
	}
	}

I assume that both of this functions are able to publish the quaternion. Is there any cause for that? When checking with QGC both MAVLINK_MSG_ID_ATTITUDE_QUATERNION and MAVLINK_MSG_ID_ATTITUDE are published with 50 Hz. I guess as MAVSDK takes both messages into account and therefore the receiving rate of 100 Hz is archived.

But when using Telemetry.set_rate_attitude() I assume that this will set the rate of the subscription to a attitude message, for example the quaternion, to the desired one. This is not the case right now.

[julianoes]

Good point, this is a bit messy. The problem is that PX4 was supposed to move from ATTITUDE to ATTITUDE_QUATERNION in general, however, that has not happened, and for compatibility reasons probably won't happen soon.

In an idea world neither of the messages would be sent by default allowing you to request them at whatever rate you want them.

My suggestion would be to change PX4 so it does not send both at 50 Hz.

[elpiel]

My suggestion would be to change PX4 so it does not send both at 50 Hz.
Is this the default for sending telemetry messages in PX4?

In my observation, I receive telemetry responses for Position & Quarternions in 200 milliseconds from the MAVSDK server which is well below 50 Hz, I think that's 5 Hz?

I still have some evaluation to do and tweaking of the setup in order to validate how the communication is performed. I'm currently using a simulation & MAVSDK server for ease of use.

[linuxguy123]

This is still a problem in MAVSDK version: v1.4.3.

[julianoes]

Right. I guess we need to decouple the two attitude subscriptions and respective set_rate callers. So, for instance, instead of calling the attitude_euler callback when ATTITUDE or ATTITUDE_QUATERNION are called, only do so when ATTITUDE arrives.

[julianoes]

Fix coming:

It would be great if someone could review and test it, so I don't have to 😄.

mavlink/MAVSDK-Proto#293
#1878

[julianoes]

👋 anyone willing to test this? Or am I on my own here...

[elpiel]

@julianoes let me try to look into this but last time I was running the sim locally I was getting bad performance. Probably because of my machine, however, I will try to test this anyway.

[julianoes]

You should be able to test it even when it's running a bit slower. You can see how fast it runs in the bottom left in Gazebo, and then you need to adjust the rates that telemetry is sent by it.

[julianoes]

@elpiel 👋

[elpiel]

You should be able to test it even when it's running a bit slower. You can see how fast it runs in the bottom left in Gazebo, and then you need to adjust the rates that telemetry is sent by it.

I'm still planning to look into it, I've had some personal errands and I'm still traveling. I'll be more available in the coming 10 days.