ArduPilot · peterbarker · Jan 13, 2025
diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
@@ -539,6 +539,10 @@ void AP_Periph_FW::update()
     networking_periph.update();
 #endif
 
+#if AP_PERIPH_VOLZ_ENABLED
+    volz_update();
+#endif  // AP_PERIPH_VOLZ_ENABLED
+
 #if (defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) && HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY == 8) || defined(HAL_PERIPH_ENABLE_NOTIFY)
     update_rainbow();
 #endif

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
@@ -46,6 +46,7 @@
 #include <SITL/SITL.h>
 #endif
 #include <AP_AHRS/AP_AHRS.h>
+#include <AP_Servo_Telem/AP_Servo_Telem_config.h>
 
 #ifdef HAL_PERIPH_ENABLE_RELAY
 #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT
@@ -113,6 +114,22 @@
 #undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
 #endif
 
+#ifndef AP_PERIPH_VOLZ_ENABLED
+#define AP_PERIPH_VOLZ_ENABLED 0
+#endif
+
+// support for sending the actuator_status_Volz DroneCAN message
+#ifndef AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+#define AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED AP_PERIPH_VOLZ_ENABLED && AP_SERVO_TELEM_ENABLED
+#endif
+
+#if AP_PERIPH_VOLZ_ENABLED
+#include <AP_Volz_Protocol/AP_Volz_Protocol.h>
+#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+#include <AP_Servo_Telem/AP_Servo_Telem.h>
+#endif  // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+#endif  // AP_PERIPH_VOLZ_ENABLED
+
 #include "Parameters.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -334,7 +351,25 @@ class AP_Periph_FW {
 #if AP_KDECAN_ENABLED
     AP_KDECAN kdecan;
 #endif
-
+
+#if AP_SERVO_TELEM_ENABLED
+    AP_Servo_Telem st;
+#endif
+
+#if AP_PERIPH_VOLZ_ENABLED
+    void volz_init();
+    void volz_update();
+    struct {
+        uint32_t last_servo_telem_update_ms;
+        uint32_t last_actuator_status_send_ms;
+        uint8_t next_servo_to_send;
+    } volz;
+    AP_Volz_Protocol volz_protocol;
+    void volz_update_servo_telem();
+#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+    void send_com_volz_servo_ActuatorStatus(uint8_t id, const AP_Servo_Telem::TelemetryData &telem_data);
+#endif  // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+#endif
 #ifdef HAL_PERIPH_ENABLE_ESC_APD
     ESC_APD_Telem *apd_esc_telem[APD_ESC_INSTANCES];
     void apd_esc_telem_update();

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
@@ -2,6 +2,10 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 #include "AP_Periph.h"
 
+#if AP_PERIPH_VOLZ_ENABLED
+#include <AP_Volz_Protocol/AP_Volz_Protocol.h>
+#endif  // AP_PERIPH_VOLZ_ENABLED
+
 extern const AP_HAL::HAL &hal;
 
 #ifndef HAL_PERIPH_LED_BRIGHT_DEFAULT
@@ -737,6 +741,12 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(imu, "INS", AP_InertialSensor),
 #endif
 
+#if AP_PERIPH_VOLZ_ENABLED
+    // @Group: VOLZ_
+    // @Path: ../libraries/AP_Volz/AP_Volz.cpp
+    GOBJECT(volz_protocol, "VOLZ_",   AP_Volz_Protocol),
+#endif  // AP_PERIPH_VOLZ_ENABLED
+
     AP_VAREND
 };
 

diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
@@ -99,6 +99,8 @@ class Parameters {
         k_param_esc_extended_telem_rate,
         k_param_imu_sample_rate,
         k_param_imu,
+        k_param_volz_protocol,
+        k_param_volz_port,
     };
 
     AP_Int16 format_version;

diff --git a/Tools/AP_Periph/rc_out.cpp b/Tools/AP_Periph/rc_out.cpp
@@ -198,6 +198,7 @@ void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id)
     }
     sim_actuator.last_send_ms = now_ms;
 
