Merge pull request #71 from nasa-jpl/abrinkma-diagnostics

Abrinkma diagnostics and fault handling
nasa-jpl · Jan 25, 2023 · 2a54438 · 2a54438
2 parents 9503a0c + 1dcdbee
commit 2a54438
Show file tree

Hide file tree

Showing 23 changed files with 609 additions and 158 deletions.
diff --git a/src/fastcat_devices/fts.cc b/src/fastcat_devices/fts.cc
@@ -153,10 +153,17 @@ fastcat::FaultType fastcat::Fts::Process()
 bool fastcat::Fts::Write(DeviceCmd& cmd)
 {
   if (cmd.type == FTS_TARE_CMD) {
-    for (int ii = 0; ii < FC_FTS_N_DIMS; ii++) {
-      sig_offset_[ii] = -wrench_[ii];
+
+    // Do not permit taring if there is a fault
+    if(device_fault_active_){
+      ERROR("Taring FTS is not permited with a active fault, reset first");
+      return false;
+    }else{
+      for (int ii = 0; ii < FC_FTS_N_DIMS; ii++) {
+        sig_offset_[ii] = -wrench_[ii];
+      }
+      return true;
     }
-    return true;
   }
   else if (cmd.type == FTS_ENABLE_GUARD_FAULT_CMD) {
     enable_fts_guard_fault_ = cmd.fts_enable_guard_fault_cmd.enable;

diff --git a/src/fastcat_devices/pid.cc b/src/fastcat_devices/pid.cc
@@ -114,6 +114,11 @@ bool fastcat::Pid::Write(DeviceCmd& cmd)
     return false;
   }
 
+  if(device_fault_active_){
+    ERROR("Unable to Activate PID (%s) with an active fault, reset first", name_.c_str());
+    return false;
+  }
+
   state_->pid_state.active = true;
   pid_activate_cmd_        = cmd.pid_activate_cmd;
   activation_time_         = state_->time;

diff --git a/src/fcgen/commander.cc.cog b/src/fcgen/commander.cc.cog
@@ -179,6 +179,12 @@ bool fastcat::Commander::Read()
 
 bool fastcat::Commander::Write(DeviceCmd& cmd)
 {
+
+  if(device_fault_active_){
+    ERROR("Commander (%s) cannot be commanded with an active fault, reset first", name_.c_str());
+    return false;
+  }
+
   if (cmd.type == COMMANDER_ENABLE_CMD) {
     state_->commander_state.enable = true;
     enable_duration_ = cmd.commander_enable_cmd.duration;

diff --git a/src/jsd/actuator.cc b/src/jsd/actuator.cc
@@ -304,72 +304,19 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
     return (sdoResult == SDO_RET_VAL_SUCCESS);
   }
 
+  // Honor these Non-motion Commands even when faulted
   switch (cmd.type) {
-    case ACTUATOR_CSP_CMD:
-      if (!HandleNewCSPCmd(cmd)) {
-        ERROR("Failed to handle Actuator CSP command");
-        return false;
-      }
-      break;
-
-    case ACTUATOR_CSV_CMD:
-      if (!HandleNewCSVCmd(cmd)) {
-        ERROR("Failed to handle Actuator CSV command");
-        return false;
-      }
-      break;
-
-    case ACTUATOR_CST_CMD:
-      if (!HandleNewCSTCmd(cmd)) {
-        ERROR("Failed to handle Actuator CST command");
-        return false;
-      }
-      break;
-
-    case ACTUATOR_PROF_POS_CMD:
-      if (!HandleNewProfPosCmd(cmd)) {
-        ERROR("Failed to setup Actuator Profiled Pos command");
-        return false;
-      }
-      break;
-
-    case ACTUATOR_PROF_VEL_CMD:
-      if (!HandleNewProfVelCmd(cmd)) {
-        ERROR("Failed to setup Actuator Profiled Pos command");
-        return false;
-      }
-      break;
-
-    case ACTUATOR_PROF_TORQUE_CMD:
-      if (!HandleNewProfTorqueCmd(cmd)) {
-        ERROR("Failed to setup Actuator Profiled Pos command");
-        return false;
-      }
-      break;
-
     case ACTUATOR_RESET_CMD:
       Reset();
-      break;
-
-    case ACTUATOR_HALT_CMD:
-      if (!HandleNewHaltCmd()) {
-        ERROR("Failed to handle Halt Command");
-        return false;
-      }
+      return true;
       break;
 
     case ACTUATOR_SET_OUTPUT_POSITION_CMD:
       if (!HandleNewSetOutputPositionCmd(cmd)) {
         ERROR("Failed to handle Set Output Position Command");
         return false;
       }
-      break;
-
-    case ACTUATOR_CALIBRATE_CMD:
-      if (!HandleNewCalibrationCmd(cmd)) {
-        ERROR("Failed to handle Calibrate Command");
-        return false;
-      }
+      return true;
       break;
 
     case ACTUATOR_SET_MAX_CURRENT_CMD:
@@ -378,13 +325,15 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       // phases so don't check the state machine
       peak_current_limit_amps_ = cmd.actuator_set_max_current_cmd.current;
       EgdSetPeakCurrent(peak_current_limit_amps_);
+      return true;
       break;
 
     case ACTUATOR_SDO_SET_UNIT_MODE_CMD:
       if (!HandleNewSetUnitModeCmd(cmd)) {
         ERROR("Failed to handle Set Unit Mode Command");
         return false;
-      }
+      } 
+      return true;
       break;
 
     case ACTUATOR_SDO_DISABLE_GAIN_SCHEDULING_CMD: {
@@ -395,6 +344,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       EgdSetGainSchedulingMode(
           JSD_EGD_GAIN_SCHEDULING_MODE_DISABLED,
           cmd.actuator_sdo_disable_gain_scheduling_cmd.app_id);
+      return true;
       break;
     }
 
@@ -406,6 +356,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       EgdSetGainSchedulingMode(
           JSD_EGD_GAIN_SCHEDULING_MODE_SPEED,
           cmd.actuator_sdo_enable_speed_gain_scheduling_cmd.app_id);
+      return true;
       break;
     }
 
@@ -417,6 +368,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       EgdSetGainSchedulingMode(
           JSD_EGD_GAIN_SCHEDULING_MODE_POSITION,
           cmd.actuator_sdo_enable_position_gain_scheduling_cmd.app_id);
+      return true;
       break;
     }
 
@@ -428,6 +380,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       EgdSetGainSchedulingMode(
           JSD_EGD_GAIN_SCHEDULING_MODE_MANUAL_LOW, 
           cmd.actuator_sdo_enable_manual_gain_scheduling_cmd.app_id);
+      return true;
       break;
     }
 
