Actuator Faulhaber (#3283)

* Actuator Faulhaber

code style

activate

fix test

* Update conf/modules/actuators_faulhaber.xml

* try to fix

* Update conf/modules/actuators_faulhaber.xml

* 16-bit code

conf/airframes/tudelft/rot_wing_25kg.xml

@@ -23,6 +23,7 @@
             <define name="RADIO_FMODE"              value="RADIO_AUX2"/>
             <define name="RADIO_FBW_MODE"           value="RADIO_AUX3"/>
             <define name="RADIO_CONTROL_THRUST_X"   value="RADIO_AUX4"/>
+            <define name="RADIO_PARACHUTE"          value="RADIO_AUX6"/>
 
             <!-- EKF2 configure inputs -->
             <define name="INS_EKF2_GYRO_ID"         value="IMU_PIXHAWK2_ID"/>
@@ -41,8 +42,6 @@
 
             <!-- Log in high speed (Remove for outdoor flights) -->
             <!-- <define name="IMU_LOG_HIGHSPEED"    value="TRUE"/> -->
-
-            <define name="SERVO_ROTATION_MECH_IDX" value="0"/> <!-- !!!!!!!!!!!!!!!!!!!!!!!!!!!! FIXMEEE !!!!!!!!!!!!!!!!!!!!!!!!!! -->
         </target>
 
         <target name="nps" board="pc">
@@ -106,6 +105,8 @@
             <configure value="TRUE" name="UAVCAN_USE_CAN1"/>
             <configure value="TRUE" name="UAVCAN_USE_CAN2"/>
         </module>
+        <!-- Rotation mechanism actuator on UART -->
+        <module name="actuators" type="faulhaber"/>
 
         <!-- Control -->
         <module name="stabilization" type="indi">
@@ -179,6 +180,10 @@
         <servo no="6"  name="BSERVO_RUDDER"  min="-3250" neutral="0" max="3250"/>
     </servos>
 
+    <servos driver="Faulhaber">
+        <servo no="0"  name="ROTATION_MECH"     min="29" neutral="1707" max="3385"/>
+    </servos>
+
     <commands>
         <axis name="ROLL"       failsafe_value="0"/>
         <axis name="PITCH"      failsafe_value="0"/>
@@ -221,6 +226,9 @@
         <set servo="BAIL_LEFT"          value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
         <set servo="BFLAP_LEFT"         value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
         <set servo="BPARACHUTE"         value="RadioControlValues(RADIO_AUX6)"/-->
+
+        <!-- Rotation mechanism -->
+        <set servo="ROTATION_MECH"      value="rotwing_state_skewing.servo_pprz_cmd"/>
     </command_laws>
 
 

conf/modules/actuators_faulhaber.xml

@@ -0,0 +1,45 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="actuators_faulhaber" dir="actuators" task="actuators">
+  <doc>
+    <description>
+      Actuators Driver for the Faulhaber controller
+    </description>
+    <configure name="FAULHABER_DEV" value="UARTX" description="UART port (default UART4)"/>
+  </doc>
+  <settings>
+    <dl_settings>
+      <dl_settings NAME="Faulhaber">
+        <dl_setting var="faulhaber.mode" min="0" step="1" max="4" shortname="mode" module="modules/actuators/actuators_faulhaber" handler="SetMode"/>
+        <dl_setting var="faulhaber.setpoint_position" min="0" step="1000" max="3600000"/>
+        <dl_setting var="faulhaber.real_position" min="0" step="1000" max="3300000"/>
+      </dl_settings>
+    </dl_settings>
+  </settings>
+
+  <dep>
+    <depends>uart</depends>
+    <provides></provides>
+  </dep>
+  <header>
+    <file name="actuators_faulhaber.h"/>
+  </header>
+  <init fun="actuators_faulhaber_init()"/>
+  <periodic fun="actuators_faulhaber_periodic()" freq="50"/>
+  <event fun="actuators_faulhaber_event()" />
+  <makefile target="!sim">
+    <configure name="FAULHABER_DEV" default="UART4" case="upper|lower"/>
+    <define name="FAULHABER_DEV" value="$(FAULHABER_DEV_LOWER)"/>
+    <define name="USE_$(FAULHABER_DEV_UPPER)"/>
+    <define name="$(FAULHABER_DEV_UPPER)_BAUD" value="B115200"/>
+    <file name="actuators_faulhaber.c"/>
+    <test>
+        <define name="USE_UART4"/>
+        <define name="PERIODIC_FREQUENCY" value="500"/>
+        <define name="FAULHABER_DEV" value="uart4"/>
+        <define name="get_servo_min_FAULHABER" value=""/>
+        <define name="get_servo_max_FAULHABER" value=""/>
+        <define name="get_servo_idx_FAULHABER" value=""/>
+    </test>
+  </makefile>
+</module>

sw/airborne/modules/actuators/actuators_faulhaber.c

@@ -0,0 +1,342 @@
+/*
+ * Copyright (C) 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/actuators/actuators_faulhaber.c"
+ * @author Freek van Tienen
+ * Serial Connection module between ap and a faulhaber motor controller
+ */
+
+#include "modules/actuators/actuators_faulhaber.h"
+#include "mcu_periph/uart.h"
+#include "mcu_periph/sys_time.h"
+#include "modules/datalink/downlink.h"
+#include "modules/core/abi.h"
+#include <stdio.h>
+#include <string.h>
+
+static struct uart_periph *faulhaber_dev = &(FAULHABER_DEV);
+
+struct faulhaber_parser_t {
+  uint8_t state;
+  uint8_t node_nb;
+  uint8_t cmd_code;
+  uint8_t data_length;
+  uint8_t data_idx;
+  uint8_t data[64];
+
+  uint8_t calc_crc8;
+};
+static struct faulhaber_parser_t faulhaber_p;
+struct faulhaber_t faulhaber;
+
+#define Polynom 0xD5
+uint8_t faulhaber_crc8(uint8_t u8Byte, uint8_t u8CRC)
+{
+  uint8_t i;
+  u8CRC = u8CRC ^ u8Byte;
+  for (i = 0; i < 8; i++) {
+    if (u8CRC & 0x01) {
+      u8CRC = (u8CRC >> 1) ^ Polynom;
+    } else {
+      u8CRC >>= 1;
+    }
+  }
+  return u8CRC;
+}
+
+static void faulhaber_send_command(struct uart_periph *dev, uint8_t cmd_code, uint8_t *data, uint8_t data_length)
+{
+  uart_put_byte(dev, 0, 'S'); // Character (S) as Start of Frame
+  uart_put_byte(dev, 0, data_length + 4); // Telegram length without SOF/EOF (packet length)
+  uint8_t crc8 = faulhaber_crc8(data_length + 4, 0xFF); // Start CRC with 0xFF
+  uart_put_byte(dev, 0, 0x01); // Node number of the slave (0 = Broadcast)
+  crc8 = faulhaber_crc8(0x01, crc8);
+  uart_put_byte(dev, 0, cmd_code); // See Tab. 2
+  crc8 = faulhaber_crc8(cmd_code, crc8);
+  for (uint8_t i = 0; i < data_length; i++) { // Data area (length = packet length – 4)
+    uart_put_byte(dev, 0, data[i]);
+    crc8 = faulhaber_crc8(data[i], crc8);
+  }
+  uart_put_byte(dev, 0, crc8); // CRC8 with polynomial 0xD5 over byte 2–N
+  uart_put_byte(dev, 0, 'E'); // Character (E) as End of Frame
+  uart_send_message(dev, 0);
+}
+
+#define UINIT           0
+#define GOT_SOF         1
+#define GOT_LENGTH      2
+#define GOT_NODE_NB     3
+#define GET_DATA        4
+#define GET_CRC         5
+#define GET_EOF         6
+#define GOT_FULL_PACKET 7
+
+static void faulhaber_parser(struct faulhaber_parser_t *p, uint8_t c)
+{
+  switch (p->state) {
+    case UINIT:
+      if (c == 'S') {
+        p->state = GOT_SOF;
+        p->calc_crc8 = 0xFF;
+      }
+      break;
+    case GOT_SOF:
+      if (c - 4 < 0) {
+        p->state = UINIT;
+      } else {
+        p->data_length = c - 4;
+        p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8);
+        p->state = GOT_LENGTH;
+      }
+      break;
+    case GOT_LENGTH:
+      p->node_nb = c;
+      p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8);
+      p->state = GOT_NODE_NB;
+      break;
+    case GOT_NODE_NB:
+      p->cmd_code = c;
+      p->data_idx = 0;
+      p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8);
+      p->state = GET_DATA;
+      break;
+    case GET_DATA:
+      p->data[p->data_idx++] = c;
+      p->calc_crc8 = faulhaber_crc8(c, p->calc_crc8);
+      if (p->data_idx >= p->data_length) {
+        p->state = GET_CRC;
+      }
+      break;
+    case GET_CRC:
+      if (p->calc_crc8 == c) {
+        p->state = GET_EOF;
+      } else {
+        p->state = UINIT;
+      }
+      break;
+    case GET_EOF:
+      if (c == 'E') {
+        p->state = GOT_FULL_PACKET;
+      } else {
+        p->state = UINIT;
+      }
+      break;
+    default:
+      p->state = UINIT;
+      break;
+  }
+}
+
+// Controlword 0x6040.00
+// <Statusword 0x6041.00
+// 0x6060 (Mode of Operation) (6: HOME, 1: PP)
+// <0x6061.00(Mode of Operation feedback)
+// 0x607A.00 (Target Position)
+// <0x607A.00 (Actual Position)
+// Start positioning via the rising edge in bit 4 of the controlword. Also set the optional bits here.
+
+void actuators_faulhaber_init(void)
+{
+  faulhaber_p.state = UINIT;
+
+  faulhaber.mode = FH_MODE_INIT;
+  faulhaber.state = 0;
+  faulhaber.setpoint_position = 0;
+  faulhaber.target_position = 0;
+}
+
+
+void actuators_faulhaber_periodic(void)
+{
+  switch (faulhaber.mode) {
+
+    /* HOME MODE */
+    case FH_MODE_HOME:
+      switch (faulhaber.state) {
+        case 0: {
+          // Set the homing mode
+          uint8_t data[] = { 0x60, 0x60, 0x00, 0x06 }; // Set 0x6060.00 to 0x06:  Homing mode
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          faulhaber.state++;
+          break;
+        }
+        case 1: {
+          // Set the homing method
+          uint8_t data[] = { 0x98, 0x60, 0x00, 0x11 }; // Set 0x6098.00 to 0x11:  Homing method to 17
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          faulhaber.state++;
+          break;
+        }
+        case 2: {
+          // Enable operation
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          break;
+        }
+        case 3: {
+          // Start moving
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x1F, 0x00}; // Set 0x6040.00 to 0x001F: Start moving
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          break;
+        }
+        default:
+          // Goto position mode
+          faulhaber.mode = FH_MODE_INIT;
+          faulhaber.state = 0;
+          break;
+      }
+      break;
+
+    /* POSITION MODE */
+    case FH_MODE_POSITION:
+      switch (faulhaber.state) {
+        case 0: {
+          uint8_t data[] = { 0x60, 0x60, 0x00, 0x01 }; // Set 0x6060.00 to 0x01:  Position mode
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 4);
+          faulhaber.state++;
+          break;
+        }
+        case 1: {
+          // Set the target position
+          faulhaber.target_position = faulhaber.setpoint_position;
+          uint8_t data[] = { 0x7A, 0x60, 0x00, (faulhaber.target_position & 0xFF), ((faulhaber.target_position >> 8) & 0xFF), ((faulhaber.target_position >> 16) & 0xFF), ((faulhaber.target_position >> 24) & 0xFF) }; // Set 0x607A.00 to 0x00000000:  Target position 0
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 7);
+          faulhaber.state++;
+          break;
+        }
+        case 2: {
+          // Enable operation
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Enable operation
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          break;
+        }
+        case 3: {
+          // Start moving
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x3F, 0x00}; // Set 0x6040.00 to 0x003F: Start moving
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          break;
+        }
+        default:
+          faulhaber.mode = FH_MODE_IDLE;
+          faulhaber.state = 0;
+          break;
+
+      }
+      break;
+
+    /* FH_MODE_IDLE */
+    case FH_MODE_IDLE: {
+      // Move to the new position
+      if (faulhaber.target_position != faulhaber.setpoint_position) {
+        faulhaber.mode = FH_MODE_POSITION;
+        faulhaber.state = 0;
+        break;
+      }
+
+      // Request the position
+      uint8_t data[] = { 0x64, 0x60, 0x00}; // Get 0x6064.00: Get the actual position
+      faulhaber_send_command(faulhaber_dev, 0x01, data, 3);
+      faulhaber.state = 0;
+      break;
+    }
+
+    case FH_MODE_ENABLE:
+      switch (faulhaber.state) {
+        case 0: {
+          // Disable drive
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x80, 0x00}; // Set 0x6040.00 to 0x0007: Disable drive
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          //break;
+        }
+        case 1: {
+          // Disable drive
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x06, 0x00}; // Set 0x6040.00 to 0x0006: Disable drive
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          //break;
+        }
+        case 2: {
+          // Enable drive
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x07, 0x00}; // Set 0x6040.00 to 0x0007: Enable drive
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          //break;
+        }
+        case 3: {
+          // Execute
+          uint8_t data[] = { 0x40, 0x60, 0x00, 0x0F, 0x00}; // Set 0x6040.00 to 0x000F: Execute
+          faulhaber_send_command(faulhaber_dev, 0x02, data, 5);
+          faulhaber.state++;
+          //break;
+        }
+        default:
+          faulhaber.mode = FH_MODE_INIT;
+          faulhaber.state = 0;
+          break;
+      }
+      break;
+
+    /* Do nothing */
+    default:
+      break;
+  }
+
+}
+
+static void faulhaber_parse_msg(struct faulhaber_parser_t *p)
+{
+  // Process position message
+  if (p->cmd_code == 0x01 && p->data[0] == 0x64 && p->data[1] == 0x60 && p->data[2] == 0x00) {
+    faulhaber.real_position = p->data[3] | (p->data[4] << 8) | (p->data[5] << 16) | (p->data[6] << 24);
+
+    struct act_feedback_t feedback = {0};
+    feedback.position = (faulhaber.real_position - get_servo_min_FAULHABER(0)) / (double)(get_servo_max_FAULHABER(
+                          0) - get_servo_min_FAULHABER(0)) * M_PI_2; // In radians
+    feedback.set.position = true;
+    feedback.idx = get_servo_idx_FAULHABER(0);
+
+    // Send ABI message
+    AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_FAULHABER_ID, &feedback, 1);
+  }
+}
+
+
+void actuators_faulhaber_event(void)
+{
+  while (uart_char_available(faulhaber_dev)) {
+    faulhaber_parser(&faulhaber_p, uart_getch(faulhaber_dev));
+    if (faulhaber_p.state == GOT_FULL_PACKET) {
+      faulhaber_parse_msg(&faulhaber_p);
+      faulhaber_p.state = UINIT;
+    }
+  }
+}
+
+void actuators_faulhaber_SetMode(uint8_t mode)
+{
+  faulhaber.mode = mode;
+  faulhaber.state = 0;
+}