+#if !AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
     for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
         if ((sim_actuator.mask & (1U<<i)) == 0) {
             continue;
@@ -220,6 +221,7 @@ void AP_Periph_FW::sim_update_actuator(uint8_t actuator_id)
                          &buffer[0],
                          total_size);
     }
+#endif  // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
 }
 #endif // AP_SIM_ENABLED
 

diff --git a/Tools/AP_Periph/volz.cpp b/Tools/AP_Periph/volz.cpp
@@ -0,0 +1,80 @@
+#include "AP_Periph.h"
+
+#if AP_PERIPH_VOLZ_ENABLED
+
+#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+#include <AP_Servo_Telem/AP_Servo_Telem.h>
+#include <dronecan_msgs.h>
+#endif  // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+
+#include <AP_HAL/HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+void AP_Periph_FW::volz_update()
+{
+    volz_protocol.update();
+
+    volz_update_servo_telem();
+}
+
+void AP_Periph_FW::volz_update_servo_telem()
+{
+    AP_Servo_Telem *servo_telem = AP_Servo_Telem::get_singleton();
+    if (servo_telem == nullptr) {
+        return;
+    }
+    const uint32_t now_ms = AP_HAL::millis();
+    if (now_ms - volz.last_servo_telem_update_ms > 100) {
+        servo_telem->update();
+    }
+    volz.last_servo_telem_update_ms = now_ms;
+
+    // send a servo CAN message every 10ms
+    if (now_ms - volz.last_actuator_status_send_ms < 10) {
+        return;
+    }
+    volz.last_actuator_status_send_ms = now_ms;
+    AP_Servo_Telem::TelemetryData telem_data;
+    bool found = false;
+    for (uint8_t first=volz.next_servo_to_send++; first != volz.next_servo_to_send; volz.next_servo_to_send++) {
+        if (!servo_telem->get_telem(volz.next_servo_to_send, telem_data)) {
+            continue;
+        }
+        if (telem_data.stale(now_ms)) {
+            continue;
+        }
+        found = true;
+        break;
+    }
+    if (!found) {
+        return;
+    }
+
+#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+    send_com_volz_servo_ActuatorStatus(volz.next_servo_to_send, telem_data);
+#endif
+}
+
+#if AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+void AP_Periph_FW::send_com_volz_servo_ActuatorStatus(uint8_t id, const AP_Servo_Telem::TelemetryData &telem_data)
+{
+    com_volz_servo_ActuatorStatus actuator_status {};
+    actuator_status.actuator_id = id;
+    actuator_status.actual_position = radians(telem_data.measured_position);
+    actuator_status.current = MIN(telem_data.current / 0.025, 255);
+    actuator_status.voltage = MIN(telem_data.voltage / 0.2, 255);
+    actuator_status.motor_pwm = 0;  // actually power?
+    actuator_status.motor_temperature = MIN(telem_data.motor_temperature_cdeg + 50, 255);
+
+    uint8_t buffer[COM_VOLZ_SERVO_ACTUATORSTATUS_MAX_SIZE];
+    const uint16_t total_size = com_volz_servo_ActuatorStatus_encode(&actuator_status, buffer, !canfdout());
+    canard_broadcast(COM_VOLZ_SERVO_ACTUATORSTATUS_SIGNATURE,
+                    COM_VOLZ_SERVO_ACTUATORSTATUS_ID,
+                    CANARD_TRANSFER_PRIORITY_LOW,
+                    &buffer[0],
+                    total_size);
+}
+#endif  // AP_PERIPH_VOLZ_SEND_COM_VOLZ_SERVO_ACTUATORSTATUS_ENABLED
+
+#endif  // AP_PERIPH_VOLZ_ENABLED
diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript
@@ -84,6 +84,7 @@ def build(bld):
                      'GCS_MAVLink',
                      'AP_Relay',
                      'AP_MultiHeap',
+                     'AP_Servo_Telem',
                      ]
 
     bld.ap_stlib(

diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py
@@ -980,6 +980,20 @@ def configure_env(self, cfg, env):
             HAL_PERIPH_ENABLE_GPS = 1,
         )
 
