Merge pull request #130 from nasa-jpl/dloret-commutation-search-support

Adds ability to modify the timeout in Profile Disengaging states

Author: d-loret

src/fcgen/fastcat_types.yaml

@@ -851,6 +851,11 @@ commands:
     - name: position
       type: double
 
+  - name: actuator_set_prof_disengaging_timeout
+    fields:
+    - name: timeout
+      type: double
+
   - name: actuator_calibrate
     fields:
     - name: velocity

src/jsd/actuator.cc

@@ -255,6 +255,14 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       return true;
       break;
 
+    case ACTUATOR_SET_PROF_DISENGAGING_TIMEOUT_CMD:
+      if (!HandleNewSetProfDisengagingTimeoutCmd(cmd)) {
+        ERROR("Failed to handle Set Profile Disengaging Timeout Command");
+        return false;
+      }
+      return true;
+      break;
+
     case ACTUATOR_SET_DIGITAL_OUTPUT_CMD:
       ElmoSetDigitalOutput(
           cmd.actuator_set_digital_output_cmd.digital_output_index,

src/jsd/actuator.h

@@ -138,6 +138,14 @@ class Actuator : public JsdDeviceBase
 
   DeviceCmd last_cmd_ = {};
 
+  // Timeout in seconds after which a fault should occur if a transition out of
+  // PROF_*_DISENGAGING does not take place. Default value corresponds to the
+  // maximum value the drive's brake disengage parameter (BP[2]) can take.
+  // prof_disengaging_timeout_ can be temporarily increased to allow for an
+  // initial commutation search in actuators that do not have hall sensors, for
+  // example.
+  double prof_disengaging_timeout_ = 1.0;
+
  private:
   bool PosExceedsCmdLimits(double pos_eu);
   bool VelExceedsCmdLimits(double vel_eu);
@@ -149,6 +157,7 @@ class Actuator : public JsdDeviceBase
   bool HandleNewResetCmd();
   bool HandleNewSetOutputPositionCmd(const DeviceCmd& cmd);
   bool HandleNewSetUnitModeCmd(const DeviceCmd& cmd);
+  bool HandleNewSetProfDisengagingTimeoutCmd(const DeviceCmd& cmd);
   bool HandleNewCalibrationCmd(const DeviceCmd& cmd);
   bool HandleNewCSPCmd(const DeviceCmd& cmd);
   bool HandleNewCSVCmd(const DeviceCmd& cmd);

src/jsd/actuator_fsm_helpers.cc

@@ -345,6 +345,41 @@ bool fastcat::Actuator::HandleNewSetUnitModeCmd(const DeviceCmd& cmd)
   return true;
 }
 
