Add AP_Servo_Telem #28651

IamPete1 · 2024-11-16T13:56:04Z

This adds a servo telem library. Logging only at this stage. There are some benefits, DroneCAN servo logging can be rate limited, this is currently not possible as each instance is logged when it arrives, so it is rate limited individual rather than as a block of each instance.

In the longer term scripting bindings will be added to get and provide this data. That will allow servo health checking and failsafes.

IamPete1 · 2024-11-16T13:56:40Z

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -343,7 +346,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
    Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
    Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};

-#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
+#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
+    Canard::ObjCallback<AP_DroneCAN, com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo};


himark servo was just broken, there was a function but the callback was never registered.

this must have been a regression, it worked when it went in

In the change of DroneCAN library I think, I fixed the same thing for the volz message, #27924 we should probably check all the rest....

I have checked the rest, we are OK no others missing.

IamPete1 · 2024-11-16T13:57:50Z

libraries/AP_PiccoloCAN/AP_PiccoloCAN.cpp

+                    .command_position = servo.commandedPosition(),
+                    .measured_position = servo.position(),


Strictly these are not the correct units, should be degrees. I'm not familiar enough with piccolo CAN to know if we can do a conversion.

IamPete1 · 2024-11-16T13:58:15Z

libraries/AP_PiccoloCAN/AP_PiccoloCAN.h

@@ -131,7 +131,6 @@ class AP_PiccoloCAN : public AP_CANDriver, public AP_ESC_Telem_Backend
    AP_Int16 _ecu_id;       //!< ECU Node ID
    AP_Int16 _ecu_hz;       //!< ECU update rate (Hz)

-    HAL_Semaphore _telem_sem;


This was only ever called in one place, so its not needed.

IamPete1 · 2024-11-16T13:59:13Z

libraries/AP_Volz_Protocol/AP_Volz_Protocol.cpp

-                telem.data[i].primary_current,
-                telem.data[i].secondary_current,
-                telem.data[i].primary_voltage,
-                telem.data[i].secondary_voltage,


We do loose logging of the secondary current and voltage, they could be added as extra fields in the future.

tridge · 2024-11-18T21:21:18Z

@IamPete1 I have added simulation of DroneCAN actuator status and fixed some bugs in the PR here:
https://github.com/tridge/ardupilot/commits/servo_telem/

libraries/AP_DroneCAN/AP_DroneCAN.cpp

tridge · 2024-11-18T21:24:50Z

libraries/AP_DroneCAN/AP_DroneCAN.h

@@ -343,7 +346,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
    Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
    Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};

-#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
+#if AP_DRONECAN_HIMARK_SERVO_SUPPORT && AP_SERVO_TELEM_ENABLED
+    Canard::ObjCallback<AP_DroneCAN, com_himark_servo_ServoInfo> himark_servo_ServoInfo_cb{this, &AP_DroneCAN::handle_himark_servoinfo};


this must have been a regression, it worked when it went in

libraries/AP_Servo_Telem/AP_Servo_Telem.cpp

IamPete1 · 2024-11-21T21:39:32Z

@IamPete1 I have added simulation of DroneCAN actuator status and fixed some bugs in the PR here: https://github.com/tridge/ardupilot/commits/servo_telem/

Thanks, I have pulled in your changes.

…` lib

…lem` lib

allows for testing of DroneCAN servo logging in SITL

and fixed typo in logging

andyp1per · 2024-11-25T14:19:51Z

libraries/AP_Logger/README.md

@@ -68,6 +68,7 @@ Please keep the names consistent with Tools/autotest/param_metadata/param.py:33
 | 's' | "s" | seconds|
 | 'q' | "rpm" | revolutions per minute|  Not an SI unit, but sometimes more intuitive than Hertz|
 | 'r' | "rad" | radians|
+| 't' | "N.m" | Newton meters | torque |


This should be "N-m" or "N m"

Yeah, I agree, it's just matched the existing ones.

andyp1per

Very nice!

IamPete1 · 2024-11-30T15:34:05Z

Some test results:

tridge · 2024-12-02T20:38:12Z

@IamPete1 nice work, thanks!

Ryanf55 · 2024-12-03T18:46:01Z

