Skip to content

Commit 09350d1

Browse files
authored
Merge pull request #130 from nasa-jpl/dloret-commutation-search-support
Adds ability to modify the timeout in Profile Disengaging states
2 parents 7ccb32b + d9886e0 commit 09350d1

File tree

6 files changed

+72
-18
lines changed

6 files changed

+72
-18
lines changed

src/fcgen/fastcat_types.yaml

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -851,6 +851,11 @@ commands:
851851
- name: position
852852
type: double
853853

854+
- name: actuator_set_prof_disengaging_timeout
855+
fields:
856+
- name: timeout
857+
type: double
858+
854859
- name: actuator_calibrate
855860
fields:
856861
- name: velocity

src/jsd/actuator.cc

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -255,6 +255,14 @@ bool fastcat::Actuator::Write(DeviceCmd& cmd)
255255
return true;
256256
break;
257257

258+
case ACTUATOR_SET_PROF_DISENGAGING_TIMEOUT_CMD:
259+
if (!HandleNewSetProfDisengagingTimeoutCmd(cmd)) {
260+
ERROR("Failed to handle Set Profile Disengaging Timeout Command");
261+
return false;
262+
}
263+
return true;
264+
break;
265+
258266
case ACTUATOR_SET_DIGITAL_OUTPUT_CMD:
259267
ElmoSetDigitalOutput(
260268
cmd.actuator_set_digital_output_cmd.digital_output_index,

src/jsd/actuator.h

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -138,6 +138,14 @@ class Actuator : public JsdDeviceBase
138138

139139
DeviceCmd last_cmd_ = {};
140140

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

src/jsd/actuator_fsm_helpers.cc

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -345,6 +345,41 @@ bool fastcat::Actuator::HandleNewSetUnitModeCmd(const DeviceCmd& cmd)
345345
return true;
346346
}
347347

