diff --git a/conf/modules/actuators_sts3032.xml b/conf/modules/actuators_sts3032.xml
new file mode 100644
index 0000000000..2557e0441d
--- /dev/null
+++ b/conf/modules/actuators_sts3032.xml
@@ -0,0 +1,54 @@
+
+
+
+
+ Feetech sts3032 servo.
+ Their range is 0->4095. Configure min, neutral and max accordingly.
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ uart,actuators
+ actuators
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/airborne/modules/actuators/actuators_sts3032.c b/sw/airborne/modules/actuators/actuators_sts3032.c
new file mode 100644
index 0000000000..8743dc880b
--- /dev/null
+++ b/sw/airborne/modules/actuators/actuators_sts3032.c
@@ -0,0 +1,339 @@
+/*
+ * Copyright (C) 2023 Flo&Fab
+ *
+ * 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/servo_sts3032.c"
+ * @author Flo&Fab
+ * feetech sts3032 servo
+ */
+
+#include "modules/actuators/actuators_sts3032.h"
+#include "modules/datalink/telemetry.h"
+#include "string.h"
+#include "mcu_periph/uart.h"
+#include "mcu_periph/sys_time.h"
+#include "mcu_periph/sys_time_arch.h"
+#include
+#include "peripherals/sts3032_regs.h"
+
+#ifndef STS3032_IDS
+#error "STS3032_IDS must be defined"
+#endif
+
+#ifndef STS3032_DEBUG
+#define STS3032_DEBUG FALSE
+#endif
+
+#ifndef STS3032_RESPONSE_LEVEL
+#define STS3032_RESPONSE_LEVEL 0
+#endif
+
+struct sts3032 sts;
+uint8_t cbuf[50 * SERVOS_STS3032_NB]; // buffer large enough to queue a command to each servo
+
+#ifndef STS3032_DELAY_MSG
+#define STS3032_DELAY_MSG 1000
+#endif
+
+#ifndef STS3032_DELAY_MSG_MIN
+#define STS3032_DELAY_MSG_MIN 0
+#endif
+
+#define BUF_MAX_LENGHT 15
+uint8_t buf_rx_param[255];
+
+
+
+/****** settings globals *********/
+int sts3032_enabled = 1;
+int sts3032_current_id = 1;
+int sts3032_future_id = 1;
+int sts3032_lock_eprom = 1;
+int sts3032_move = 1024;
+int sts3032_response_level = 0;
+/**********************************/
+
+
+static void sts3032_event(struct sts3032 *sts);
+static void write_buf(struct sts3032 *sts, uint8_t id, uint8_t *data, uint8_t len, uint8_t fun, uint8_t reply);
+static void handle_reply(struct sts3032 *sts, uint8_t id, uint8_t state, uint8_t *buf_param, uint8_t length);
+
+void sts3032_write_pos(struct sts3032 *sts, uint8_t id, int16_t position);
+void sts3032_event(struct sts3032 *sts);
+void sts3032_read_mem(struct sts3032 *sts, uint8_t id, uint8_t addr, uint8_t len);
+void sts3032_init(struct sts3032 *sts, struct uart_periph *periph, uint8_t *cbuf, size_t cbuf_len);
+void sts3032_read_pos(struct sts3032 *sts, uint8_t id);
+void sts3032_enable_torque(struct sts3032 *sts, uint8_t id, uint8_t enable);
+void sts3032_lock_eeprom(struct sts3032 *sts, uint8_t id, uint8_t lock);
+void sts3032_set_response_level(struct sts3032 *sts, uint8_t id, uint8_t level);
+void sts3032_set_id(struct sts3032 *sts, uint8_t id, uint8_t new_id);
+
+
+void actuators_sts3032_init(void)
+{
+ sts.periph = &(STS3032_DEV);
+ sts.rx_state = STS3032_RX_IDLE;
+ sts.nb_bytes_expected = 1;
+ sts.nb_failed_checksum = 0;
+ circular_buffer_init(&sts.msg_buf, cbuf, sizeof(cbuf));
+
+ static const uint8_t tmp_ids[SERVOS_STS3032_NB] = STS3032_IDS;
+ memcpy(sts.ids, tmp_ids, sizeof(tmp_ids));
+}
+
+void actuators_sts3032_event(void)
+{
+ sts3032_event(&sts);
+}
+
+void actuators_sts3032_periodic(void)
+{
+
+ for (int i = 0; i < SERVOS_STS3032_NB; i++) {
+ sts3032_read_pos(&sts, sts.ids[i]);
+ }
+
+#if STS3032_DEBUG
+ float data[SERVOS_STS3032_NB + 1];
+ data[0] = sts.nb_failed_checksum;
+ for (int i = 0; i < SERVOS_STS3032_NB; i++) {
+ data[i + 1] = sts.pos[i];
+ }
+ DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, SERVOS_STS3032_NB + 1, data);
+#endif
+
+}
+
+
+
+static void sts3032_event(struct sts3032 *sts)
+{
+
+ uint32_t now = get_sys_time_usec();
+ uint32_t dt = now - sts->time_last_msg;
+ if ((!sts->wait_reply && dt > STS3032_DELAY_MSG_MIN) || dt > STS3032_DELAY_MSG) {
+ uint8_t buf[BUF_MAX_LENGHT];
+ int size = circular_buffer_get(&sts->msg_buf, buf, BUF_MAX_LENGHT);
+ if (size > 0) {
+ // first byte of the buffer indicate whether or not a replay is expected
+ // the rest is the message itself
+ uart_put_buffer(sts->periph, 0, buf + 1, size - 1);
+ sts->wait_reply = buf[0];
+ sts->time_last_msg = now;
+ sts->echo = true;
+ }
+ }
+
+ while (uart_char_available(sts->periph) >= sts->nb_bytes_expected) {
+ switch (sts->rx_state) {
+ case STS3032_RX_IDLE:
+ if (uart_char_available(sts->periph) > 0) {
+ sts->buf_header[1] = sts->buf_header[0];
+ sts->buf_header[0] = uart_getch(sts->periph);
+ if (sts->buf_header[0] == 0xFF && sts->buf_header[1] == 0xFF) {
+ sts->rx_state = STS3032_RX_HEAD_OK;
+ sts->nb_bytes_expected = 2;
+ sts->buf_header[0] = 0;
+ sts->buf_header[1] = 0;
+ }
+ }
+ break;
+ case STS3032_RX_HEAD_OK:
+ if (uart_char_available(sts->periph) >= sts->nb_bytes_expected) {
+ sts->rx_id = uart_getch(sts->periph);
+ sts->nb_bytes_expected = uart_getch(sts->periph); //length
+ sts->rx_checksum = sts->rx_id + sts->nb_bytes_expected;
+ sts->rx_state = STS3032_RX_GOT_LENGTH;
+ }
+ break;
+ case STS3032_RX_GOT_LENGTH:
+ if (uart_char_available(sts->periph) >= sts->nb_bytes_expected) {
+ uint8_t current_state = uart_getch(sts->periph);
+ sts->rx_checksum += current_state;
+ for (int i = 0; i < sts->nb_bytes_expected - 2; i++) {
+ buf_rx_param[i] = uart_getch(sts->periph);
+ sts->rx_checksum += buf_rx_param[i];
+ }
+ uint8_t checksum = uart_getch(sts->periph);
+ sts->rx_checksum = ~sts->rx_checksum;
+ if (sts->rx_checksum == checksum) {
+ if (!sts->echo) {
+ handle_reply(sts, sts->rx_id, current_state, buf_rx_param, sts->nb_bytes_expected - 2);
+ }
+ } else {
+ sts->nb_failed_checksum++;
+ }
+ sts->echo = false;
+ sts->rx_state = STS3032_RX_IDLE;
+ sts->nb_bytes_expected = 1;
+ }
+ break;
+ default:
+ break;
+ }
+ }
+}
+
+static void handle_reply(struct sts3032 *sts, uint8_t id, uint8_t state, uint8_t *buf_param, uint8_t length)
+{
+ uint8_t index = id_idx(sts, id);
+ if (index == 255) {
+ return;
+ }
+ sts->states[index] = state;
+ uint8_t offset = 0;
+ while (length > offset) {
+
+ switch (sts->read_addr + offset) {
+ case SMS_STS_PRESENT_POSITION_L:
+ if (offset + 2 < length) {
+ return;
+ }
+ sts->pos[index] = buf_param[offset] | (buf_param[offset + 1] << 8);
+ offset += 2;
+ break;
+
+ default:
+ offset++;
+ break;
+ }
+ }
+}
+
+
+uint8_t id_idx(struct sts3032 *sts, uint8_t id)
+{
+ for (int i = 0; i < SERVOS_STS3032_NB; i++) {
+ if (sts->ids[i] == id) {
+ return i;
+ }
+ }
+ return 255;
+}
+
+
+void sts3032_lock_eeprom(struct sts3032 *sts, uint8_t id, uint8_t lock)
+{
+ // lock = 0: unlock eeprom
+ // lock = 1: lock eeprom
+ uint8_t buf[2];
+ buf[0] = SMS_STS_LOCK;
+ buf[1] = lock;
+ write_buf(sts, id, buf, 2, INST_WRITE, STS3032_RESPONSE_LEVEL);
+}
+
+void sts3032_set_response_level(struct sts3032 *sts, uint8_t id, uint8_t level)
+{
+ // response level: 0: reply only on read and ping
+ // 1: always repond
+ uint8_t buf[2];
+ buf[0] = SMS_STS_RESPONSE_LEVEL;
+ buf[1] = level;
+ write_buf(sts, id, buf, 2, INST_WRITE, STS3032_RESPONSE_LEVEL);
+}
+
+void sts3032_set_id(struct sts3032 *sts, uint8_t id, uint8_t new_id)
+{
+ uint8_t buf[2];
+ buf[0] = SMS_STS_ID;
+ buf[1] = new_id;
+ write_buf(sts, id, buf, 2, INST_WRITE, STS3032_RESPONSE_LEVEL);
+}
+
+void sts3032_write_pos(struct sts3032 *sts, uint8_t id, int16_t position)
+{
+ if (position < 0) {
+ position = -position;
+ position |= (1 << 15);
+ }
+ uint8_t buf[3];
+ buf[0] = SMS_STS_GOAL_POSITION_L;
+ buf[1] = position & 0xff;
+ buf[2] = position >> 8;
+ write_buf(sts, id, buf, 3, INST_WRITE, STS3032_RESPONSE_LEVEL);
+}
+
+void sts3032_enable_torque(struct sts3032 *sts, uint8_t id, uint8_t enable)
+{
+ uint8_t buf[2];
+ buf[0] = SMS_STS_TORQUE_ENABLE;
+ buf[1] = enable;
+ write_buf(sts, id, buf, 2, INST_WRITE, STS3032_RESPONSE_LEVEL);
+}
+
+void sts3032_read_pos(struct sts3032 *sts, uint8_t id)
+{
+ sts3032_read_mem(sts, id, SMS_STS_PRESENT_POSITION_L, 2);
+}
+
+void sts3032_read_mem(struct sts3032 *sts, uint8_t id, uint8_t addr, uint8_t len)
+{
+ uint8_t buf[2] = {addr, len};
+ write_buf(sts, id, buf, 2, INST_READ, 1);
+ sts->read_addr = addr;
+}
+
+static void write_buf(struct sts3032 *sts, uint8_t id, uint8_t *data, uint8_t len, uint8_t fun, uint8_t reply)
+{
+ uint8_t buf[BUF_MAX_LENGHT];
+ uint8_t checksum = id + fun + len + 2;
+
+ buf[0] = reply; // wait for a reply to this message?
+
+ buf[1] = 0xff;
+ buf[2] = 0xff;
+ buf[3] = id;
+ buf[4] = 2 + len;
+ buf[5] = fun;
+ memcpy(&buf[6], data, len);
+ for (int i = 0; i < len; i++) {
+ checksum += data[i];
+ }
+ buf[len + 6] = ~checksum;
+
+ circular_buffer_put(&sts->msg_buf, buf, len + 7);
+}
+
+
+/******************** settings handlers **********************/
+void actuators_sts3032_set_id(float future_id)
+{
+ sts3032_future_id = (int)(future_id + 0.5);
+ sts3032_set_id(&sts, (uint8_t)sts3032_current_id, sts3032_future_id);
+}
+
+void actuators_sts3032_lock_eprom(float lock)
+{
+ sts3032_lock_eprom = (int)(lock + 0.5);
+ sts3032_lock_eeprom(&sts, (uint8_t)sts3032_current_id, sts3032_lock_eprom);
+}
+
+void actuators_sts3032_move(float pos)
+{
+ sts3032_move = pos;
+ sts3032_write_pos(&sts, sts3032_current_id, sts3032_move);
+}
+
+void actuators_sts3032_set_response_level(float level)
+{
+ sts3032_response_level = (int)(level + 0.5);
+ sts3032_set_response_level(&sts, sts3032_current_id, sts3032_response_level);
+}
+
+/************************************************************/
diff --git a/sw/airborne/modules/actuators/actuators_sts3032.h b/sw/airborne/modules/actuators/actuators_sts3032.h
new file mode 100644
index 0000000000..d221fe3111
--- /dev/null
+++ b/sw/airborne/modules/actuators/actuators_sts3032.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2023 Flo&Fab
+ *
+ * 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/servo_sts3032.h"
+ * @author Flo&Fab
+ * feetech sts3032 servo
+ */
+
+#ifndef SERVO_STS3032_H
+#define SERVO_STS3032_H
+
+#include
+#include "mcu_periph/uart.h"
+#include "utils/circular_buffer.h"
+#include "generated/airframe.h"
+
+
+enum sts3032_rx_state {
+ STS3032_RX_IDLE,
+ STS3032_RX_HEAD_OK,
+ STS3032_RX_GOT_LENGTH,
+};
+
+struct sts3032 {
+ struct uart_periph *periph;
+
+
+ uint8_t ids[SERVOS_STS3032_NB];
+ uint8_t states[SERVOS_STS3032_NB];
+ uint16_t pos[SERVOS_STS3032_NB];
+
+ uint32_t time_last_msg; // last send msg time
+
+
+ enum sts3032_rx_state rx_state;
+ uint8_t nb_bytes_expected;
+ uint8_t buf_header[2];
+ uint8_t rx_id;
+ uint8_t rx_checksum;
+ bool echo; // wait for sended message
+ uint8_t read_addr;
+ bool wait_reply;
+ uint16_t nb_failed_checksum;
+
+ struct circular_buffer msg_buf;
+};
+
+
+extern struct sts3032 sts;
+
+extern void actuators_sts3032_init(void);
+extern void actuators_sts3032_periodic(void);
+extern void actuators_sts3032_event(void);
+uint8_t id_idx(struct sts3032 *sts, uint8_t id);
+void sts3032_write_pos(struct sts3032 *sts, uint8_t id, int16_t position);
+
+/* Actuator macros */
+#define ActuatorSTS3032Set(_i, _v) { if(sts3032_enabled) {sts3032_write_pos(&sts, sts.ids[_i], _v);} }
+#define ActuatorsSTS3032Init() actuators_sts3032_init()
+#define ActuatorsSTS3032Commit() {}
+
+extern int sts3032_enabled;
+extern int sts3032_current_id;
+extern int sts3032_future_id;
+extern int sts3032_lock_eprom;
+extern int sts3032_move;
+extern int sts3032_response_level;
+
+extern void actuators_sts3032_set_id(float future_id);
+extern void actuators_sts3032_lock_eprom(float lock);
+extern void actuators_sts3032_move(float pos);
+extern void actuators_sts3032_set_response_level(float level);
+
+#endif // SERVO_STS3032_H
diff --git a/sw/airborne/peripherals/sts3032_regs.h b/sw/airborne/peripherals/sts3032_regs.h
new file mode 100644
index 0000000000..4dfa66473e
--- /dev/null
+++ b/sw/airborne/peripherals/sts3032_regs.h
@@ -0,0 +1,63 @@
+#pragma once
+
+
+#define INST_PING 0x01
+#define INST_READ 0x02
+#define INST_WRITE 0x03
+#define INST_REG_WRITE 0x04
+#define INST_REG_ACTION 0x05
+#define INST_SYNC_READ 0x82
+#define INST_SYNC_WRITE 0x83
+
+
+#define SMS_STS_1M 0
+#define SMS_STS_0_5M 1
+#define SMS_STS_250K 2
+#define SMS_STS_128K 3
+#define SMS_STS_115200 4
+#define SMS_STS_76800 5
+#define SMS_STS_57600 6
+#define SMS_STS_38400 7
+
+//memory table definition
+//-------EPROM(read only)--------
+#define SMS_STS_MODEL_L 3
+#define SMS_STS_MODEL_H 4
+
+//-------EPROM(read and write)--------
+#define SMS_STS_ID 5
+#define SMS_STS_BAUD_RATE 6
+#define SMS_STS_RESPONSE_LEVEL 8
+#define SMS_STS_MIN_ANGLE_LIMIT_L 9
+#define SMS_STS_MIN_ANGLE_LIMIT_H 10
+#define SMS_STS_MAX_ANGLE_LIMIT_L 11
+#define SMS_STS_MAX_ANGLE_LIMIT_H 12
+#define SMS_STS_CW_DEAD 26
+#define SMS_STS_CCW_DEAD 27
+#define SMS_STS_OFS_L 31
+#define SMS_STS_OFS_H 32
+#define SMS_STS_MODE 33
+
+//-------SRAM(read and write)--------
+#define SMS_STS_TORQUE_ENABLE 40
+#define SMS_STS_ACC 41
+#define SMS_STS_GOAL_POSITION_L 42
+#define SMS_STS_GOAL_POSITION_H 43
+#define SMS_STS_GOAL_TIME_L 44
+#define SMS_STS_GOAL_TIME_H 45
+#define SMS_STS_GOAL_SPEED_L 46
+#define SMS_STS_GOAL_SPEED_H 47
+#define SMS_STS_LOCK 55
+
+//-------SRAM(read only)--------
+#define SMS_STS_PRESENT_POSITION_L 56
+#define SMS_STS_PRESENT_POSITION_H 57
+#define SMS_STS_PRESENT_SPEED_L 58
+#define SMS_STS_PRESENT_SPEED_H 59
+#define SMS_STS_PRESENT_LOAD_L 60
+#define SMS_STS_PRESENT_LOAD_H 61
+#define SMS_STS_PRESENT_VOLTAGE 62
+#define SMS_STS_PRESENT_TEMPERATURE 63
+#define SMS_STS_MOVING 66
+#define SMS_STS_PRESENT_CURRENT_L 69
+#define SMS_STS_PRESENT_CURRENT_H 70