Unfortunately, this broke the DDS build.

libraries/AP_DDS/generated/ardupilot_msgs/msg/GlobalPosition.h:47:15: error: expected identifier before numeric constant
   47 | #define FORCE 512
      |               ^~~
../../libraries/AP_Servo_Telem/AP_Servo_Telem.h:45:13: note: in expansion of macro ‘FORCE’
   45 |             FORCE              = 1 << 2,
      |             ^~~~~

I'm not happy about the generator using global defines, but it's written for C... Is an easy fix to rename the enum in this class?

IamPete1 commented Nov 16, 2024

View reviewed changes

tridge requested changes Nov 18, 2024

View reviewed changes

IamPete1 force-pushed the servo_telem branch from bf25f51 to f6a94ff Compare November 21, 2024 21:38

IamPete1 requested a review from tridge November 21, 2024 21:41

IamPete1 and others added 12 commits November 23, 2024 12:12

AP_Logger: add new unit t for torque in N.m

2157993

AP_DroneCAN: send incomming servo telem data to new AP_Servo_Telem lib

65b8963

AP_PiccoloCAN: send incomming servo telem data to new `AP_Servo_Telem…

365f9c8

…` lib

AP_Volz_Protocol: send incomming servo telem data to new `AP_Servo_Te…

57c0d3f

…lem` lib

AP_Logger: remove Write_ServoStatus and CSRV definition

c6ffd4b

add AP_Servo_Telem

ac4d5ac

AP_Vechicle: add AP_Servo_Telem and call at 50Hz

44c25e9

Tools: ardupilotwaf: add AP_Servo_Telem to common vehicle libraries

dfe52d7

Tools: scripts: build options and extract features: add AP_Servo_Telem

09222cc

AP_Periph: added simulation of DroneCAN servo status

648688b

allows for testing of DroneCAN servo logging in SITL

AP_DroneCAN: actuator status is measured, not commanded

e2ab42d

AP_ServoTelem: added active mask

e8c5252

and fixed typo in logging

IamPete1 force-pushed the servo_telem branch from f6a94ff to e8c5252 Compare November 23, 2024 11:12

andyp1per reviewed Nov 25, 2024

View reviewed changes

andyp1per approved these changes Nov 25, 2024

View reviewed changes

IamPete1 added the DevCallTopic label Nov 30, 2024

tridge approved these changes Dec 2, 2024

View reviewed changes

tridge merged commit 445c03c into ArduPilot:master Dec 2, 2024
99 checks passed

IamPete1 removed the DevCallTopic label Dec 2, 2024

Ryanf55 mentioned this pull request Dec 3, 2024

AP_ExternalControl: Only enable external control when DDS is enabled #28713

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add AP_Servo_Telem #28651

Add AP_Servo_Telem #28651

IamPete1 commented Nov 16, 2024

IamPete1 Nov 16, 2024

tridge Nov 18, 2024

IamPete1 Nov 21, 2024

IamPete1 Nov 23, 2024 •

edited

Loading

IamPete1 Nov 16, 2024

IamPete1 Nov 16, 2024

IamPete1 Nov 16, 2024

tridge commented Nov 18, 2024

tridge Nov 18, 2024

IamPete1 commented Nov 21, 2024

andyp1per Nov 25, 2024

IamPete1 Nov 25, 2024

andyp1per left a comment

IamPete1 commented Nov 30, 2024 •

edited

Loading

tridge commented Dec 2, 2024

Ryanf55 commented Dec 3, 2024

		.command_position = servo.commandedPosition(),
		.measured_position = servo.position(),

Add AP_Servo_Telem #28651

Add AP_Servo_Telem #28651

Conversation

IamPete1 commented Nov 16, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IamPete1 Nov 23, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tridge commented Nov 18, 2024

Choose a reason for hiding this comment

IamPete1 commented Nov 21, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

andyp1per left a comment

Choose a reason for hiding this comment

IamPete1 commented Nov 30, 2024 • edited Loading

tridge commented Dec 2, 2024

Ryanf55 commented Dec 3, 2024

IamPete1 Nov 23, 2024 •

edited

Loading

IamPete1 commented Nov 30, 2024 •

edited

Loading