@@ -438,8 +391,77 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
       }
       EgdSetGainSchedulingIndex( 
           cmd.actuator_set_gain_scheduling_index_cmd.gain_scheduling_index);
+      return true;
       break;
     }
+    default:
+      // no-op
+      break;
+  }
+
+
+  // Return early if a fault is active
+  // So as to not honor these motion commands when faulted 
+  if(device_fault_active_){
+    return false;
+  }
+
+  switch (cmd.type) {
+    case ACTUATOR_CSP_CMD:
+      if (!HandleNewCSPCmd(cmd)) {
+        ERROR("Failed to handle Actuator CSP command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_CSV_CMD:
+      if (!HandleNewCSVCmd(cmd)) {
+        ERROR("Failed to handle Actuator CSV command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_CST_CMD:
+      if (!HandleNewCSTCmd(cmd)) {
+        ERROR("Failed to handle Actuator CST command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_PROF_POS_CMD:
+      if (!HandleNewProfPosCmd(cmd)) {
+        ERROR("Failed to setup Actuator Profiled Pos command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_PROF_VEL_CMD:
+      if (!HandleNewProfVelCmd(cmd)) {
+        ERROR("Failed to setup Actuator Profiled Pos command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_PROF_TORQUE_CMD:
+      if (!HandleNewProfTorqueCmd(cmd)) {
+        ERROR("Failed to setup Actuator Profiled Pos command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_CALIBRATE_CMD:
+      if (!HandleNewCalibrationCmd(cmd)) {
+        ERROR("Failed to handle Calibrate Command");
+        return false;
+      }
+      break;
+
+    case ACTUATOR_HALT_CMD:
+      if (!HandleNewHaltCmd()) {
+        ERROR("Failed to handle Halt Command");
+        return false;
+      }
+      break;
 
     default:
       WARNING("That command type is not supported in this mode!");

diff --git a/src/jsd/ati_fts.cc b/src/jsd/ati_fts.cc
@@ -141,13 +141,20 @@ bool fastcat::AtiFts::Write(DeviceCmd& cmd)
   }
 
   if (cmd.type == FTS_TARE_CMD) {
-    bias_[0] = -state_->fts_state.raw_fx;
-    bias_[1] = -state_->fts_state.raw_fy;
-    bias_[2] = -state_->fts_state.raw_fz;
-    bias_[3] = -state_->fts_state.raw_tx;
-    bias_[4] = -state_->fts_state.raw_ty;
-    bias_[5] = -state_->fts_state.raw_tz;
-    return true;
+
+    // Do not permit taring if there is a fault
+    if(device_fault_active_){
+      ERROR("Taring (%s) FTS is not permited with an active fault, reset first", name_.c_str());
+      return false;
+    }else{
+      bias_[0] = -state_->fts_state.raw_fx;
+      bias_[1] = -state_->fts_state.raw_fy;
+      bias_[2] = -state_->fts_state.raw_fz;
+      bias_[3] = -state_->fts_state.raw_tx;
+      bias_[4] = -state_->fts_state.raw_ty;
+      bias_[5] = -state_->fts_state.raw_tz;
+      return true;
+    }
   }
   else if (cmd.type == FTS_ENABLE_GUARD_FAULT_CMD) {
     enable_fts_guard_fault_ = cmd.fts_enable_guard_fault_cmd.enable;

diff --git a/src/manager.cc b/src/manager.cc
@@ -655,13 +655,6 @@ bool fastcat::Manager::ConfigOfflineBusFromYaml(YAML::Node node)
 
 bool fastcat::Manager::WriteCommands()
 {
-  if (faulted_) {
-    // Clear device command queue
-    while (!cmd_queue_->empty()) {
-      cmd_queue_->pop();
-    }
-    return true;
-  }
 
   while (!cmd_queue_->empty()) {
     DeviceCmd cmd = cmd_queue_->front();

diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
@@ -21,9 +21,6 @@ target_link_libraries(test_config ${fastcat_test_libs})
 add_executable(test_cli test_cli.cc)
 target_link_libraries(test_cli ${fastcat_test_libs})
 
-add_executable(test_filter test_filter.cc)
-target_link_libraries(test_filter ${fastcat_test_libs})
-
 find_package(GTest)
 if(GTEST_FOUND)
     add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/test_unit)

diff --git a/test/test_filter.cc b/test/test_filter.cc
diff --git a/test/test_unit/CMakeLists.txt b/test/test_unit/CMakeLists.txt
@@ -1,17 +1,20 @@
 find_package(Threads REQUIRED)
 
 set(TEST_SOURCES
+    test_actuator.cc
+    test_commander.cc
     test_conditional.cc
     test_function.cc
+    test_jsd_device_base.cc
+    test_linear_interpolation.cc
     test_schmitt_trigger.cc
     test_signal_generator.cc
     test_transform_utils.cc
-    test_yaml_parser.cc
-    test_linear_interpolation.cc
-
-    test_jsd_device_base.cc
-
     test_trap.cc
+    test_yaml_parser.cc
+    test_fts.cc
+    test_ati_fts.cc
+    test_virtual_fts.cc
     )
 
 foreach(TEST_SOURCE ${TEST_SOURCES})