UAVCAN servo feedback added #19822

henrykotze · 2022-06-21T06:08:09Z

Describe problem solved by this pull request

There is no feedback implemented for uavcan servo's

Describe your solution

Follow a close implementation of that of uavcan esc's:

Added servo_status and servo_report uorb messages and resembles that of esc_status and esc_report
Using the uavcan::equipment::actuator::Status message in the callback to fill in the feedback info

Test data / coverage

Tested with Hitec MD950 CAN servo. Work as excepted using the Actuator Tab in QGC when assigning the Landing Gear Function to the corresponding Servo

Additional context

The following is also added in the code comments at the relevant places:

The check_servos_status() function is only helpful if you have more than 1 UAVCAN servo connected, since if you have only one, and it gets disconnected, the callback function will not execute, thus not updating the check_servo_status() function
The timeout check in the check_servo_status() function can be too short for a given servo periodic stream rate, and may result in a servo being timeout. (untested)
Make sure your ID number set on your uavcan servo is between 0-8 otherwise it wont be picked up here. (untested)
The above points is also applicable to the uavcan ESC's implementation. (untested)
Also added a similar implementation for the mixerChanged() method as that of UavcanMixingInterfaceESC::mixerChangerd() but not too sure of the implications.

dagar · 2022-06-21T15:01:26Z

Minor style failure in CI (run make check_format locally). https://github.com/PX4/PX4-Autopilot/runs/6986710557?check_suite_focus=true

dagar · 2022-06-21T15:06:52Z

msg/servo_status.msg

+uint64 timestamp					# time since system start (microseconds)
+uint8 CONNECTED_SERVO_MAX = 8				# The number of ESCs supported. Current (Q2/2013) we support 8 ESCs
+
+uint8 SERVO_CONNECTION_TYPE_PPM = 0			# Traditional PPM ESC


I would drop these until we actually need them, many of these connection types will never be real.

Cool, I have dropped these.

dagar · 2022-06-21T15:07:10Z

msg/servo_report.msg

@@ -0,0 +1,8 @@
+uint64 timestamp					# time since system start (microseconds)
+
+float32 servo_position					# feedback position of servo


Are there are units we can supply here?

dagar · 2022-06-21T15:07:23Z

msg/servo_report.msg

+
+float32 servo_position					# feedback position of servo
+float32 servo_speed					# feedback speed of servo
+float32 servo_force					# force experienced by servo


Units?
Set to NAN if force not provided?

Added units in latest commit. servo_force has been updated to servo_acceleration. More on this in the conversation section

dagar · 2022-06-21T15:11:08Z

msg/servo_report.msg

+float32 servo_force					# force experienced by servo
+uint8 servo_id						# id of servo
+uint8 servo_function					# servo output function (one of Servo1...ServoN)
+


Does Hitec populate power rating?

Yes, Hitec provides a power rating, or a load percentage which the servo is experiencing. I have updated as such

dagar · 2022-06-21T15:15:33Z

This looks good and is fine for now if it solves your immediate needs, but longer term I was thinking about changing the interface a bit.

Fundamentally we get status per available actuator asynchronously, so I think ultimately we should be publishing an instance of servo_status or servo_report (or whatever we want to call it) per actuator. This would also remove the hard coded limit (currently 8). It might make sense to specialize this msg for different types of feedback (position available, torque, etc) depending on what's actually available.

Separately I was also going to put together a more generic actuator_status feedback message that the output modules publish and the control allocator consumes. This would be a more generic message where the feedback (if any) is provided in the units (normalized range) used by control_allocator. This is what the system would use to know if all configured actuators are present and later what we could use to properly handle actuator failure (and dynamic control effectiveness updates).

henrykotze · 2022-09-25T08:51:52Z

Hitec provides a configuration app, to set many specifics of the servo such as: units of the feedback, max torque, max speed and more. So its possible that other servo manufacturers will send their feedback in other units resulting in the uorb topic definition to be incorrect.
I have set it here as unitless in the Hitec app, because it makes viewing the log much more easier.

Remarks on power_rating_pct and force in the Dronecan message

uavcan::equipment::actuator::Status::power_rating_pct -> servo_report.load_pct
uavcan::equipment::actuator::Status::force -> servo_report.servo_acceleration

