Skip to content

Commit

Permalink
Merge pull request #82 from nasa-jpl/davkim-fault-to-purge
Browse files Browse the repository at this point in the history 
Bumped JSD, purging on fault, update to change of egd to elmo
  • Loading branch information
@alex-brinkman
alex-brinkman authored Mar 31, 2023
2 parents bfcb765 + e2ad625 commit 99bcbb6
Show file tree
Hide file tree
Showing 9 changed files with 78 additions and 63 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ build
externals
bin
.vscode
log

# doxygen
/doxygen_html
Expand Down
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ find_package(YamlCpp REQUIRED 0.6.3)
include(FetchContent)
FetchContent_Declare(jsd
GIT_REPOSITORY https://github.com/nasa-jpl/jsd.git
GIT_TAG v2.1.2
GIT_TAG v2.3.0
)
FetchContent_MakeAvailable(jsd)

Expand Down
35 changes: 21 additions & 14 deletions src/jsd/actuator.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ bool fastcat::Actuator::ConfigFromYaml(YAML::Node node)
} else {
// Use mode saved in driver's non-volatile memory.
jsd_slave_config_.egd.ctrl_gain_scheduling_mode =
JSD_EGD_GAIN_SCHEDULING_MODE_PRELOADED;
JSD_ELMO_GAIN_SCHEDULING_MODE_PRELOADED;
}

if (!ParseOptVal(node, "absolute_encoder",
Expand Down Expand Up @@ -361,7 +361,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
return false;
}
EgdSetGainSchedulingMode(
JSD_EGD_GAIN_SCHEDULING_MODE_DISABLED,
JSD_ELMO_GAIN_SCHEDULING_MODE_DISABLED,
cmd.actuator_sdo_disable_gain_scheduling_cmd.app_id);
return true;
break;
Expand All @@ -373,7 +373,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
return false;
}
EgdSetGainSchedulingMode(
JSD_EGD_GAIN_SCHEDULING_MODE_SPEED,
JSD_ELMO_GAIN_SCHEDULING_MODE_SPEED,
cmd.actuator_sdo_enable_speed_gain_scheduling_cmd.app_id);
return true;
break;
Expand All @@ -385,7 +385,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
return false;
}
EgdSetGainSchedulingMode(
JSD_EGD_GAIN_SCHEDULING_MODE_POSITION,
JSD_ELMO_GAIN_SCHEDULING_MODE_POSITION,
cmd.actuator_sdo_enable_position_gain_scheduling_cmd.app_id);
return true;
break;
Expand All @@ -397,7 +397,7 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
return false;
}
EgdSetGainSchedulingMode(
JSD_EGD_GAIN_SCHEDULING_MODE_MANUAL_LOW,
JSD_ELMO_GAIN_SCHEDULING_MODE_MANUAL_LOW,
cmd.actuator_sdo_enable_manual_gain_scheduling_cmd.app_id);
return true;
break;
Expand Down Expand Up @@ -555,6 +555,7 @@ fastcat::FaultType fastcat::Actuator::Process()
void fastcat::Actuator::Fault()
{
WARNING("Faulting Actuator %s", name_.c_str());
EgdFault();

// need to clear so that old commands are not left over
// for new commands
Expand Down Expand Up @@ -757,6 +758,12 @@ void fastcat::Actuator::EgdClearErrors()
jsd_egd_clear_errors((jsd_t*)context_, slave_id_);
}

void fastcat::Actuator::EgdFault()
{
MSG("Faulting EGD through JSD: %s", name_.c_str());
jsd_egd_fault((jsd_t*)context_, slave_id_);
}

void fastcat::Actuator::EgdReset()
{
MSG("Resetting EGD through JSD: %s", name_.c_str());
Expand All @@ -776,23 +783,23 @@ void fastcat::Actuator::EgdSetUnitMode(int32_t mode, uint16_t app_id)
jsd_egd_async_sdo_set_unit_mode((jsd_t*)context_, slave_id_, mode, app_id);
}

void fastcat::Actuator::EgdCSP(jsd_egd_motion_command_csp_t jsd_csp_cmd)
void fastcat::Actuator::EgdCSP(jsd_elmo_motion_command_csp_t jsd_csp_cmd)
{
jsd_egd_set_motion_command_csp((jsd_t*)context_, slave_id_, jsd_csp_cmd);
}

void fastcat::Actuator::EgdCSV(jsd_egd_motion_command_csv_t jsd_csv_cmd)
void fastcat::Actuator::EgdCSV(jsd_elmo_motion_command_csv_t jsd_csv_cmd)
{
jsd_egd_set_motion_command_csv((jsd_t*)context_, slave_id_, jsd_csv_cmd);
}

