From 37591215615a518495e5a71c9daba3b730cd5bde Mon Sep 17 00:00:00 2001
From: Josh Pieper <jjp@pobox.com>
Date: Sun, 23 Jul 2023 20:14:27 -0400
Subject: [PATCH] Implement a new timeout mode for position hold

When this mode is configured (mode 10), upon timeout the controller
will use the default acceleration limit to decelerate to 0 speed, then
hold that position using the current PID gains and no torque limit.
---
 docs/reference.md       |  6 ++++++
 fw/bldc_servo.cc        | 17 ++++++++++++++++-
 fw/bldc_servo_structs.h |  1 +
 3 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/docs/reference.md b/docs/reference.md
index 8283ae53..76758dff 100644
--- a/docs/reference.md
+++ b/docs/reference.md
@@ -1820,9 +1820,15 @@ Selects what behavior will take place in the position timeout mode.
 The allowable values are a subset of the top level modes.
 
 * 0 - "stopped" - the driver is disengaged
+* 10 - "decelerate to 0 velocity and hold position"
 * 12 - "zero velocity"
 * 15 - "brake"
 
+For mode 10, `servo.default_velocity_limit` and
+`servo.default_accel_limit` are used to control the deceleration
+profile to zero speed.  The default PID gains are used.  The only
+limit on torque when in this timeout mode is `servo.max_current_A`.
+
 ## `aux[12].pins.X.mode` ##
 
 Selects what functionality will be used on the given pin.
diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc
index 1031fb8f..4df8841c 100644
--- a/fw/bldc_servo.cc
+++ b/fw/bldc_servo.cc
@@ -1123,7 +1123,8 @@ class BldcServo::Impl {
         return true;
       }
       case kPositionTimeout: {
-        return config_.timeout_mode == BldcServoMode::kZeroVelocity;
+        return (config_.timeout_mode == BldcServoMode::kZeroVelocity ||
+                config_.timeout_mode == BldcServoMode::kPosition);
       }
     }
     return false;
@@ -1829,6 +1830,20 @@ class BldcServo::Impl {
   void ISR_DoPositionTimeout(const SinCos& sin_cos, CommandData* data) MOTEUS_CCM_ATTRIBUTE {
     if (config_.timeout_mode == kStopped) {
       ISR_DoStopped(sin_cos);
+    } else if (config_.timeout_mode == kPosition) {
+      CommandData timeout_data;
+      timeout_data.mode = kPosition;
+      timeout_data.position = std::numeric_limits<float>::quiet_NaN();
+      timeout_data.velocity_limit = config_.default_velocity_limit;
+      timeout_data.accel_limit = config_.default_accel_limit;
+      timeout_data.timeout_s = std::numeric_limits<float>::quiet_NaN();
+
+      PID::ApplyOptions apply_options;
+      ISR_DoPositionCommon(
+          sin_cos, &timeout_data, apply_options,
+          timeout_data.max_torque_Nm,
+          0.0f,
+          0.0f);
     } else if (config_.timeout_mode == kZeroVelocity) {
       ISR_DoZeroVelocity(sin_cos, data);
     } else if (config_.timeout_mode == kBrake) {
diff --git a/fw/bldc_servo_structs.h b/fw/bldc_servo_structs.h
index c543e0e6..6d7d36b4 100644
--- a/fw/bldc_servo_structs.h
+++ b/fw/bldc_servo_structs.h
@@ -517,6 +517,7 @@ struct BldcServoConfig {
   // options map to top level modes, although only the following are
   // valid:
   //  0 - "stopped" - motor driver disengaged
+  //  10 - "decelerate to 0 and hold position"
   //  12 - "zero velocity" - derivative only position control
   //  15 - "brake" - all motor phases shorted to ground
   uint8_t timeout_mode = 12;