Its not clear for me whether the force variable in the dronecan message is torque or acceleration. Here is a test bench log or here is a screenshot:

If I press hard on the servo horn (between timestamp 40 and 50) we can see the load_pct increasing due to the load I am applying, however servo_acceleration stays close to zero, which tells me uavcan::equipment::actuator::Status::force is not a torque as mentioned in the definition.

When I am sweeping the servo as seen from timestamp 50, the acceleration spikes during servo_speed changes and steadies at zero during constant speed. During this sweeping, we can also see load_pct is a non-zero constant.

However I cannot confirm the acceleration values being logged, and might be coupled by another gain converting it to torque such as:

henrykotze · 2023-01-06T07:38:42Z

Any comments? 😄
If "LGTM" , I will rebase unless any other todo's

- Added same functionality that is seen for ESC's in UAVCAN as for servo's: * Added servo_report & servo_status similiar to that of esc_status and esc_report. * check for online servos * connection types - Tested with Hitec MD950 CAN servo. Work as excepted using the Actuator Tab in QGC. Updated units and feedback received Update format Style Conform to new uorb msg naming scheme

henrykotze · 2023-03-09T13:50:55Z

Rebased on main and quickly tested the UAVCAN servo, and works as expected.

sfuhrer

A couple of ideas to make the message more informative and concise. The servos we're using report

min/max/mid position
current position
torque
voltage
temperature
failure

sfuhrer · 2023-03-10T09:23:43Z

msg/ServoReport.msg

+uint64 timestamp					# time since system start (microseconds)
+
+float32 servo_position					# feedback position of servo. [-1;1]
+float32 servo_speed					# feedback speed of servo [deg/s]


Is it not enough to derive the speed from the position?

yes true, but since the servo provides feedback about its speed, we should at least log it.

sfuhrer · 2023-03-10T09:24:42Z

msg/ServoReport.msg

+
+float32 servo_position					# feedback position of servo. [-1;1]
+float32 servo_speed					# feedback speed of servo [deg/s]
+float32 servo_acceleration				# feedback of acceleration


Just curious, what would you use acceleration feedback for?

We are not using it for something specific currently, but since the servo provides feedback about its acceleration, we should at least log it.

sfuhrer · 2023-03-10T09:25:10Z

msg/ServoReport.msg

+float32 servo_position					# feedback position of servo. [-1;1]
+float32 servo_speed					# feedback speed of servo [deg/s]
+float32 servo_acceleration				# feedback of acceleration
+uint8 load_pct						# Load percentage


Is this equal to Torque?

I dont think so, because if I look at the graphs, during constant speed, load_pct is constant and my expectation would be a first order behavior for the torque.

It could be a constant current/torque for a given speed command, but not too sure.

dagar reviewed Jun 21, 2022

View reviewed changes

dagar added Admin: Enhancement (improvement) 💡 UAVCAN labels Jun 21, 2022

henrykotze force-pushed the pr-uavcan-servo-feedback branch 2 times, most recently from 1603ebc to 83552b3 Compare September 25, 2022 08:15

henrykotze force-pushed the pr-uavcan-servo-feedback branch from 83552b3 to edd8a3b Compare March 9, 2023 13:49

sfuhrer reviewed Mar 10, 2023

View reviewed changes

sfuhrer mentioned this pull request Mar 25, 2025

UAVCAN: Read servo telemetry over CAN bus and publish to uorb #24587

Open

		@@ -0,0 +1,8 @@
		uint64 timestamp # time since system start (microseconds)

		float32 servo_position # feedback position of servo

UAVCAN servo feedback added #19822

Are you sure you want to change the base?

UAVCAN servo feedback added #19822

Conversation

henrykotze commented Jun 21, 2022

Describe problem solved by this pull request

Describe your solution

Test data / coverage

Additional context

Uh oh!

dagar commented Jun 21, 2022

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

henrykotze Sep 25, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

henrykotze Sep 25, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

dagar commented Jun 21, 2022

Uh oh!

henrykotze commented Sep 25, 2022

Remarks on power_rating_pct and force in the Dronecan message

Uh oh!

henrykotze commented Jan 6, 2023

Uh oh!

henrykotze commented Mar 9, 2023

Uh oh!

sfuhrer left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

henrykotze Sep 25, 2022 •

edited

Loading

henrykotze Sep 25, 2022 •

edited

Loading