void fastcat::Actuator::EgdCST(jsd_egd_motion_command_cst_t jsd_cst_cmd)
void fastcat::Actuator::EgdCST(jsd_elmo_motion_command_cst_t jsd_cst_cmd)
{
jsd_egd_set_motion_command_cst((jsd_t*)context_, slave_id_, jsd_cst_cmd);
}

void fastcat::Actuator::EgdSetGainSchedulingMode(
jsd_egd_gain_scheduling_mode_t mode, uint16_t app_id)
jsd_elmo_gain_scheduling_mode_t mode, uint16_t app_id)
{
jsd_egd_async_sdo_set_ctrl_gain_scheduling_mode((jsd_t*)context_, slave_id_,
mode, app_id);
Expand All @@ -804,17 +811,17 @@ void fastcat::Actuator::EgdSetGainSchedulingIndex(uint16_t index)
}

bool fastcat::Actuator::GSModeFromString(
std::string gs_mode_string, jsd_egd_gain_scheduling_mode_t& gs_mode)
std::string gs_mode_string, jsd_elmo_gain_scheduling_mode_t& gs_mode)
{
MSG("Converting gain scheduling mode to string.");
if (gs_mode_string.compare("DISABLED") == 0) {
gs_mode = JSD_EGD_GAIN_SCHEDULING_MODE_DISABLED;
gs_mode = JSD_ELMO_GAIN_SCHEDULING_MODE_DISABLED;
} else if (gs_mode_string.compare("SPEED") == 0) {
gs_mode = JSD_EGD_GAIN_SCHEDULING_MODE_SPEED;
gs_mode = JSD_ELMO_GAIN_SCHEDULING_MODE_SPEED;
} else if (gs_mode_string.compare("POSITION") == 0) {
gs_mode = JSD_EGD_GAIN_SCHEDULING_MODE_POSITION;
gs_mode = JSD_ELMO_GAIN_SCHEDULING_MODE_POSITION;
} else if (gs_mode_string.compare("MANUAL") == 0) {
gs_mode = JSD_EGD_GAIN_SCHEDULING_MODE_MANUAL_LOW;
gs_mode = JSD_ELMO_GAIN_SCHEDULING_MODE_MANUAL_LOW;
} else {
ERROR("Gain scheduling mode %s is invalid", gs_mode_string.c_str());
return false;
Expand Down
11 changes: 6 additions & 5 deletions src/jsd/actuator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,14 +150,15 @@ class Actuator : public JsdDeviceBase
virtual void EgdSetConfig();
virtual void EgdProcess();
virtual void EgdClearErrors();
virtual void EgdFault();
virtual void EgdReset();
virtual void EgdHalt();
virtual void EgdSetPeakCurrent(double current);
virtual void EgdSetUnitMode(int32_t mode, uint16_t app_id);
virtual void EgdCSP(jsd_egd_motion_command_csp_t jsd_csp_cmd);
virtual void EgdCSV(jsd_egd_motion_command_csv_t jsd_csv_cmd);
virtual void EgdCST(jsd_egd_motion_command_cst_t jsd_cst_cmd);
virtual void EgdSetGainSchedulingMode(jsd_egd_gain_scheduling_mode_t mode,
virtual void EgdCSP(jsd_elmo_motion_command_csp_t jsd_csp_cmd);
virtual void EgdCSV(jsd_elmo_motion_command_csv_t jsd_csv_cmd);
virtual void EgdCST(jsd_elmo_motion_command_cst_t jsd_cst_cmd);
virtual void EgdSetGainSchedulingMode(jsd_elmo_gain_scheduling_mode_t mode,
uint16_t app_id);
virtual void EgdSetGainSchedulingIndex(uint16_t index);

Expand All @@ -181,7 +182,7 @@ class Actuator : public JsdDeviceBase

private:
bool GSModeFromString(std::string gs_mode_string,
jsd_egd_gain_scheduling_mode_t& gs_mode);
jsd_elmo_gain_scheduling_mode_t& gs_mode);

bool prof_pos_hold_;

Expand Down
28 changes: 14 additions & 14 deletions src/jsd/actuator_fsm_helpers.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ bool fastcat::Actuator::HandleNewCSPCmd(DeviceCmd& cmd)
return false;
}

jsd_egd_motion_command_csp_t jsd_cmd;
jsd_elmo_motion_command_csp_t jsd_cmd;
jsd_cmd.target_position = PosEuToCnts(cmd.actuator_csp_cmd.target_position);
jsd_cmd.position_offset = EuToCnts(cmd.actuator_csp_cmd.position_offset);
jsd_cmd.velocity_offset = EuToCnts(cmd.actuator_csp_cmd.velocity_offset);
Expand Down Expand Up @@ -136,7 +136,7 @@ bool fastcat::Actuator::HandleNewCSVCmd(DeviceCmd& cmd)
return false;
}

