[uavcan] Add support for the EQUIPMENT_ACTUATOR_ARRAYCOMMAND message (#…

…3016)

conf/airframes/examples/cube_orange.xml

@@ -160,8 +160,16 @@
     <servo name="MOTOR_10" no="2" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_11" no="3" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_12" no="4" min="-8191" neutral="1500" max="8191"/>
+  </servos>
+
+  <!-- CAN BUS 1 command outputs-->
+  <servos driver="Uavcan1Cmd">
     <servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
     <servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
+  </servos>
+
+  <!-- CAN BUS 1 command outputs-->
+  <servos driver="Uavcan2Cmd">
     <servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
     <servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
   </servos>

sw/airborne/modules/actuators/actuators_uavcan.c

@@ -50,6 +50,15 @@ static struct actuators_uavcan_telem_t uavcan1_telem[SERVOS_UAVCAN1_NB];
 int16_t actuators_uavcan2_values[SERVOS_UAVCAN2_NB];
 static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB];
 #endif
+#ifdef SERVOS_UAVCAN1CMD_NB
+int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB];
+#endif
+#ifdef SERVOS_UAVCAN2CMD_NB
+int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB];
+#endif
+
+/* UNUSED value for CMD */
+#define UAVCAN_CMD_UNUSED (MIN_PPRZ-1)
 
 /* uavcan EQUIPMENT_ESC_STATUS message definition */
 #define UAVCAN_EQUIPMENT_ESC_STATUS_ID                     1034
@@ -61,6 +70,16 @@ static struct actuators_uavcan_telem_t uavcan2_telem[SERVOS_UAVCAN2_NB];
 #define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE          (0x217F5C87D7EC951DULL)
 #define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE           ((285 + 7)/8)
 
+/* uavcan EQUIPMENT_ACTUATOR_STATUS message definition */
+#define UAVCAN_EQUIPMENT_ACTUATOR_STATUS_ID                1011
+#define UAVCAN_EQUIPMENT_ACTUATOR_STATUS_SIGNATURE         (0x5E9BBA44FAF1EA04ULL)
+#define UAVCAN_EQUIPMENT_ACTUATOR_STATUS_MAX_SIZE          ((64 + 7)/8)
+
+/* uavcan EQUIPMENT_ACTUATOR_ARRAYCOMMAND message definition */
+#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID          1010
+#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE   (0xD8A7486238EC3AF3ULL)
+#define UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE    ((484 + 7)/8)
+
 /* private variables */
 static bool actuators_uavcan_initialized = false;
 static uavcan_event esc_status_ev;
@@ -185,6 +204,16 @@ void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ESC, actuators_uavcan_send_esc);
 #endif
 
+  // Set default to not set
+#ifdef SERVOS_UAVCAN1CMD_NB
+  for(uint8_t i = 0; i < SERVOS_UAVCAN1CMD_NB; i++)
+    actuators_uavcan1cmd_values[i] = UAVCAN_CMD_UNUSED;
+#endif
+#ifdef SERVOS_UAVCAN2CMD_NB
+  for(uint8_t i = 0; i < SERVOS_UAVCAN2CMD_NB; i++)
+    actuators_uavcan2cmd_values[i] = UAVCAN_CMD_UNUSED;
+#endif
+
   // Set initialization
   actuators_uavcan_initialized = true;
 
@@ -193,7 +222,7 @@ void actuators_uavcan_init(struct uavcan_iface_t *iface __attribute__((unused)))
 }
 
 /**
- * Commit actuator values to the uavcan interface
+ * Commit actuator values to the uavcan interface (EQUIPMENT_ESC_RAWCOMMAND)
  */
 void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
 {
@@ -210,3 +239,37 @@ void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint
   uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID,
                    CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8);
 }
