Skip to content

Commit

Permalink
Remove debug lines for calibration14 in embObjMotionControl::setCalib…
Browse files Browse the repository at this point in the history 
…rationParametersRaw()

Fix tmestamp in log for temperature errors using relative time from construction of embObjMC device
Change yError to yWarning when failing to read temmperatures
  • Loading branch information
@MSECode
MSECode committed Jan 8, 2025
1 parent 6b76f04 commit d321219
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 29 deletions.
28 changes: 2 additions & 26 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1659,15 +1659,15 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda
{
if(! _temperatureSensorErrorWatchdog.at(motor).isStarted())
{
yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << motor << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
yWarning() << getBoardInfo() << "At time" << (_temperatureSensorErrorWatchdog.at(motor).getAbsoluteTime() - yarp::os::Time::now()) << "In motor" << motor << "cannot read Temperature from I2C. There might be cabling problems, TDB cable might be broken or sensor unreachable";
_temperatureSensorErrorWatchdog.at(motor).start();
}
else
{
_temperatureSensorErrorWatchdog.at(motor).increment();
if( _temperatureSensorErrorWatchdog.at(motor).isExpired())
{
yError()<< getBoardInfo() << "Motor" << motor << "failed to read" << _temperatureSensorErrorWatchdog.at(motor).getCount() << "temperature readings for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds";
yWarning()<< getBoardInfo() << "Motor" << motor << "failed to read" << _temperatureSensorErrorWatchdog.at(motor).getCount() << "temperature readings for" << yarp::os::Time::now() - _temperatureSensorErrorWatchdog.at(motor).getStartTime() << "seconds";
_temperatureSensorErrorWatchdog.at(motor).start();
}
}
Expand Down Expand Up @@ -2313,12 +2313,6 @@ bool embObjMotionControl::setCalibrationParametersRaw(int j, const CalibrationPa
calib.params.type14.offset = (int32_t)S_32(params.param5);
calib.params.type14.calibrationZero = (int32_t)S_32(_measureConverter->posA2E(params.paramZero, j));

yDebug() << "***** calib.params.type14.pwmlimit = " <<calib.params.type14.pwmlimit;
yDebug() << "***** calib.params.type14.final_pos = " <<calib.params.type14.final_pos;
yDebug() << "***** calib.params.type14.invertdirection = " <<calib.params.type14.invertdirection;
yDebug() << "***** calib.params.type14.rotation = " <<calib.params.type14.rotation;
yDebug() << "***** calib.params.type14.offset = " <<calib.params.type14.offset;
yDebug() << "***** calib.params.type14.calibrationZero = " <<calib.params.type14.calibrationZero;
break;

default:
Expand Down Expand Up @@ -4895,7 +4889,6 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val)
eOmc_motor_status_basic_t status;
eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_status_basic);

// if (_temperatureSensorsVector.at(m) == nullptr || (_temperatureSensorsVector.at(motor)->getType() == motor_temperature_sensor_none))
*val = NAN;
if (_temperatureSensorsVector.at(m)->getType() == motor_temperature_sensor_none)
return true;
Expand All @@ -4907,11 +4900,6 @@ bool embObjMotionControl::getTemperatureRaw(int m, double* val)
yError() << getBoardInfo() << "At timestamp" << yarp::os::Time::now() << "In motor" << m << "embObjMotionControl::getTemperatureRaw failed to complete getLocalValue()";
return ret;
}

// if (((double)status.mot_temperature) == temperatureErrorValue_s) //using -5000 as the default value on 2FOC for initializing the temperature. If cannot read from I2C the value cannot be updated
// {
// return false;
// }

*val = _temperatureSensorsVector.at(m)->convertRawToTempCelsius((double)status.mot_temperature);

Expand All @@ -4931,18 +4919,6 @@ bool embObjMotionControl::getTemperaturesRaw(double *vals)

bool embObjMotionControl::getTemperatureLimitRaw(int m, double *temp)
{
// eOprotID32_t protid = eoprot_ID_get(eoprot_endpoint_motioncontrol, eoprot_entity_mc_motor, m, eoprot_tag_mc_motor_config_temperaturelimit);
// uint16_t size;
// eOmeas_temperature_t temperaturelimit = {0};
// *temp = 0;
// if(!askRemoteValue(protid, &temperaturelimit, size))
// {
// yError() << "embObjMotionControl::getTemperatureLimitRaw() can't read temperature limits for" << getBoardInfo() << " motor " << m;
// return false;
// }

// *temp = (double) temperaturelimit;

*temp= _temperatureLimits[m].warningTemperatureLimit;

return true;
Expand Down
8 changes: 5 additions & 3 deletions src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,13 +118,14 @@ class Watchdog
bool _isStarted;
uint32_t _count;
double _time;
double _abstime;
uint32_t _threshold; // use 10000 as limit on the watchdog for the error on the temperature sensor receiving of the values -
// since the ETH callback timing is 2ms by default so using 10000 we can set a checking threshould of 5 second
// in which we can allow the tdb to not respond. If cannot receive response over 1s we trigger the error
public:

Watchdog(): _count(0), _isStarted(false), _threshold(60000), _time(0){;}
Watchdog(uint32_t threshold):_count(0), _isStarted(false), _threshold(threshold), _time(0){;}
Watchdog(): _count(0), _isStarted(false), _threshold(60000), _time(0), _abstime(yarp::os::Time::now()){;}
Watchdog(uint32_t threshold):_count(0), _isStarted(false), _threshold(threshold), _time(0), _abstime(yarp::os::Time::now()){;}
~Watchdog() = default;
Watchdog(const Watchdog& other) = default;
Watchdog(Watchdog&& other) noexcept = default;
Expand All @@ -134,13 +135,14 @@ Watchdog& operator=(Watchdog&& other) noexcept = default;

bool isStarted(){return _isStarted;}
void start() {_count = 0; _time = yarp::os::Time::now(); _isStarted = true;}
bool isExpired() {return (_count > _threshold);}
bool isExpired() {return (_count >= _threshold);}
void increment() {++_count;}
void clear(){_isStarted=false;}
double getStartTime() {return _time;}
uint32_t getCount() {return _count; }
void setThreshold(uint8_t txrateOfRegularROPs){ if(txrateOfRegularROPs != 0) _threshold = _threshold / txrateOfRegularROPs;}
uint32_t getThreshold(){return _threshold;}
double getAbsoluteTime(){return _abstime;}

};

Expand Down

0 comments on commit d321219

Please sign in to comment.