jsd_egd_motion_command_csv_t jsd_cmd;
jsd_elmo_motion_command_csv_t jsd_cmd;
jsd_cmd.target_velocity = EuToCnts(cmd.actuator_csv_cmd.target_velocity);
jsd_cmd.velocity_offset = EuToCnts(cmd.actuator_csv_cmd.velocity_offset);
jsd_cmd.torque_offset_amps = cmd.actuator_csv_cmd.torque_offset_amps;
Expand Down Expand Up @@ -165,7 +165,7 @@ bool fastcat::Actuator::HandleNewCSTCmd(DeviceCmd& cmd)
return false;
}

jsd_egd_motion_command_cst_t jsd_cmd;
jsd_elmo_motion_command_cst_t jsd_cmd;
jsd_cmd.target_torque_amps = cmd.actuator_cst_cmd.target_torque_amps;
jsd_cmd.torque_offset_amps = cmd.actuator_cst_cmd.torque_offset_amps;

Expand Down Expand Up @@ -524,11 +524,11 @@ bool fastcat::Actuator::IsMotionFaultConditionMet()
return true;
}
if (state_->actuator_state.egd_state_machine_state ==
JSD_EGD_STATE_MACHINE_STATE_QUICK_STOP_ACTIVE ||
JSD_ELMO_STATE_MACHINE_STATE_QUICK_STOP_ACTIVE ||
state_->actuator_state.egd_state_machine_state ==
JSD_EGD_STATE_MACHINE_STATE_FAULT_REACTION_ACTIVE ||
JSD_ELMO_STATE_MACHINE_STATE_FAULT_REACTION_ACTIVE ||
state_->actuator_state.egd_state_machine_state ==
JSD_EGD_STATE_MACHINE_STATE_FAULT) {
JSD_ELMO_STATE_MACHINE_STATE_FAULT) {
ERROR("%s: EGD state machine state is off nominal", name_.c_str());
fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_INVALID_EGD_SMS_DURING_MOTION;
return true;
Expand Down Expand Up @@ -568,7 +568,7 @@ fastcat::FaultType fastcat::Actuator::ProcessProfPos()
return ALL_DEVICE_FAULT;
}

jsd_egd_motion_command_csp_t jsd_cmd;
jsd_elmo_motion_command_csp_t jsd_cmd;

double pos_eu, vel;
int complete = trap_update(&trap_, state_->time, &pos_eu, &vel);
Expand All @@ -594,7 +594,7 @@ fastcat::FaultType fastcat::Actuator::ProcessProfVel()
return ALL_DEVICE_FAULT;
}

jsd_egd_motion_command_csv_t jsd_cmd;
jsd_elmo_motion_command_csv_t jsd_cmd;

double pos_eu, vel;
int complete = trap_update_vel(&trap_, state_->time, &pos_eu, &vel);
Expand All @@ -620,7 +620,7 @@ fastcat::FaultType fastcat::Actuator::ProcessProfTorque()
return ALL_DEVICE_FAULT;
}

jsd_egd_motion_command_cst_t jsd_cmd;
jsd_elmo_motion_command_cst_t jsd_cmd;

double dummy_pos_eu, current;
int complete = trap_update_vel(&trap_, state_->time, &dummy_pos_eu, &current);
Expand Down Expand Up @@ -678,7 +678,7 @@ fastcat::FaultType fastcat::Actuator::ProcessCalMoveToHardstop()
TransitionToState(ACTUATOR_SMS_CAL_AT_HARDSTOP);
}

jsd_egd_motion_command_csp_t jsd_cmd;
jsd_elmo_motion_command_csp_t jsd_cmd;

double pos_eu, vel;
int complete = trap_update(&trap_, state_->time, &pos_eu, &vel);
Expand Down Expand Up @@ -713,7 +713,7 @@ fastcat::FaultType fastcat::Actuator::ProcessCalAtHardstop()
// Loop here until the drive is no longer faulted