+
+/**
+ * Commit actuator values to the uavcan interface (EQUIPMENT_ACTUATOR_ARRAYCOMMAND)
+ */
+void actuators_uavcan_cmd_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb)
+{
+  uint8_t buffer[UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_MAX_SIZE];
+  uint32_t offset = 0;
+  uint8_t command_type = 0; // 0:UNITLESS, 1:meter or radian, 2:N or Nm, 3:m/s or rad/s
+
+  // Encode the values for each command
+  for (uint8_t i = 0; i < nb; i++) {
+    // Skip unused commands
+    if(values[i] == UAVCAN_CMD_UNUSED || values[i] < MIN_PPRZ || values[i] > MAX_PPRZ)
+      continue;
+
+    // Set the command id
+    canardEncodeScalar(buffer, offset, 8, (void*)&i); // 255
+    offset += 8;
+
+    // Set the command type
+    canardEncodeScalar(buffer, offset, 8, (void*)&command_type); // 255
+    offset += 8;
+
+    // Set the command value
+    uint16_t tmp_float = canardConvertNativeFloatToFloat16((float)values[i] / (float)MAX_PPRZ);
+    canardEncodeScalar(buffer, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+  }
+
+  // Broadcast the raw command message on the interface
+  uavcan_broadcast(iface, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_SIGNATURE, UAVCAN_EQUIPMENT_ACTUATOR_ARRAYCOMMAND_ID,
+                   CANARD_TRANSFER_PRIORITY_HIGH, buffer, (offset + 7) / 8);
+}

sw/airborne/modules/actuators/actuators_uavcan.h

@@ -28,5 +28,6 @@
 /* External functions */
 extern void actuators_uavcan_init(struct uavcan_iface_t *iface);
 extern void actuators_uavcan_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb);
+extern void actuators_uavcan_cmd_commit(struct uavcan_iface_t *iface, int16_t *values, uint8_t nb);
 
 #endif /* ACTUATORS_UAVCAN_H */

sw/airborne/modules/actuators/actuators_uavcan1cmd.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef ACTUATORS_UAVCAN1_CMD_H
+#define ACTUATORS_UAVCAN1_CMD_H
+
+#include "actuators_uavcan.h"
+
+/** Stub file needed per uavcan interface because of generator */
+extern int16_t actuators_uavcan1cmd_values[SERVOS_UAVCAN1CMD_NB];
+
+#if USE_NPS
+#define ActuatorsUavcan1CmdInit() {}
+#define ActuatorUavcan1CmdSet(_i, _v) {}
+#define ActuatorsUavcan1CmdCommit()  {}
+#else
+#define ActuatorsUavcan1CmdInit() actuators_uavcan_init(&uavcan1)
+#define ActuatorUavcan1CmdSet(_i, _v) { actuators_uavcan1cmd_values[_i] = _v; }
+#define ActuatorsUavcan1CmdCommit()  actuators_uavcan_cmd_commit(&uavcan1, actuators_uavcan1cmd_values, SERVOS_UAVCAN1CMD_NB)
+#endif
+
+#endif /* ACTUATORS_UAVCAN1_CMD_H */

sw/airborne/modules/actuators/actuators_uavcan2cmd.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2023 Freek van Tienen <freek.v.tienen@gmail.com>
+ *
+ * This file is part of Paparazzi.
+ *
+ * Paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * Paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+#ifndef ACTUATORS_UAVCAN2_CMD_H
+#define ACTUATORS_UAVCAN2_CMD_H
+
+#include "actuators_uavcan.h"
+
+/** Stub file needed per uavcan interface because of generator */
+extern int16_t actuators_uavcan2cmd_values[SERVOS_UAVCAN2CMD_NB];
+
+#if USE_NPS
+#define ActuatorsUavcan2CmdInit() {}
+#define ActuatorUavcan2CmdSet(_i, _v) {}
+#define ActuatorsUavcan2CmdCommit()  {}
+#else
+#define ActuatorsUavcan2CmdInit() actuators_uavcan_init(&uavcan2)
+#define ActuatorUavcan2CmdSet(_i, _v) { actuators_uavcan2cmd_values[_i] = _v; }
+#define ActuatorsUavcan2CmdCommit()  actuators_uavcan_cmd_commit(&uavcan2, actuators_uavcan2cmd_values, SERVOS_UAVCAN2CMD_NB)
+#endif
+
+#endif /* ACTUATORS_UAVCAN2_CMD_H */