+class sitl_periph_volz(sitl_periph):
+    def configure_env(self, cfg, env):
+        cfg.env.AP_PERIPH = 1
+        super(sitl_periph_volz, self).configure_env(cfg, env)
+        env.DEFINES.update(
+            HAL_BUILD_AP_PERIPH = 1,
+            PERIPH_FW = 1,
+            CAN_APP_NODE_NAME = '"org.ardupilot.ap_periph_volz"',
+            APJ_BOARD_ID = 101,
+
+            AP_PERIPH_VOLZ_ENABLED = 1,
+            SIM_VOLZ_ENABLED = 1,
+        )
+
 class sitl_periph_battmon(sitl_periph):
     def configure_env(self, cfg, env):
         cfg.env.AP_PERIPH = 1

diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py
@@ -8,6 +8,7 @@
 import copy
 import math
 import os
+import pathlib
 import signal
 
 from pymavlink import quaternion
@@ -6759,6 +6760,89 @@ def tests1a(self):
         ])
         return ret
 
+    def VolzPeriph(self):
+        '''test Volz support in AP_Periph'''
+
+        self.progress("Building Volz peripheral")
+        volz_builddir = util.reltopdir('build-VolzPeriph')
+        board = 'sitl_periph_volz'
+        util.build_SITL(
+            'bin/AP_Periph',
+            board=board,
+            clean=False,
+            configure=True,
+            debug=True,
+            extra_configure_args=[
+                '--out', volz_builddir,
+            ],
+            extra_defines={
+                "AP_SERVO_TELEM_ENABLED": 1,
+                "HAL_PERIPH_ENABLE_RC_OUT": 1,
+                "HAL_PWM_COUNT": 32,
+                "HAL_PERIPH_SHOW_SERIAL_MANAGER_PARAMS": 1,
+            },
+        )
+
+        self.progress("Building Plane with AP_DRONECAN_VOLZ_FEEDBACK_ENABLED support")
+        volz_plane_builddir = util.reltopdir('build-VolzPlane')
+        util.build_SITL(
+            'bin/arduplane',
+            clean=False,
+            configure=True,
+            debug=True,
+            extra_configure_args=[
+                '--enable-Volz_DroneCAN',
+                '--out', volz_plane_builddir,
+            ],
+            # extra_defines={
+            #     'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED': 1,
+            # },
+        )
+
+        self.progress("Running Volz peripheral")
+        volz_rundir = util.reltopdir('run-VolzPeriph')
+        if not os.path.exists(volz_rundir):
+            os.mkdir(volz_rundir)
+        binary_path = pathlib.Path(volz_builddir, board, 'bin', 'AP_Periph')
+        volz1 = util.start_SITL(
+            binary_path,
+            cwd=volz_rundir,
+            stdout_prefix="volz1",
+            supplementary=True,
+            gdb=self.gdb,
+            valgrind=self.valgrind,
+            customisations=[
+                '-I', '1',
+                '--serial5=sim:volz',
+            ],
+            param_defaults={
+                "VOLZ_MASK": 11,
+                "SIM_VOLZ_MASK": 11,
+                "SIM_VOLZ_ENA": 1,
+                "SERIAL5_PROTOCOL": 14,  # 14 is Volz
+            },
+        )
+        self.expect_list_add(volz1)
+
+        self.set_parameters({
+            "CAN_P1_DRIVER": 1,
+        })
+
+        self.progress("Running Volz Plane")
+
+        binary_path = pathlib.Path(volz_plane_builddir, 'sitl', 'bin', 'arduplane')
+        self.customise_SITL_commandline([], binary=binary_path)
+
+        self.set_parameters({
+            "CAN_D1_UC_SRV_BM": 11,
+            #            "CAN_D1_UC_OPTION": 64,  # himark message
+            "SIM_SPEEDUP": 1,
+        })
+        self.delay_sim_time(100000)
+
+        self.expect_list_remove(volz1)
+        util.pexpect_close(volz1)
+
     def tests1b(self):
         return [
             self.TerrainLoiter,
@@ -6846,6 +6930,7 @@ def tests1b(self):
             self.BadRollChannelDefined,
             self.VolzMission,
             self.Volz,
+            self.VolzPeriph,
         ]
 
     def disabled_tests(self):