+bool fastcat::Actuator::HandleNewSetProfDisengagingTimeoutCmd(
+    const DeviceCmd& cmd)
+{
+  switch (actuator_sms_) {
+    case ACTUATOR_SMS_FAULTED:
+    case ACTUATOR_SMS_HALTED:
+    case ACTUATOR_SMS_HOLDING:
+      break;
+
+    case ACTUATOR_SMS_PROF_POS:
+    case ACTUATOR_SMS_PROF_POS_DISENGAGING:
+    case ACTUATOR_SMS_PROF_VEL:
+    case ACTUATOR_SMS_PROF_VEL_DISENGAGING:
+    case ACTUATOR_SMS_PROF_TORQUE:
+    case ACTUATOR_SMS_PROF_TORQUE_DISENGAGING:
+    case ACTUATOR_SMS_CS:
+    case ACTUATOR_SMS_CAL_MOVE_TO_HARDSTOP:
+    case ACTUATOR_SMS_CAL_AT_HARDSTOP:
+    case ACTUATOR_SMS_CAL_MOVE_TO_SOFTSTOP:
+      ERROR("Act %s: %s", name_.c_str(),
+            "Cannot set profile disengaging timeout, motion command is active");
+      fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_INVALID_CMD_DURING_MOTION;
+      return false;
+
+    default:
+      ERROR("Act %s: %s: %d", name_.c_str(), "Bad Act State ", actuator_sms_);
+      return false;
+  }
+
+  prof_disengaging_timeout_ =
+      cmd.actuator_set_prof_disengaging_timeout_cmd.timeout;
+
+  return true;
+}
+
 bool fastcat::Actuator::HandleNewCalibrationCmd(const DeviceCmd& cmd)
 {
   switch (actuator_sms_) {

src/jsd/gold_actuator.cc

@@ -344,11 +344,10 @@ fastcat::FaultType fastcat::GoldActuator::ProcessProfPosDisengaging()
 
     ElmoCSP(jsd_cmd);
 
-    // Check runout timer here, brake engage/disengage time cannot exceed 1
-    // second per MAN-G-CR Section BP - Brake Parameters
-    if ((cycle_mono_time_ - last_transition_time_) > (1.0 + 2 * loop_period_)) {
-      ERROR("Act %s: Brake Disengage 1.0 sec runout timer expired, faulting",
-            name_.c_str());
+    if ((cycle_mono_time_ - last_transition_time_) >
+        (prof_disengaging_timeout_ + 2 * loop_period_)) {
+      ERROR("Act %s: Brake Disengage %lf sec timeout expired, faulting",
+            name_.c_str(), prof_disengaging_timeout_);
       fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_BRAKE_DISENGAGE_TIMEOUT_EXCEEDED;
       return ALL_DEVICE_FAULT;
     }
@@ -387,11 +386,10 @@ fastcat::FaultType fastcat::GoldActuator::ProcessProfVelDisengaging()
 
     ElmoCSV(jsd_cmd);
 
-    // Check runout timer here, brake engage/disengage time cannot exceed 1
-    // second per MAN-G-CR Section BP - Brake Parameters
-    if ((cycle_mono_time_ - last_transition_time_) > (1.0 + 2 * loop_period_)) {
-      ERROR("Act %s: Brake Disengage 1.0 sec runout timer expired, faulting",
-            name_.c_str());
+    if ((cycle_mono_time_ - last_transition_time_) >
+        (prof_disengaging_timeout_ + 2 * loop_period_)) {
+      ERROR("Act %s: Brake Disengage %lf sec timeout expired, faulting",
+            name_.c_str(), prof_disengaging_timeout_);
       fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_BRAKE_DISENGAGE_TIMEOUT_EXCEEDED;
       return ALL_DEVICE_FAULT;
     }
@@ -428,11 +426,10 @@ fastcat::FaultType fastcat::GoldActuator::ProcessProfTorqueDisengaging()
 
     ElmoCST(jsd_cmd);
 
-    // Check runout timer here, brake engage/disengage time cannot exceed 1
-    // second per MAN-G-CR Section BP - Brake Parameters
-    if ((cycle_mono_time_ - last_transition_time_) > (1.0 + 2 * loop_period_)) {
-      ERROR("Act %s: Brake Disengage 1.0 sec runout timer expired, faulting",
-            name_.c_str());
+    if ((cycle_mono_time_ - last_transition_time_) >
+        (prof_disengaging_timeout_ + 2 * loop_period_)) {
+      ERROR("Act %s: Brake Disengage %lf sec timeout expired, faulting",
+            name_.c_str(), prof_disengaging_timeout_);
       fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_BRAKE_DISENGAGE_TIMEOUT_EXCEEDED;
       return ALL_DEVICE_FAULT;
     }

src/jsd/platinum_actuator.cc

@@ -199,12 +199,12 @@ fastcat::FaultType fastcat::PlatinumActuator::ProcessProfPosDisengaging()
   // Transition to ACTUATOR_SMS_PROF_POS once the drive acknowledges reception
   // of the command. Otherwise, the state variables used to check for the
   // completion of the command (i.e. target_reached) might refer to a previous
-  // command. If the drive does not acknowledge the command within 1 second,
-  // fault.
+  // command. If the drive does not acknowledge the command within the
+  // configured timeout, fault.
   if (state_->platinum_actuator_state.setpoint_ack_rise) {
     TransitionToState(ACTUATOR_SMS_PROF_POS);
   } else if ((cycle_mono_time_ - last_transition_time_) >
-             (1.0 + 2.0 * loop_period_)) {
+             (prof_disengaging_timeout_ + 2.0 * loop_period_)) {
     ERROR(
         "Act %s: Profiled Position command was not acknowledged by drive "
         "before timeout, faulting",