diff --git a/conf/abi.xml b/conf/abi.xml
index 834808e8fe..57522280bf 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -265,6 +265,15 @@
+
+ Pointer pointing at the actuators_t4_out struct
+ Pointer pointing at the actuators_t4_extra_data_out array
+
+
+
+ Pointer pointing at the actuators_t4_in struct
+ Pointer pointing at the actuators_t4_extra_data_in array
+
diff --git a/conf/airframes/OPENUAS/openuas_atomrc_seal_evo.xml b/conf/airframes/OPENUAS/openuas_atomrc_seal_evo.xml
new file mode 100644
index 0000000000..92b300937f
--- /dev/null
+++ b/conf/airframes/OPENUAS/openuas_atomrc_seal_evo.xml
@@ -0,0 +1,864 @@
+
+
+
+ A good fixedwing example airframe to demonstrate the use and setup of a T4 Actuators Board
+
+ ABOUT:
+ + Model: Eachine AtomRC Seal 1500mm Wingspan EPO FPV RC Airplane
+ + Autopilot: Cube Orange Plus(+) (THe manufacturer names it FMU v2.1)
+ + Actuators: All Servo are Serial Bus Servos connected to a T4 Actuators Board
+
+ + GNSS: Two (2x) uBlox ZED F9P with RTK
+ + MAG: RM3100
+ + ESC: Default 30A with flashed AM32 Opensource Firmware
+ + RCRX: OpenRXSR Receiver (or TBS Crossfire Micro swapped)
+ + AIRSPEED: Sensirion DPS3x
+ + TELEMETRY: - Si10xx Chip based with firmware enabling PPRZ RSSI message
+ - or Herelink
+ - or ESP32C3 Wifi module
+ + CURRENT: A standard Hextronix PowerBrick sensor on the default analog ports
+ + RANGER: SF20 sensor on I2C
+ + MOTOR: Default
+ + PROP: Default 8x4
+
+ NOTES:
+ + Servo pins point to tail of airframe
+ + Engine battery voltage and current values HExtronix powerbrick
+ + Servos and Sonar powered via BEC of ESC
+ + Flightcontroller powered via BEC on powerbrick
+ + Temporary telemetry powered via BEC on powerbrick
+ + Herelink Telemetry powered via additional CC BEC at 7.1V
+ + Uploading of the firmware can be performed via original PX4 or ARDU bootloader...
+ Simple, press upload button in GUI...then USB cable in, wait... voila PPRZdized aircraft
+ + Separate left and right Aileron servo outputs pin for more flexibility
+ + Advised battery for this airframe is a 3300mAh 4S LiPo min. 15C
+ + Modified the motor thrust angle to be more, 2mm washers under bottom of motor x cross
+ + Default indicated CoG
+ + There is an AOA as well as a SSA sensor on a probebar
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/actuators_t4.xml b/conf/modules/actuators_t4.xml
new file mode 100644
index 0000000000..eabbe7793d
--- /dev/null
+++ b/conf/modules/actuators_t4.xml
@@ -0,0 +1,18 @@
+
+
+
+
+ Intermediate Actuators Driver when using a T4 actuators board
+
+
+
+ actuators
+ actuators
+
+
+
+
+
+
diff --git a/conf/modules/actuators_t4_uart.xml b/conf/modules/actuators_t4_uart.xml
new file mode 100644
index 0000000000..b8d5cadc8d
--- /dev/null
+++ b/conf/modules/actuators_t4_uart.xml
@@ -0,0 +1,42 @@
+
+
+
+
+ Actuators Driver using T4 protocol to a T4 actuators board via UART bus.
+
+ If you have a T4 Actuator Board, use this module to set serialbus servos, PWM ESC's, DShot ESC's and PWM servos connected to a T4 Actuators Board.
+ Prominent use-case is that one can use status information that is returned in the autopilot control loop.
+ All the information like: RPM, Angle, Torque, Temperature, Current, usable state information for more precisly contolled flights.
+
+ For the hardware and software used to make your own T4 Actuators board, find all information here: https://github.com/tudelft/t4_actuators_board/
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/aoa_t4.xml b/conf/modules/aoa_t4.xml
new file mode 100644
index 0000000000..e0f83aa438
--- /dev/null
+++ b/conf/modules/aoa_t4.xml
@@ -0,0 +1,75 @@
+
+
+
+
+
+ Angle of Attack (AOA) sensor using angle input from a modiefied hall servo connected to a T4 Actuators Board.
+ For the hardware and software used to make your own T4 Actuators board, find all information here:
+
+ https://github.com/tudelft/t4_actuators_board/
+
+ To make it work, the motor of the servo needs to be removed, only the hall sensors is used for angle feedback.
+ If the MOTOR is NOT REMOVED from the servo there will be to much friction and the sensor will NOT WORK properly.
+
+ Budget wise it is a flexible solution for angle of attack sensor if one uses a T4 Actuators Board already.
+
+ Aditionall a second servo can added to be assigned to fuction as a SideSlip angle (SSA) sensor.
+
+ If SSA is added, it is assumed that both sensors are the same, offset and direction can be set to different values.
+
+ A Servo modified for sensor use which works well is a Feetech STS3032. It is a 360 degree serial bus servo with hall sensor feedback.
+
+ Simply add the "aoa_t4" module to your myname_airframe.xml file to be able to use this solution.
+ Also for initial offset determination, enabeling the SYNC message is very helpfull. Just add the SYNC_SEND_AOA_T4 define.
+
+ For a 3D design of a vane that can be 3D printed and mounted on the servo used, download it here:
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/modules/gps_ublox.xml b/conf/modules/gps_ublox.xml
index 4c73c31fee..dbb08dd93d 100644
--- a/conf/modules/gps_ublox.xml
+++ b/conf/modules/gps_ublox.xml
@@ -50,7 +50,7 @@
-
+
ifdef SECONDARY_GPS
@@ -85,4 +85,3 @@
-
diff --git a/conf/telemetry/t4_actuators_board_debugdata.xml b/conf/telemetry/t4_actuators_board_debugdata.xml
new file mode 100644
index 0000000000..a4a221b5d7
--- /dev/null
+++ b/conf/telemetry/t4_actuators_board_debugdata.xml
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/OPENUAS/openuas_conf.xml b/conf/userconf/OPENUAS/openuas_conf.xml
index a3daf6219e..60dcf92d1a 100644
--- a/conf/userconf/OPENUAS/openuas_conf.xml
+++ b/conf/userconf/OPENUAS/openuas_conf.xml
@@ -1,4 +1,15 @@
+
-
-
@@ -165,33 +163,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -200,7 +171,6 @@
-
@@ -246,7 +216,7 @@
-
+
@@ -258,6 +228,35 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -271,12 +270,42 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c
new file mode 100644
index 0000000000..ae90ed4697
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.c
@@ -0,0 +1,102 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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.
+ */
+/**
+ * @file arch/chibios/modules/actuators/actuators_t4_arch.c
+ * @brief Actuator interface for T4 driver
+ * @author Sunyou Hwang
+ */
+
+#include "modules/actuators/actuators_t4_arch.h"
+#include "modules/actuators/actuators_t4.h"
+
+#include "modules/core/abi.h"
+
+#ifndef ACTUATORS_T4_REFRESH_FREQUENCY
+#define ACTUATORS_T4_REFRESH_FREQUENCY 200;
+#endif
+
+#ifndef ACTUATORS_T4_NUM_MAX_SERVOS
+#define ACTUATORS_T4_NUM_MAX_SERVOS 12
+#endif
+
+#ifndef ACTUATORS_T4_NUM_MAX_ESCS
+#define ACTUATORS_T4_NUM_MAX_ESCS 4
+#endif
+
+/**
+ * Print the configuration variables from the header
+ */
+PRINT_CONFIG_VAR(ACTUATORS_T4_NB)
+
+int32_t actuators_t4_values[ACTUATORS_T4_NB];
+
+struct ActuatorsT4Out actuators_t4_out_local;
+float actuators_t4_extra_data_out_local[255] __attribute__((aligned));
+
+void actuators_t4_arch_init(void) {
+ /* Arm ESCs and servos by default, Note that only armed servo directly provide force needed for e.g. landing gear */
+ //TODO: Add option set or check is disarmed / is armed as a default value per actuator
+ actuators_t4_out_local.esc_arm = (1 << ACTUATORS_T4_NUM_MAX_ESCS) -1;
+ actuators_t4_out_local.servo_arm = (1 << ACTUATORS_T4_NUM_MAX_SERVOS) - 1;
+
+ /* comm_refresh_frequency [Hz] */
+ actuators_t4_extra_data_out_local[0] = ACTUATORS_T4_REFRESH_FREQUENCY;
+}
+
+void actuators_t4_commit(void) {
+ /*
+ For developers wanting to improve here note that:
+ DRIVER_NO: and SERVO_IDX: defined order in AF config
+ actuators_t4_values[driver_no] = actuators[servo_idx].val;
+ get_servo_min_T4(_idx);
+ get_servo_max_T4(_idx);
+ get_servo_idx_T4(_idx_driver); returns servo idx
+ */
+
+ /* Yes, indeed it could well be that we waste two bytes here, feel free to improve
+ Servos connected to pin outputs 1~10 (Serial Bus),
+ Note that connector 0 does not exist on a T4 actuators board,
+ indeed we waste two bytes here, feel free to improve.
+ */
+ actuators_t4_out_local.servo_1_cmd = (int16_t)(actuators_t4_values[1]);
+ actuators_t4_out_local.servo_2_cmd = (int16_t)(actuators_t4_values[2]);
+ actuators_t4_out_local.servo_3_cmd = (int16_t)(actuators_t4_values[3]);
+ actuators_t4_out_local.servo_4_cmd = (int16_t)(actuators_t4_values[4]);
+ actuators_t4_out_local.servo_5_cmd = (int16_t)(actuators_t4_values[5]);
+ actuators_t4_out_local.servo_6_cmd = (int16_t)(actuators_t4_values[6]);
+ actuators_t4_out_local.servo_7_cmd = (int16_t)(actuators_t4_values[7]);
+ actuators_t4_out_local.servo_8_cmd = (int16_t)(actuators_t4_values[8]);
+ actuators_t4_out_local.servo_9_cmd = (int16_t)(actuators_t4_values[9]);
+ actuators_t4_out_local.servo_10_cmd = (int16_t)(actuators_t4_values[10]);
+
+ /* PWM Servos (or ESCs) connected to pin outputs 11~12 (PWM) */
+ actuators_t4_out_local.servo_11_cmd = (int16_t)(actuators_t4_values[11]);
+ actuators_t4_out_local.servo_12_cmd = (int16_t)(actuators_t4_values[12]);
+
+ /* DShot capable ESC's connected to pin outputs 13~16 (DShot) */
+ actuators_t4_out_local.esc_1_dshot_cmd = (int16_t)(actuators_t4_values[13]);
+ actuators_t4_out_local.esc_2_dshot_cmd = (int16_t)(actuators_t4_values[14]);
+ actuators_t4_out_local.esc_3_dshot_cmd = (int16_t)(actuators_t4_values[15]);
+ actuators_t4_out_local.esc_4_dshot_cmd = (int16_t)(actuators_t4_values[16]);
+
+ // Send ABI message
+ AbiSendMsgACTUATORS_T4_OUT(ABI_ACTUATORS_T4_OUT_ID, &actuators_t4_out_local, &actuators_t4_extra_data_out_local[0]);
+}
diff --git a/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h
new file mode 100644
index 0000000000..69ee68e340
--- /dev/null
+++ b/sw/airborne/arch/chibios/modules/actuators/actuators_t4_arch.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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.
+ */
+/**
+ * @file arch/chibios/modules/actuators/actuators_t4_arch.h
+ * @brief Actuator interface for T4 driver
+ */
+#ifndef ACTUATORS_T4_ARCH_H
+#define ACTUATORS_T4_ARCH_H
+
+#include "std.h"
+
+// Servo 1 to 12 plus ESC 1 to 4 is a total of 16
+#ifndef ACTUATORS_T4_NB
+#define ACTUATORS_T4_NB 17 //Not 16 since using 0 as index
+#endif
+
+extern int32_t actuators_t4_values[ACTUATORS_T4_NB];
+
+extern void actuators_t4_commit(void);
+
+#define ActuatorT4Set(_i, _v) { actuators_t4_values[_i] = _v; }
+#define ActuatorsT4Commit actuators_t4_commit
+
+#endif /* ACTUATORS_T4_ARCH_H */
diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c
new file mode 100644
index 0000000000..36cbf163fc
--- /dev/null
+++ b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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.
+ */
+/**
+ * @file arch/sim/modules/actuators/actuators_t4_arch.c
+ * @brief Actuator interface for T4 driver
+ * @author Sunyou Hwang
+ */
+
+#include
+#include "modules/actuators/actuators_t4_arch.h"
+#include "modules/actuators/actuators_t4.h"
+
+#include "modules/core/abi.h"
+
+#ifndef ACTUATORS_T4_REFRESH_FREQUENCY
+#define ACTUATORS_T4_REFRESH_FREQUENCY 200;
+#endif
+
+#ifndef ACTUATORS_T4_NUM_MAX_SERVOS
+#define ACTUATORS_T4_NUM_MAX_SERVOS 12
+#endif
+
+#ifndef ACTUATORS_T4_NUM_MAX_ESCS
+#define ACTUATORS_T4_NUM_MAX_ESCS 4
+#endif
+
+/**
+ * Print the configuration variables from the header
+ */
+PRINT_CONFIG_VAR(ACTUATORS_T4_NB)
+
+int32_t actuators_t4_values[ACTUATORS_T4_NB];
+
+struct ActuatorsT4Out actuators_t4_out_local;
+float actuators_t4_extra_data_out_local[255] __attribute__((aligned));
+
+void actuators_t4_arch_init(void) {
+ /* Arm ESCs and servos by default, Note that only armed servo directly provide force needed for e.g. landing gear */
+ //TODO: Add option set or check is disarmed / is armed as a default value per actuator
+ actuators_t4_out_local.esc_arm = (1 << ACTUATORS_T4_NUM_MAX_ESCS) -1;
+ actuators_t4_out_local.servo_arm = (1 << ACTUATORS_T4_NUM_MAX_SERVOS) - 1;
+
+ /* comm_refresh_frequency [Hz] */
+ actuators_t4_extra_data_out_local[0] = ACTUATORS_T4_REFRESH_FREQUENCY;
+}
+
+void actuators_t4_commit(void) {
+ /*
+ For developers wanting to improve here note that:
+ DRIVER_NO: and SERVO_IDX: defined order in AF config
+ actuators_t4_values[driver_no] = actuators[servo_idx].val;
+ get_servo_min_T4(_idx);
+ get_servo_max_T4(_idx);
+ get_servo_idx_T4(_idx_driver); returns servo idx
+ */
+
+ /* Yes, indeed it could well be that we waste two bytes here, feel free to improve
+ Servos connected to pin outputs 1~10 (Serial Bus),
+ Note that connector 0 does not exist on a T4 actuators board,
+ indeed we waste two bytes here, feel free to improve.
+ */
+ actuators_t4_out_local.servo_1_cmd = (int16_t)(actuators_t4_values[1]);
+ actuators_t4_out_local.servo_2_cmd = (int16_t)(actuators_t4_values[2]);
+ actuators_t4_out_local.servo_3_cmd = (int16_t)(actuators_t4_values[3]);
+ actuators_t4_out_local.servo_4_cmd = (int16_t)(actuators_t4_values[4]);
+ actuators_t4_out_local.servo_5_cmd = (int16_t)(actuators_t4_values[5]);
+ actuators_t4_out_local.servo_6_cmd = (int16_t)(actuators_t4_values[6]);
+ actuators_t4_out_local.servo_7_cmd = (int16_t)(actuators_t4_values[7]);
+ actuators_t4_out_local.servo_8_cmd = (int16_t)(actuators_t4_values[8]);
+ actuators_t4_out_local.servo_9_cmd = (int16_t)(actuators_t4_values[9]);
+ actuators_t4_out_local.servo_10_cmd = (int16_t)(actuators_t4_values[10]);
+
+ /* PWM Servos (or ESCs) connected to pin outputs 11~12 (PWM) */
+ actuators_t4_out_local.servo_11_cmd = (int16_t)(actuators_t4_values[11]);
+ actuators_t4_out_local.servo_12_cmd = (int16_t)(actuators_t4_values[12]);
+
+ /* DShot capable ESC's connected to pin outputs 13~16 (DShot) */
+ actuators_t4_out_local.esc_1_dshot_cmd = (int16_t)(actuators_t4_values[13]);
+ actuators_t4_out_local.esc_2_dshot_cmd = (int16_t)(actuators_t4_values[14]);
+ actuators_t4_out_local.esc_3_dshot_cmd = (int16_t)(actuators_t4_values[15]);
+ actuators_t4_out_local.esc_4_dshot_cmd = (int16_t)(actuators_t4_values[16]);
+
+ // Send ABI message
+ AbiSendMsgACTUATORS_T4_OUT(ABI_ACTUATORS_T4_OUT_ID, &actuators_t4_out_local, &actuators_t4_extra_data_out_local[0]);
+}
diff --git a/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h
new file mode 100644
index 0000000000..8621dd673f
--- /dev/null
+++ b/sw/airborne/arch/sim/modules/actuators/actuators_t4_arch.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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.
+ */
+/**
+ * @file arch/sim/modules/actuators/actuators_t4_arch.h
+ * @brief Actuator interface for T4 driver
+ */
+#ifndef ACTUATORS_T4_ARCH_H
+#define ACTUATORS_T4_ARCH_H
+
+#include "std.h"
+
+// Servo 1 to 12 plus ESC 1 to 4 is a total of 16
+#ifndef ACTUATORS_T4_NB
+#define ACTUATORS_T4_NB 17 //Not 16 since using 0 as index
+#endif
+
+extern int32_t actuators_t4_values[ACTUATORS_T4_NB];
+
+extern void actuators_t4_commit(void);
+
+#define ActuatorT4Set(_i, _v) { actuators_t4_values[_i] = _v; }
+#define ActuatorsT4Commit actuators_t4_commit
+
+#endif /* ACTUATORS_T4_ARCH_H */
diff --git a/sw/airborne/modules/actuators/actuators_t4.h b/sw/airborne/modules/actuators/actuators_t4.h
new file mode 100644
index 0000000000..de24850551
--- /dev/null
+++ b/sw/airborne/modules/actuators/actuators_t4.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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
+ * .
+ */
+/**
+ * @file "modules/actuators/actuators_t4.h"
+ * @author Alessandro Mancinelli, Sunyou Hwang, OpenUAS
+ * @brief Uses a T4 Actuators Board as fly by wire system. This Board can control serial bus servos, ESC's and PWM servos, with as big benefir providing real time telemetry in return into the autopilot state.
+ * Read more on how to create your own T4 Board here: https://github.com/tudelft/t4_actuators_board/
+ */
+
+#ifndef ACTUATORS_T4_H
+#define ACTUATORS_T4_H
+
+#include "modules/actuators/actuators_t4_arch.h"
+#include "actuators_t4_uart.h" //#include "modules/actuators/actuators_t4_uart.h"
+
+/** Arch dependent init file.
+ * implemented in arch files
+ */
+extern void actuators_t4_arch_init(void);
+
+#define ActuatorsT4Init() actuators_t4_arch_init()
+
+#endif /* ACTUATORS_T4_H */
diff --git a/sw/airborne/modules/actuators/actuators_t4_uart.c b/sw/airborne/modules/actuators/actuators_t4_uart.c
new file mode 100644
index 0000000000..6516c3c27c
--- /dev/null
+++ b/sw/airborne/modules/actuators/actuators_t4_uart.c
@@ -0,0 +1,260 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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
+ * .
+ */
+/**
+ * @file "modules/actuators/actuators_t4_uart.c"
+ * @author Alessandro Mancinelli, Sunyou Hwang, OpenUAS
+ * @brief Uses a T4 Actuators Board as fly by wire system. This Board can control serial bus servos, ESC's and PWM servos, with as big benefir providing real time telemetry in return into the autopilot state.
+ * Read more on how to create your own T4 Board here: https://github.com/tudelft/t4_actuators_board/
+ */
+
+#include "pprzlink/pprz_transport.h"
+#include "mcu_periph/uart.h"
+#include "mcu_periph/sys_time.h"
+#include
+#include
+#include "modules/core/abi.h"
+#include "modules/actuators/actuators_t4_uart.h"
+
+/* Variables for outbound packet */
+static abi_event ACTUATORS_T4_OUT;
+uint8_t actuators_t4_out_msg_id;
+struct ActuatorsT4Out actuators_t4_out;
+float actuators_t4_extra_data_out[255]__attribute__((aligned));
+
+/* Variables for outbound packet */
+struct ActuatorsT4In actuators_t4_in;
+float actuators_t4_extra_data_in[255]__attribute__((aligned));
+
+uint16_t actuators_t4_buf_in_cnt = 0;
+uint32_t actuators_t4_missed_packets_in = 0;
+uint32_t actuators_t4_received_packets = 0;
+uint16_t actuators_t4_message_frequency_in = 0;
+float actuators_t4_last_ts = 0;
+
+static uint8_t actuators_t4_msg_buf_in[sizeof(struct ActuatorsT4In)*2]__attribute__((aligned)); ///< The message buffer for the device chosen to be 2* message_size total
+
+
+#if PERIODIC_TELEMETRY
+
+#include "modules/datalink/telemetry.h"
+
+static void actuators_t4_downlink(struct transport_tx *trans, struct link_device *dev) {
+ int16_t esc_1_rpm_telemetry = actuators_t4_in.esc_1_rpm;
+ int16_t esc_2_rpm_telemetry = actuators_t4_in.esc_2_rpm;
+ int16_t esc_3_rpm_telemetry = actuators_t4_in.esc_3_rpm;
+ int16_t esc_4_rpm_telemetry = actuators_t4_in.esc_4_rpm;
+
+ int16_t esc_1_error_code_telemetry = actuators_t4_in.esc_1_error_code;
+ int16_t esc_2_error_code_telemetry = actuators_t4_in.esc_2_error_code;
+ int16_t esc_3_error_code_telemetry = actuators_t4_in.esc_3_error_code;
+ int16_t esc_4_error_code_telemetry = actuators_t4_in.esc_4_error_code;
+
+ int16_t esc_1_current_telemetry = actuators_t4_in.esc_1_current;
+ int16_t esc_2_current_telemetry = actuators_t4_in.esc_2_current;
+ int16_t esc_3_current_telemetry = actuators_t4_in.esc_3_current;
+ int16_t esc_4_current_telemetry = actuators_t4_in.esc_4_current;
+
+ int16_t esc_1_voltage_telemetry = actuators_t4_in.esc_1_voltage;
+ int16_t esc_2_voltage_telemetry = actuators_t4_in.esc_2_voltage;
+ int16_t esc_3_voltage_telemetry = actuators_t4_in.esc_3_voltage;
+ int16_t esc_4_voltage_telemetry = actuators_t4_in.esc_4_voltage;
+
+ // Not used, and if so maybe to uint16_t in Kelvin not Celcius
+ //int16_t esc_1_temperature_telemetry = actuators_t4_in.esc_1_temperature;
+ //int16_t esc_2_temperature_telemetry = actuators_t4_in.esc_2_temperature;
+ //int16_t esc_3_temperature_telemetry = actuators_t4_in.esc_3_temperature;
+ //int16_t esc_4_temperature_telemetry = actuators_t4_in.esc_4_temperature;
+
+ int16_t servo_1_angle_telemetry = actuators_t4_in.servo_1_angle;
+ int16_t servo_2_angle_telemetry = actuators_t4_in.servo_2_angle;
+ int16_t servo_3_angle_telemetry = actuators_t4_in.servo_3_angle;
+ int16_t servo_4_angle_telemetry = actuators_t4_in.servo_4_angle;
+ int16_t servo_5_angle_telemetry = actuators_t4_in.servo_5_angle;
+ int16_t servo_6_angle_telemetry = actuators_t4_in.servo_6_angle;
+ int16_t servo_7_angle_telemetry = actuators_t4_in.servo_7_angle;
+ int16_t servo_8_angle_telemetry = actuators_t4_in.servo_8_angle;
+ int16_t servo_9_angle_telemetry = actuators_t4_in.servo_9_angle;
+ int16_t servo_10_angle_telemetry = actuators_t4_in.servo_10_angle;
+ int16_t servo_11_angle_telemetry = actuators_t4_in.servo_11_angle;
+ int16_t servo_12_angle_telemetry = actuators_t4_in.servo_12_angle;
+
+ int16_t servo_1_load_telemetry = actuators_t4_in.servo_1_load;
+ int16_t servo_2_load_telemetry = actuators_t4_in.servo_2_load;
+ int16_t servo_3_load_telemetry = actuators_t4_in.servo_3_load;
+ int16_t servo_4_load_telemetry = actuators_t4_in.servo_4_load;
+ int16_t servo_5_load_telemetry = actuators_t4_in.servo_5_load;
+ int16_t servo_6_load_telemetry = actuators_t4_in.servo_6_load;
+ int16_t servo_7_load_telemetry = actuators_t4_in.servo_7_load;
+ int16_t servo_8_load_telemetry = actuators_t4_in.servo_8_load;
+ int16_t servo_9_load_telemetry = actuators_t4_in.servo_9_load;
+ int16_t servo_10_load_telemetry = actuators_t4_in.servo_10_load;
+ uint16_t bitmask_servo_health_telemetry = actuators_t4_in.bitmask_servo_health;
+
+ float rolling_msg_in_telemetry = actuators_t4_in.rolling_msg_in;
+ uint8_t rolling_msg_in_id_telemetry = actuators_t4_in.rolling_msg_in_id;
+
+ pprz_msg_send_ACTUATORS_T4_IN(trans, dev, AC_ID,
+ &esc_1_rpm_telemetry, &esc_2_rpm_telemetry, &esc_3_rpm_telemetry, &esc_4_rpm_telemetry,
+ &servo_1_angle_telemetry, &servo_2_angle_telemetry, &servo_3_angle_telemetry, &servo_4_angle_telemetry,
+ &servo_5_angle_telemetry, &servo_6_angle_telemetry, &servo_7_angle_telemetry, &servo_8_angle_telemetry,
+ &servo_9_angle_telemetry, &servo_10_angle_telemetry, &servo_11_angle_telemetry, &servo_12_angle_telemetry,
+ &actuators_t4_missed_packets_in, &actuators_t4_message_frequency_in,
+ &rolling_msg_in_telemetry, &rolling_msg_in_id_telemetry,
+ &esc_1_error_code_telemetry, &esc_2_error_code_telemetry, &esc_3_error_code_telemetry, &esc_4_error_code_telemetry,
+ &servo_1_load_telemetry, &servo_2_load_telemetry, &servo_3_load_telemetry, &servo_4_load_telemetry,
+ &servo_5_load_telemetry, &servo_6_load_telemetry, &servo_7_load_telemetry, &servo_8_load_telemetry,
+ &servo_9_load_telemetry, &servo_10_load_telemetry,
+ &bitmask_servo_health_telemetry,
+ &esc_1_current_telemetry, &esc_2_current_telemetry, &esc_3_current_telemetry, &esc_4_current_telemetry,
+ &esc_1_voltage_telemetry, &esc_2_voltage_telemetry, &esc_3_voltage_telemetry, &esc_4_voltage_telemetry);
+}
+
+ static void actuators_t4_uplink(struct transport_tx *trans, struct link_device *dev) {
+
+ uint8_t esc_arm_telemetry = actuators_t4_out.esc_arm;
+ uint16_t servo_arm_telemetry = actuators_t4_out.servo_arm;
+ int16_t esc_1_dshot_cmd_telemetry = actuators_t4_out.esc_1_dshot_cmd;
+ int16_t esc_2_dshot_cmd_telemetry = actuators_t4_out.esc_2_dshot_cmd;
+ int16_t esc_3_dshot_cmd_telemetry = actuators_t4_out.esc_3_dshot_cmd;
+ int16_t esc_4_dshot_cmd_telemetry = actuators_t4_out.esc_4_dshot_cmd;
+
+ int16_t servo_1_angle_cmd_telemetry = actuators_t4_out.servo_1_cmd;
+ int16_t servo_2_angle_cmd_telemetry = actuators_t4_out.servo_2_cmd;
+ int16_t servo_3_angle_cmd_telemetry = actuators_t4_out.servo_3_cmd;
+ int16_t servo_4_angle_cmd_telemetry = actuators_t4_out.servo_4_cmd;
+ int16_t servo_5_angle_cmd_telemetry = actuators_t4_out.servo_5_cmd;
+ int16_t servo_6_angle_cmd_telemetry = actuators_t4_out.servo_6_cmd;
+ int16_t servo_7_angle_cmd_telemetry = actuators_t4_out.servo_7_cmd;
+ int16_t servo_8_angle_cmd_telemetry = actuators_t4_out.servo_8_cmd;
+ int16_t servo_9_angle_cmd_telemetry = actuators_t4_out.servo_9_cmd;
+ int16_t servo_10_angle_cmd_telemetry = actuators_t4_out.servo_10_cmd;
+ int16_t servo_11_angle_cmd_telemetry = actuators_t4_out.servo_11_cmd;
+ int16_t servo_12_angle_cmd_telemetry = actuators_t4_out.servo_12_cmd;
+
+ float rolling_msg_out_telemetry = actuators_t4_out.rolling_msg_out;
+ uint8_t rolling_msg_out_id_telemetry = actuators_t4_out.rolling_msg_out_id;
+
+ pprz_msg_send_ACTUATORS_T4_OUT(trans, dev, AC_ID,
+ &esc_arm_telemetry, &servo_arm_telemetry,
+ &esc_1_dshot_cmd_telemetry, &esc_2_dshot_cmd_telemetry, &esc_3_dshot_cmd_telemetry, &esc_4_dshot_cmd_telemetry,
+ &servo_1_angle_cmd_telemetry, &servo_2_angle_cmd_telemetry, &servo_3_angle_cmd_telemetry, &servo_4_angle_cmd_telemetry,
+ &servo_5_angle_cmd_telemetry, &servo_6_angle_cmd_telemetry, &servo_7_angle_cmd_telemetry, &servo_8_angle_cmd_telemetry,
+ &servo_9_angle_cmd_telemetry, &servo_10_angle_cmd_telemetry, &servo_11_angle_cmd_telemetry, &servo_12_angle_cmd_telemetry,
+ &rolling_msg_out_telemetry, &rolling_msg_out_id_telemetry);
+
+ }
+
+#endif // PERIODIC_TELEMETRY
+
+static void data_actuators_t4_out(uint8_t sender_id __attribute__((unused)), struct ActuatorsT4Out * actuators_t4_out_ptr, float * actuators_t4_extra_data_out_ptr){
+
+ /* Copying the struct to be transmitted and the extra data in a local variable: */
+ memcpy(&actuators_t4_out,actuators_t4_out_ptr,sizeof(struct ActuatorsT4Out));
+ memcpy(&actuators_t4_extra_data_out,actuators_t4_extra_data_out_ptr, sizeof(actuators_t4_extra_data_out) );
+
+ /* Increase the counter to track the sending messages: */
+ actuators_t4_out.rolling_msg_out = actuators_t4_extra_data_out[actuators_t4_out_msg_id];
+ actuators_t4_out.rolling_msg_out_id = actuators_t4_out_msg_id;
+ actuators_t4_out_msg_id++;
+ if(actuators_t4_out_msg_id == 255){
+ actuators_t4_out_msg_id = 0;
+ }
+
+ /* Send the message over UART to the T4 Actuators Board: */
+ uint8_t *buf_send = (uint8_t *)&actuators_t4_out;
+ //Calculating the checksum
+ uint8_t checksum_out_local = 0;
+ for(uint16_t i = 0; i < sizeof(struct ActuatorsT4Out) - 1; i++){
+ checksum_out_local += buf_send[i];
+ }
+ actuators_t4_out.checksum_out = checksum_out_local;
+
+#ifdef ACTUATORS_T4_SIM
+ /* don't send the data if it is SIM */
+ //TODO: why not, could be useful for HITL
+#else
+ /* Do send the bytes */
+ uart_put_byte(&(ACTUATORS_T4_PORT), 0, START_BYTE_ACTUATORS_T4);
+ for(uint8_t i = 0; i < sizeof(struct ActuatorsT4Out) ; i++){
+ uart_put_byte(&(ACTUATORS_T4_PORT), 0, buf_send[i]);
+ }
+#endif
+}
+
+void actuators_t4_uart_init()
+{
+ actuators_t4_buf_in_cnt = 0;
+ actuators_t4_out_msg_id = 0;
+
+ /* Init ABI bind msg: */
+ AbiBindMsgACTUATORS_T4_OUT(ABI_BROADCAST, &ACTUATORS_T4_OUT, data_actuators_t4_out);
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS_T4_IN, actuators_t4_downlink);
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ACTUATORS_T4_OUT, actuators_t4_uplink);
+#endif
+}
+
+/* Send the received message over ABI so then another module can use this actuator state info in e.g. control modules */
+void actuators_t4_uart_parse_msg_in(void)
+{
+ memcpy(&actuators_t4_in, &actuators_t4_msg_buf_in[1], sizeof(struct ActuatorsT4In)); //Starting from 1 to avoid reading the starting byte
+ /* Assign the rolling message: */
+ actuators_t4_extra_data_in[actuators_t4_in.rolling_msg_in_id] = actuators_t4_in.rolling_msg_in;
+ /* Send msg through ABI: */
+ AbiSendMsgACTUATORS_T4_IN(ABI_ACTUATORS_T4_IN_ID, &actuators_t4_in, &actuators_t4_extra_data_in[0]);
+}
+
+/* Event checking if serial packet are available on the bus */
+void actuators_t4_uart_event()
+{
+ if(fabs(get_sys_time_float() - actuators_t4_last_ts) > 5){ //Reset received packets to zero every 5 second to update the statistics
+ actuators_t4_received_packets = 0;
+ actuators_t4_last_ts = get_sys_time_float();
+ }
+#ifdef ACTUATORS_T4_SIM //TODO: use the SIM, NPS and HITL flags ,but if HIL is used it should be able to send the data to the T4 board
+ /* Don't do anything if it is SIM */
+#else
+ while(uart_char_available(&(ACTUATORS_T4_PORT)) > 0) {
+ uint8_t actuators_t4_byte_in;
+ actuators_t4_byte_in = uart_getch(&(ACTUATORS_T4_PORT));
+ if ((actuators_t4_byte_in == START_BYTE_ACTUATORS_T4) || (actuators_t4_buf_in_cnt > 0)) {
+ actuators_t4_msg_buf_in[actuators_t4_buf_in_cnt] = actuators_t4_byte_in;
+ actuators_t4_buf_in_cnt++;
+ }
+ if (actuators_t4_buf_in_cnt > sizeof(struct ActuatorsT4In) ) {
+ actuators_t4_buf_in_cnt = 0;
+ uint8_t checksum_in_local = 0;
+ for(uint16_t i = 1; i < sizeof(struct ActuatorsT4In) ; i++){
+ checksum_in_local += actuators_t4_msg_buf_in[i];
+ }
+ if(checksum_in_local == actuators_t4_msg_buf_in[sizeof(struct ActuatorsT4In)]){
+ actuators_t4_uart_parse_msg_in();
+ actuators_t4_received_packets++;
+ }
+ else {
+ actuators_t4_missed_packets_in++;
+ }
+ }
+ }
+#endif
+ actuators_t4_message_frequency_in = (uint16_t) actuators_t4_received_packets/(get_sys_time_float() - actuators_t4_last_ts);
+}
+
diff --git a/sw/airborne/modules/actuators/actuators_t4_uart.h b/sw/airborne/modules/actuators/actuators_t4_uart.h
new file mode 100644
index 0000000000..f1091633a7
--- /dev/null
+++ b/sw/airborne/modules/actuators/actuators_t4_uart.h
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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
+ * .
+ */
+/**
+ * @file "modules/actuators/actuators_t4_uart.h"
+ * @author Alessandro Mancinelli, Sunyou Hwang, OpenUAS
+ * @brief Uses a T4 Actuators Board as fly by wire system. This Board can control serial bus servos, ESC's and PWM servos, with as big benefir providing real time telemetry in return into the autopilot state.
+ * Read more on how to create your own T4 Board here: https://github.com/tudelft/t4_actuators_board/
+ *
+ */
+
+#ifndef ACTUATORS_T4_UART_H
+#define ACTUATORS_T4_UART_H
+
+#define START_BYTE_ACTUATORS_T4 0x9A //1st start block identifier byte
+
+#include "std.h"
+#include
+#include
+#include "generated/airframe.h"
+#include "pprzlink/pprz_transport.h"
+
+struct __attribute__((__packed__)) ActuatorsT4In {
+ /* ESCs telemetry & error code */
+
+ /* RPM output from ESC's to flightcontroller */
+ int16_t esc_1_rpm;
+ int16_t esc_2_rpm;
+ int16_t esc_3_rpm;
+ int16_t esc_4_rpm;
+
+ /* Error code from ESC's to flightcontroller */
+ int16_t esc_1_error_code;
+ int16_t esc_2_error_code;
+ int16_t esc_3_error_code;
+ int16_t esc_4_error_code;
+
+ /* Current (mA) output from ESC's to flightcontroller */
+ int16_t esc_1_current;
+ int16_t esc_2_current;
+ int16_t esc_3_current;
+ int16_t esc_4_current;
+
+ /* Voltage (mV) output from ESC's to flightcontroller */
+ int16_t esc_1_voltage;
+ int16_t esc_2_voltage;
+ int16_t esc_3_voltage;
+ int16_t esc_4_voltage;
+
+ /* Temperature (Celcius) not implemented, since it will take away bus bandwidth, feel free to add if really nned it, preferably with low message rate */
+ //int16_t esc_1_temperature; // FIXME Kelvin as SI it is might be better?
+ //int16_t esc_2_temperature;
+ //int16_t esc_3_temperature;
+ //int16_t esc_4_temperature;
+
+ /* SERVOS telemetry where angle is in Degrees times 100 */
+ int16_t servo_1_angle;
+ int16_t servo_2_angle;
+ int16_t servo_3_angle;
+ int16_t servo_4_angle;
+ int16_t servo_5_angle;
+ int16_t servo_6_angle;
+ int16_t servo_7_angle;
+ int16_t servo_8_angle;
+ int16_t servo_9_angle;
+ int16_t servo_10_angle;
+ /* Note that PWM servos nor PWM ESC's give feedback in real life. Servos have estimated angles */
+ int16_t servo_11_angle;
+ int16_t servo_12_angle;
+
+ /* Servo load in UNDEFINED Units */
+ int16_t servo_1_load;
+ int16_t servo_2_load;
+ int16_t servo_3_load;
+ int16_t servo_4_load;
+ int16_t servo_5_load;
+ int16_t servo_6_load;
+ int16_t servo_7_load;
+ int16_t servo_8_load;
+ int16_t servo_9_load;
+ int16_t servo_10_load;
+ /* Note that PWM servos or ESC (11 and 12) give no real feedback on load */
+ //int16_t servo_11_load;
+ //int16_t servo_12_load;
+
+ uint16_t bitmask_servo_health; //Bitmask of servo health status
+
+ /* Rolling message in */
+ float rolling_msg_in;
+ uint8_t rolling_msg_in_id;
+
+ /* CHECKSUM for the data packages */
+ uint8_t checksum_in;
+};
+
+struct __attribute__((__packed__)) ActuatorsT4Out {
+ /* Arming Command */
+ uint8_t esc_arm; //Arm ESC boolean in bitfield
+ uint16_t servo_arm; //Arm servo boolean in bitfield
+
+ /* ESC cmd values 0 - 1999 */
+ int16_t esc_1_dshot_cmd;
+ int16_t esc_2_dshot_cmd;
+ int16_t esc_3_dshot_cmd;
+ int16_t esc_4_dshot_cmd;
+
+ /* Servo cmd in Degrees * 100 */
+ int16_t servo_1_cmd;
+ int16_t servo_2_cmd;
+ int16_t servo_3_cmd;
+ int16_t servo_4_cmd;
+ int16_t servo_5_cmd;
+ int16_t servo_6_cmd;
+ int16_t servo_7_cmd;
+ int16_t servo_8_cmd;
+ int16_t servo_9_cmd;
+ int16_t servo_10_cmd;
+ int16_t servo_11_cmd;
+ int16_t servo_12_cmd;
+
+ /* Rolling message out */
+ float rolling_msg_out;
+ uint8_t rolling_msg_out_id;
+ /* CHECKSUM */
+ uint8_t checksum_out;
+};
+
+extern void actuators_t4_uart_init(void);
+extern void actuators_t4_uart_parse_msg_in(void);
+extern void actuators_t4_uart_event(void);
+
+#endif /* ACTUATORS_T4_UART_H */
+
diff --git a/sw/airborne/modules/core/abi_common.h b/sw/airborne/modules/core/abi_common.h
index 5a2d9dcd4e..b5a5cadd42 100644
--- a/sw/airborne/modules/core/abi_common.h
+++ b/sw/airborne/modules/core/abi_common.h
@@ -22,7 +22,7 @@
/**
* @file modules/core/abi_common.h
*
- * Common tools for ABI middelware.
+ * Common tools for ABI middleware.
*/
#ifndef ABI_COMMON_H
@@ -35,6 +35,7 @@
#include "modules/gps/gps.h"
#include "modules/radio_control/radio_control.h"
#include "modules/actuators/actuators.h"
+#include "modules/actuators/actuators_t4_uart.h"
/* Include here headers with structure definition you may want to use with ABI
* Ex: '#include "modules/gps/gps.h"' in order to use the GpsState structure
*/
diff --git a/sw/airborne/modules/core/abi_sender_ids.h b/sw/airborne/modules/core/abi_sender_ids.h
index d128d7e082..d5d772079f 100644
--- a/sw/airborne/modules/core/abi_sender_ids.h
+++ b/sw/airborne/modules/core/abi_sender_ids.h
@@ -120,7 +120,7 @@
#endif
/*
- * IDs of Incidence angles (message 24)
+ * IDs of Incidence angles (message 25)
*/
#ifndef AOA_ADC_ID
#define AOA_ADC_ID 1
@@ -130,6 +130,10 @@
#define AOA_PWM_ID 2
#endif
+#ifndef AOA_T4_ID
+#define AOA_T4_ID 3
+#endif
+
#ifndef INCIDENCE_NPS_ID
#define INCIDENCE_NPS_ID 20
#endif
diff --git a/sw/airborne/modules/sensors/aoa_t4.c b/sw/airborne/modules/sensors/aoa_t4.c
new file mode 100644
index 0000000000..d5339c29d4
--- /dev/null
+++ b/sw/airborne/modules/sensors/aoa_t4.c
@@ -0,0 +1,384 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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.
+ *
+ */
+
+/**
+ * @file modules/sensors/aoa_t4.c
+ * @brief Angle of Attack sensor using T4 Actuators Board and a modified serial bus servo added wind vane. The SBS must be of hall effect type to minimize friction.
+ * @author: Sunyou Hwang, OpenUAS
+ */
+
+#include "modules/sensors/aoa_t4.h"
+#include "generated/airframe.h"
+#include "modules/core/abi.h"
+#include "state.h"
+#include "std.h"
+#include "mcu_periph/uart.h"
+#include "pprzlink/messages.h"
+#include "filters/low_pass_filter.h"
+// #include "modules/sensors/actuators_t4_uart.h"
+
+/// Default offset value (assuming 0 AOA is in the middle of the range)
+#ifndef AOA_ANGLE_OFFSET
+#define AOA_ANGLE_OFFSET M_PI
+#endif
+
+/// Small offset value in radians to get the sensor perfectly lined out
+#ifndef AOA_T4_OFFSET
+#define AOA_T4_OFFSET 0.0f
+#endif
+
+/// Use lowpass filter for AOA
+#ifndef AOA_T4_USE_FILTER
+#define AOA_T4_USE_FILTER FALSE
+#endif
+
+/// Default filter value for reasonable minimum filtering
+/** Time constant for second order Butterworth low pass filter
+ * Default of 0.15 should give cut-off freq of 1/(2*pi*tau) ~= 1Hz
+ * (0.01->15.9 / 0.05->3.2 / 0.1->1.59 / 0.2->0.79 / 0.3->0.53)
+ */
+#ifndef AOA_T4_FILTER
+#define AOA_T4_FILTER 0.15f
+#endif
+
+/// To set reverse direction on readings use true
+#if AOA_T4_REVERSE
+#define AOA_T4_SIGN -1
+#else
+#define AOA_T4_SIGN 1
+#endif
+
+// Enable telemetry report
+#ifndef AOA_T4_SYNC_SEND
+#define AOA_T4_SYNC_SEND FALSE
+#endif
+
+/// Servo ID to use for the modified servo as AOA sensor
+#ifndef AOA_T4_SERVO_ID
+#error "No AOA_T4_SERVO_ID defined, however it must be defined to be able to use the aoa_t4 module"
+#endif
+
+#ifdef AOA_T4_USE_FILTER
+Butterworth2LowPass aoa_t4_lowpass_filter;
+#ifndef AOA_T4_FILTER_SAMPLING_TIME
+#define AOA_T4_FILTER_SAMPLING_TIME 0.0005 // 200Hz
+#endif
+#endif
+
+struct Aoa_T4 aoa_t4;
+
+/// Enable A1 A2 to Theta compensation for AOA sensor angle
+#ifndef AOA_T4_USE_COMPENSATION
+#define AOA_T4_USE_COMPENSATION FALSE
+#endif
+
+// linear fn; aoa += A*theta + B;
+// aoa += A1*theta+B1 when theta >= 0
+// aoa += A2*theta+B2 when theta < 0
+#ifndef AOA_T4_COMP_A1
+#define AOA_T4_COMP_A1 0.0f
+#endif
+
+#ifndef AOA_T4_COMP_B1
+#define AOA_T4_COMP_B1 0.0f
+#endif
+
+#ifndef AOA_T4_COMP_A2
+#define AOA_T4_COMP_A2 -0.5457f // TODO: Explain the magic number...
+#endif
+
+#ifndef AOA_T4_COMP_B2
+#define AOA_T4_COMP_B2 0.0f
+#endif
+
+/* SSA */
+
+/// Default offset value (assuming 0 SSA is in the middle of the range)
+#ifndef SSA_ANGLE_OFFSET
+#define SSA_ANGLE_OFFSET M_PI
+#endif
+
+/// Small offset value in radians to get the sensor perfectly lined out
+#ifndef SSA_T4_OFFSET
+#define SSA_T4_OFFSET 0.0f
+#endif
+
+/// Use lowpass filter for SSA
+#ifndef SSA_T4_USE_FILTER
+#define SSA_T4_USE_FILTER FALSE
+#endif
+
+#ifndef SSA_T4_FILTER
+#define SSA_T4_FILTER 0.20f
+#endif
+
+#if SSA_T4_REVERSE
+#define SSA_T4_SIGN -1
+#else
+#define SSA_T4_SIGN 1
+#endif
+
+#ifdef SSA_T4_USE_FILTER
+Butterworth2LowPass ssa_t4_lowpass_filter;
+//Use same AOA_T4_FILTER_SAMPLING_TIME
+#endif
+
+struct Aoa_T4 ssa_t4;
+
+struct ActuatorsT4In actuators_t4_in_local;
+
+static abi_event ACTUATORS_T4_IN;
+
+struct FloatEulers eulers_t4;
+float aoa_t4_a1 = AOA_T4_COMP_A1;
+float aoa_t4_b1 = AOA_T4_COMP_B1;
+float aoa_t4_a2 = AOA_T4_COMP_A2;
+float aoa_t4_b2 = AOA_T4_COMP_B2;
+
+enum Aoa_Type aoa_send_type;
+
+#if PERIODIC_TELEMETRY
+#include "modules/datalink/telemetry.h"
+
+static void send_aoa(struct transport_tx *trans, struct link_device *dev)
+{
+ // FIXME: Add a "sensor_id" uint8_t to AOA message to nicer send sideslip more important, be able to combine all AOA sensors like AOA_adc, AOA_pwm etc sensors
+ switch (aoa_send_type) {
+ case SEND_TYPE_SIDESLIP:
+ pprz_msg_send_AOA(trans, dev, AC_ID, &ssa_t4.raw, &ssa_t4.angle);
+ break;
+ case SEND_TYPE_AOA:
+ default:
+ pprz_msg_send_AOA(trans, dev, AC_ID, &aoa_t4.raw, &aoa_t4.angle);
+ break;
+ }
+}
+
+#endif
+
+/* The incoming angle from T4 is Degrees x 100 from -18000 to 18000 need to be re-mapped from 0 upto 36000
+ else it will not fit the raw filed with uint32 type as in default AOA raw message */
+uint32_t convert_angle_x100_to_raw(int16_t angle)
+{
+ int16_t input_min = -18000; // The default value in T4 Actuators Board code
+ int16_t input_max = 18000; // Also max default in T4 Actuators Board code
+ // Conversion range
+ uint32_t output_min = 0;
+ uint32_t output_max = abs(input_min) + input_max;
+ // Perform the mapping
+ uint32_t result = (uint32_t)((((int32_t)angle - input_min) * (uint64_t)(output_max - output_min)) / (input_max - input_min) + output_min);
+ return result;
+}
+
+static void actuators_t4_abi_in(uint8_t sender_id __attribute__((unused)), struct ActuatorsT4In *actuators_t4_in_ptr, float *actuators_t4_extra_data_in_ptr __attribute__((unused)))
+{
+ memcpy(&actuators_t4_in_local, actuators_t4_in_ptr, sizeof(struct ActuatorsT4In));
+
+ int16_t angle = 0;
+
+ switch (AOA_T4_SERVO_ID)
+ {
+ /*
+ Note that the T4 Actuator Board has a max of 10 serial bus servos that can give real feedback,
+ the PWM devices 11 and 12 do NOT GIVE real angle FEEDBACK
+ */
+
+ case 1:
+ angle = actuators_t4_in_ptr->servo_1_angle;
+ break;
+ case 2:
+ angle = actuators_t4_in_ptr->servo_2_angle;
+ break;
+ case 3:
+ angle = actuators_t4_in_ptr->servo_3_angle;
+ break;
+ case 4:
+ angle = actuators_t4_in_ptr->servo_4_angle;
+ break;
+ case 5:
+ angle = actuators_t4_in_ptr->servo_5_angle;
+ break;
+ case 6:
+ angle = actuators_t4_in_ptr->servo_6_angle;
+ break;
+ case 7:
+ angle = actuators_t4_in_ptr->servo_7_angle;
+ break;
+ case 8:
+ angle = actuators_t4_in_ptr->servo_8_angle;
+ break;
+ case 9:
+ angle = actuators_t4_in_ptr->servo_9_angle;
+ break;
+ case 10:
+ angle = actuators_t4_in_ptr->servo_10_angle;
+ break;
+ default:
+ break;
+ }
+
+ aoa_t4.raw = convert_angle_x100_to_raw(angle); // To adhere to default message type of uint32_t so from 0 to 36000
+
+#ifdef SSA_T4_SERVO_ID
+
+ angle = 0;
+
+ switch (SSA_T4_SERVO_ID)
+ {
+ case 1:
+ angle = actuators_t4_in_ptr->servo_1_angle;
+ break;
+ case 2:
+ angle = actuators_t4_in_ptr->servo_2_angle;
+ break;
+ case 3:
+ angle = actuators_t4_in_ptr->servo_3_angle;
+ break;
+ case 4:
+ angle = actuators_t4_in_ptr->servo_4_angle;
+ break;
+ case 5:
+ angle = actuators_t4_in_ptr->servo_5_angle;
+ break;
+ case 6:
+ angle = actuators_t4_in_ptr->servo_6_angle;
+ break;
+ case 7:
+ angle = actuators_t4_in_ptr->servo_7_angle;
+ break;
+ case 8:
+ angle = actuators_t4_in_ptr->servo_8_angle;
+ break;
+ case 9:
+ angle = actuators_t4_in_ptr->servo_9_angle;
+ break;
+ case 10:
+ angle = actuators_t4_in_ptr->servo_10_angle;
+ break;
+ default:
+ break;
+ }
+
+ ssa_t4.raw = convert_angle_x100_to_raw(angle);
+
+#endif //SSA_T4_SERVO_ID
+
+}
+
+void aoa_t4_init_filters(void)
+{
+ init_butterworth_2_low_pass(&aoa_t4_lowpass_filter, AOA_T4_FILTER,
+ AOA_T4_FILTER_SAMPLING_TIME, 0);
+}
+
+void ssa_t4_init_filters(void)
+{
+ init_butterworth_2_low_pass(&ssa_t4_lowpass_filter, SSA_T4_FILTER,
+ AOA_T4_FILTER_SAMPLING_TIME, 0);
+}
+
+void aoa_t4_init(void)
+{
+ aoa_t4.raw = 0;
+ aoa_t4.angle = 0.0f;
+ aoa_t4.offset = AOA_T4_OFFSET;
+ aoa_t4.filter = AOA_T4_FILTER;
+
+ ssa_t4.raw = 0;
+ ssa_t4.angle = 0.0f;
+ ssa_t4.offset = SSA_T4_OFFSET;
+ ssa_t4.filter = SSA_T4_FILTER;
+
+ aoa_send_type = SEND_TYPE_AOA; //Per default send AOA
+
+#if AOA_T4_USE_FILTER
+ aoa_t4_init_filters();
+#endif
+
+#if SSA_T4_USE_FILTER
+ ssa_t4_init_filters();
+#endif
+
+ // IN indicates INcoming values from the T4 Actuator Board
+ AbiBindMsgACTUATORS_T4_IN(ABI_BROADCAST, &ACTUATORS_T4_IN, actuators_t4_abi_in);
+
+#if PERIODIC_TELEMETRY
+ register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_AOA, send_aoa);
+#endif
+}
+
+void aoa_t4_update(void)
+{
+#if USE_AOA || USE_SIDESLIP
+ uint8_t flag = 0;
+#endif
+
+ /* Convert raw value of degrees x 100 (0 to 36000) to real degrees and to radians and add offsets */
+ float a_rad;
+ a_rad = AOA_T4_SIGN * ((float)(aoa_t4.raw / 100.0f) * (M_PI / 180.0f)) + aoa_t4.offset + AOA_ANGLE_OFFSET;
+ if (a_rad > M_PI) a_rad -= 2 * M_PI; // Adjust range to be from -PI to PI
+ aoa_t4.angle = a_rad;
+
+#if AOA_T4_USE_COMPENSATION
+ float_eulers_of_quat_zxy(&eulers_t4, stateGetNedToBodyQuat_f());
+ // First order fn
+ if (eulers_t4.theta >= 0)
+ {
+ aoa_t4.angle += aoa_t4_a1 * eulers_t4.theta + aoa_t4_b1;
+ }
+ else
+ {
+ aoa_t4.angle += aoa_t4_a2 * eulers_t4.theta + aoa_t4_b2;
+ }
+#endif
+
+#if AOA_T4_USE_FILTER
+ aoa_t4.angle = update_butterworth_2_low_pass(&aoa_t4_lowpass_filter, aoa_t4.angle);
+#endif
+
+ a_rad = SSA_T4_SIGN * ((float)(ssa_t4.raw / 100.0f) * (M_PI / 180.0f)) + ssa_t4.offset + SSA_ANGLE_OFFSET;
+ if (a_rad > M_PI) a_rad -= 2 * M_PI;
+ ssa_t4.angle = a_rad;
+
+#if SSA_T4_USE_FILTER
+ ssa_t4.angle = update_butterworth_2_low_pass(&aoa_t4_lowpass_filter, ssa_t4.angle);
+#endif
+
+ // Do set values in the state for AOA
+#if USE_AOA
+ SetBit(flag, 0); // Bit 1 is for AOA
+#endif
+
+// Do set values in the state for sideslip
+#ifdef USE_SIDESLIP
+ SetBit(flag, 1); // Bit 2 is for SSA
+#endif
+
+#if USE_AOA || USE_SIDESLIP
+ AbiSendMsgINCIDENCE(AOA_PWM_ID, flag, aoa_t4.angle, ssa_t4.angle);
+#endif
+
+#if AOA_T4_SYNC_SEND
+ RunOnceEvery(10, send_aoa(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
+#endif
+
+}
diff --git a/sw/airborne/modules/sensors/aoa_t4.h b/sw/airborne/modules/sensors/aoa_t4.h
new file mode 100644
index 0000000000..4a647b5cbc
--- /dev/null
+++ b/sw/airborne/modules/sensors/aoa_t4.h
@@ -0,0 +1,68 @@
+/*
+ * Copyright (C) 2024 The Paparazzi Team
+ *
+ * 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.
+ *
+ */
+
+/**
+ * @file modules/sensors/aoa_t4.h
+ * @author Sunyou Hwang, OpenUAS
+ * @brief Angle of Attack and optionally Sideslip Angle sensor using T4 Actuators Board and a modified serial bus servo
+ * The servo as AOA sensor must be of hall effect type and have motor and gears removed to minimize friction and this solution to work.
+ *
+ * Read more on how to create your own T4 Board here: https://github.com/tudelft/t4_actuators_board/
+ *
+ * SENSOR, example : Feetech 3032 Serial Bus Servo
+ */
+
+#ifndef AOA_T4_H
+#define AOA_T4_H
+
+#include "std.h"
+
+struct Aoa_T4 {
+ uint32_t raw; ///< Measurement in degrees times 100 from the sensor before applying direction and offset and unit scale conversion
+ float angle; ///< Angle of attack in radians after applying direction and offset
+ float offset; ///< Angle of attack offset in radians
+ float filter; ///< Filter level for sensor output, where 0.0 is no filtering and 0.95 for extreme filtering, were 1.0 would output an (useless) constant value.
+};
+
+extern struct Aoa_T4 aoa_t4; // Stores values of angle of attack sensor
+/* To allow for separate filter options an own SSA structure is available */
+extern struct Aoa_T4 ssa_t4; // Stores values of sideslip angle sensor
+
+/** Selection of sensor type to be send over telemetry for debugging or online logging
+ */
+enum Aoa_Type {
+ SEND_TYPE_AOA,
+ SEND_TYPE_SIDESLIP
+};
+extern enum Aoa_Type aoa_send_type;
+
+/* Compensation Sensor angle to theta angle */
+extern float aoa_t4_a1;
+extern float aoa_t4_b1;
+extern float aoa_t4_a2;
+extern float aoa_t4_b2;
+
+void aoa_t4_init(void);
+void aoa_t4_update(void);
+void aoa_t4_init_filters(void);
+
+#endif /* AOA_T4_H */