348+
bool fastcat::Actuator::HandleNewSetProfDisengagingTimeoutCmd(
349+
const DeviceCmd& cmd)
350+
{
351+
switch (actuator_sms_) {
352+
case ACTUATOR_SMS_FAULTED:
353+
case ACTUATOR_SMS_HALTED:
354+
case ACTUATOR_SMS_HOLDING:
355+
break;
356+
357+
case ACTUATOR_SMS_PROF_POS:
358+
case ACTUATOR_SMS_PROF_POS_DISENGAGING:
359+
case ACTUATOR_SMS_PROF_VEL:
360+
case ACTUATOR_SMS_PROF_VEL_DISENGAGING:
361+
case ACTUATOR_SMS_PROF_TORQUE:
362+
case ACTUATOR_SMS_PROF_TORQUE_DISENGAGING:
363+
case ACTUATOR_SMS_CS:
364+
case ACTUATOR_SMS_CAL_MOVE_TO_HARDSTOP:
365+
case ACTUATOR_SMS_CAL_AT_HARDSTOP:
366+
case ACTUATOR_SMS_CAL_MOVE_TO_SOFTSTOP:
367+
ERROR("Act %s: %s", name_.c_str(),
368+
"Cannot set profile disengaging timeout, motion command is active");
369+
fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_INVALID_CMD_DURING_MOTION;
370+
return false;
371+
372+
default:
373+
ERROR("Act %s: %s: %d", name_.c_str(), "Bad Act State ", actuator_sms_);
374+
return false;
375+
}
376+
377+
prof_disengaging_timeout_ =
378+
cmd.actuator_set_prof_disengaging_timeout_cmd.timeout;
379+
380+
return true;
381+
}
382+
348383
bool fastcat::Actuator::HandleNewCalibrationCmd(const DeviceCmd& cmd)
349384
{
350385
switch (actuator_sms_) {

src/jsd/gold_actuator.cc

Lines changed: 12 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -344,11 +344,10 @@ fastcat::FaultType fastcat::GoldActuator::ProcessProfPosDisengaging()
344344

345345
ElmoCSP(jsd_cmd);
346346

347-
// Check runout timer here, brake engage/disengage time cannot exceed 1
348-
// second per MAN-G-CR Section BP - Brake Parameters
349-
if ((cycle_mono_time_ - last_transition_time_) > (1.0 + 2 * loop_period_)) {
350-
ERROR("Act %s: Brake Disengage 1.0 sec runout timer expired, faulting",
351-
name_.c_str());
347+
if ((cycle_mono_time_ - last_transition_time_) >
348+
(prof_disengaging_timeout_ + 2 * loop_period_)) {
349+
ERROR("Act %s: Brake Disengage %lf sec timeout expired, faulting",
350+
name_.c_str(), prof_disengaging_timeout_);
352351
fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_BRAKE_DISENGAGE_TIMEOUT_EXCEEDED;
353352
return ALL_DEVICE_FAULT;
354353
}
@@ -387,11 +386,10 @@ fastcat::FaultType fastcat::GoldActuator::ProcessProfVelDisengaging()
387386

388387
ElmoCSV(jsd_cmd);
389388

390-
// Check runout timer here, brake engage/disengage time cannot exceed 1
391-
// second per MAN-G-CR Section BP - Brake Parameters
392-
if ((cycle_mono_time_ - last_transition_time_) > (1.0 + 2 * loop_period_)) {
393-
ERROR("Act %s: Brake Disengage 1.0 sec runout timer expired, faulting",
394-
name_.c_str());
389+
if ((cycle_mono_time_ - last_transition_time_) >
390+
(prof_disengaging_timeout_ + 2 * loop_period_)) {
391+
ERROR("Act %s: Brake Disengage %lf sec timeout expired, faulting",
392+
name_.c_str(), prof_disengaging_timeout_);
395393
fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_BRAKE_DISENGAGE_TIMEOUT_EXCEEDED;
396394
return ALL_DEVICE_FAULT;
397395
}
@@ -428,11 +426,10 @@ fastcat::FaultType fastcat::GoldActuator::ProcessProfTorqueDisengaging()
428426

429427
ElmoCST(jsd_cmd);
430428

431-
// Check runout timer here, brake engage/disengage time cannot exceed 1
432-
// second per MAN-G-CR Section BP - Brake Parameters
433-
if ((cycle_mono_time_ - last_transition_time_) > (1.0 + 2 * loop_period_)) {
434-
ERROR("Act %s: Brake Disengage 1.0 sec runout timer expired, faulting",
435-
name_.c_str());
429+
if ((cycle_mono_time_ - last_transition_time_) >
430+
(prof_disengaging_timeout_ + 2 * loop_period_)) {
431+
ERROR("Act %s: Brake Disengage %lf sec timeout expired, faulting",
432+
name_.c_str(), prof_disengaging_timeout_);
436433
fastcat_fault_ = ACTUATOR_FASTCAT_FAULT_BRAKE_DISENGAGE_TIMEOUT_EXCEEDED;
437434
return ALL_DEVICE_FAULT;
438435
}

src/jsd/platinum_actuator.cc

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -199,12 +199,12 @@ fastcat::FaultType fastcat::PlatinumActuator::ProcessProfPosDisengaging()
199199
// Transition to ACTUATOR_SMS_PROF_POS once the drive acknowledges reception
200200
// of the command. Otherwise, the state variables used to check for the
201201
// completion of the command (i.e. target_reached) might refer to a previous
202-
// command. If the drive does not acknowledge the command within 1 second,
203-
// fault.
202+
// command. If the drive does not acknowledge the command within the
203+
// configured timeout, fault.
204204
if (state_->platinum_actuator_state.setpoint_ack_rise) {
205205
TransitionToState(ACTUATOR_SMS_PROF_POS);
206206
} else if ((cycle_mono_time_ - last_transition_time_) >
207-
(1.0 + 2.0 * loop_period_)) {
207+
(prof_disengaging_timeout_ + 2.0 * loop_period_)) {
208208
ERROR(
209209
"Act %s: Profiled Position command was not acknowledged by drive "
210210
"before timeout, faulting",

0 commit comments

Comments
 (0)