if (state_->actuator_state.egd_state_machine_state !=
JSD_EGD_STATE_MACHINE_STATE_OPERATION_ENABLED &&
JSD_ELMO_STATE_MACHINE_STATE_OPERATION_ENABLED &&
state_->actuator_state.jsd_fault_code != 0) {
// We have waited too long, fault
if ((state_->time - last_transition_time_) > 5.0) {
Expand Down Expand Up @@ -785,7 +785,7 @@ fastcat::FaultType fastcat::Actuator::ProcessProfPosDisengaging()
// Otherwise, command the current position to trigger the transition and
// wait

jsd_egd_motion_command_csp_t jsd_cmd;
jsd_elmo_motion_command_csp_t jsd_cmd;

jsd_cmd.target_position =
PosEuToCnts(state_->actuator_state.actual_position);
Expand Down Expand Up @@ -831,7 +831,7 @@ fastcat::FaultType fastcat::Actuator::ProcessProfVelDisengaging()
} else {
// Otherwise, command the current position to trigger the transition and
// wait
jsd_egd_motion_command_csv_t jsd_cmd;
jsd_elmo_motion_command_csv_t jsd_cmd;

jsd_cmd.target_velocity = 0;
jsd_cmd.velocity_offset = 0;
Expand Down Expand Up @@ -873,7 +873,7 @@ fastcat::FaultType fastcat::Actuator::ProcessProfTorqueDisengaging()
} else {
// Otherwise, command the current position to trigger the transition and
// wait
jsd_egd_motion_command_cst_t jsd_cmd;
jsd_elmo_motion_command_cst_t jsd_cmd;

jsd_cmd.target_torque_amps = 0;
jsd_cmd.torque_offset_amps = 0;
Expand Down
13 changes: 9 additions & 4 deletions src/jsd/actuator_offline.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,11 @@ void fastcat::ActuatorOffline::EgdClearErrors()
// no-op
}

void fastcat::ActuatorOffline::EgdFault()
{
// no-op
}

void fastcat::ActuatorOffline::EgdReset()
{
// no-op
Expand All @@ -91,7 +96,7 @@ void fastcat::ActuatorOffline::EgdSetUnitMode(int32_t mode, uint16_t app_id)
}

void fastcat::ActuatorOffline::EgdSetGainSchedulingMode(
jsd_egd_gain_scheduling_mode_t /* mode */, uint16_t /* app_id */)
jsd_elmo_gain_scheduling_mode_t /* mode */, uint16_t /* app_id */)
{
// no-op
}
Expand All @@ -101,7 +106,7 @@ void fastcat::ActuatorOffline::EgdSetGainSchedulingIndex(uint16_t /* index */)
// no-op
}

void fastcat::ActuatorOffline::EgdCSP(jsd_egd_motion_command_csp_t jsd_csp_cmd)
void fastcat::ActuatorOffline::EgdCSP(jsd_elmo_motion_command_csp_t jsd_csp_cmd)
{
jsd_egd_state_.cmd_position = jsd_csp_cmd.target_position;
jsd_egd_state_.cmd_velocity = 0;
Expand All @@ -125,7 +130,7 @@ void fastcat::ActuatorOffline::EgdCSP(jsd_egd_motion_command_csp_t jsd_csp_cmd)
jsd_egd_state_.actual_velocity = vel; // "sure, why not"
}

void fastcat::ActuatorOffline::EgdCSV(jsd_egd_motion_command_csv_t jsd_csv_cmd)
void fastcat::ActuatorOffline::EgdCSV(jsd_elmo_motion_command_csv_t jsd_csv_cmd)
{
jsd_egd_state_.cmd_position = 0;
jsd_egd_state_.cmd_velocity = jsd_csv_cmd.target_velocity;
Expand All @@ -144,7 +149,7 @@ void fastcat::ActuatorOffline::EgdCSV(jsd_egd_motion_command_csv_t jsd_csv_cmd)
jsd_egd_state_.actual_velocity * loop_period_; // integrated
}

void fastcat::ActuatorOffline::EgdCST(jsd_egd_motion_command_cst_t jsd_cst_cmd)
void fastcat::ActuatorOffline::EgdCST(jsd_elmo_motion_command_cst_t jsd_cst_cmd)
{
jsd_egd_state_.cmd_position = 0;
jsd_egd_state_.cmd_velocity = 0;
Expand Down
9 changes: 5 additions & 4 deletions src/jsd/actuator_offline.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,14 +21,15 @@ class ActuatorOffline : public Actuator
void EgdSetConfig() override;
void EgdProcess() override;
void EgdClearErrors() override;
void EgdFault() override;
void EgdReset() override;
void EgdHalt() override;
void EgdSetPeakCurrent(double current) override;
void EgdSetUnitMode(int32_t mode, uint16_t app_id) override;
void EgdCSP(jsd_egd_motion_command_csp_t jsd_csp_cmd) override;
void EgdCSV(jsd_egd_motion_command_csv_t jsd_csv_cmd) override;
void EgdCST(jsd_egd_motion_command_cst_t jsd_cst_cmd) override;
void EgdSetGainSchedulingMode(jsd_egd_gain_scheduling_mode_t mode,
void EgdCSP(jsd_elmo_motion_command_csp_t jsd_csp_cmd) override;
void EgdCSV(jsd_elmo_motion_command_csv_t jsd_csv_cmd) override;
void EgdCST(jsd_elmo_motion_command_cst_t jsd_cst_cmd) override;
void EgdSetGainSchedulingMode(jsd_elmo_gain_scheduling_mode_t mode,
uint16_t app_id) override;
void EgdSetGainSchedulingIndex(uint16_t index) override;

Expand Down
Loading

0 comments on commit 99bcbb6

Please sign in to comment.