From 41bd12e95035ffb69ecb67007f7a686a62822b28 Mon Sep 17 00:00:00 2001
From: Nicolas Rabault <nicolas.rabault@luos.io>
Date: Thu, 1 Aug 2024 09:50:38 +0200
Subject: [PATCH] Add target_torque management on motor profiles

---
 engine/profiles/servo_motor/profile_servo_motor.c | 8 ++++++++
 engine/profiles/servo_motor/profile_servo_motor.h | 1 +
 2 files changed, 9 insertions(+)

diff --git a/engine/profiles/servo_motor/profile_servo_motor.c b/engine/profiles/servo_motor/profile_servo_motor.c
index 2857f3b3c..6b8c4beab 100644
--- a/engine/profiles/servo_motor/profile_servo_motor.c
+++ b/engine/profiles/servo_motor/profile_servo_motor.c
@@ -152,6 +152,14 @@ void ProfileServo_Handler(service_t *service, const msg_t *msg)
             }
         }
         break;
+        case TORQUE:
+        {
+            // set the motor target torque
+            if (servo_motor_profile->mode.mode_torque)
+            {
+                ForceOD_TorqueFromMsg((torque_t *)&servo_motor_profile->target_torque, msg);
+            }
+        }
         case LINEAR_POSITION:
         {
             // set the motor target linear position
diff --git a/engine/profiles/servo_motor/profile_servo_motor.h b/engine/profiles/servo_motor/profile_servo_motor.h
index 0f69cf3e7..22afc6e84 100644
--- a/engine/profiles/servo_motor/profile_servo_motor.h
+++ b/engine/profiles/servo_motor/profile_servo_motor.h
@@ -54,6 +54,7 @@ typedef struct
     servo_motor_mode_t mode;
     angular_position_t target_angular_position;
     angular_speed_t target_angular_speed;
+    torque_t target_torque;
 
     // limits
     angular_position_t limit_angular_position[2];