sw/airborne/modules/actuators/actuators_faulhaber.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 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, see
+ * <http://www.gnu.org/licenses/>.
+ */
+
+/**
+ * @file "modules/actuators/actuators_faulhaber.h"
+ * @author Freek van Tienen
+ * Serial Connection module between ap and a faulhaber motor controller
+ */
+
+#ifndef ACTUATORS_FAULHABER_H
+#define ACTUATORS_FAULHABER_H
+
+#include "std.h"
+
+enum faulhaber_modes_t {
+  FH_MODE_INIT,
+  FH_MODE_IDLE,
+  FH_MODE_HOME,
+  FH_MODE_POSITION,
+  FH_MODE_ENABLE
+};
+
+struct faulhaber_t {
+  enum faulhaber_modes_t mode;
+  uint8_t state;
+
+  int32_t setpoint_position;
+  int32_t target_position;
+  int32_t real_position;
+};
+extern struct faulhaber_t faulhaber;
+
+extern void actuators_faulhaber_init(void);
+extern void actuators_faulhaber_periodic(void);
+extern void actuators_faulhaber_event(void);
+extern void actuators_faulhaber_SetMode(uint8_t mode);
+
+
+#if USE_NPS
+#define ActuatorsFaulhaberInit() {}
+#define ActuatorFaulhaberSet(_i, _v) {}
+#define ActuatorsFaulhaberCommit()  {}
+#else
+#define ActuatorsFaulhaberInit() actuators_faulhaber_init()
+#define ActuatorFaulhaberSet(_i, _v) { faulhaber.setpoint_position = ((get_servo_max_FAULHABER(0)-_v) + get_servo_min_FAULHABER(0))*1000.0f; }
+#define ActuatorsFaulhaberCommit()  {}
+#endif
+
+#endif /* ACTUATORS_FAULHABER_H */

sw/airborne/modules/core/abi_sender_ids.h

@@ -503,6 +503,10 @@
 #define ACT_FEEDBACK_RPM_SENSOR_ID 4
 #endif
 
+#ifndef ACT_FEEDBACK_FAULHABER_ID
+#define ACT_FEEDBACK_FAULHABER_ID 5
+#endif
+
 /*
  * IDs of THRUST increment calculation (message 16)
  */