From d32121971b5ffad0a39c9da97aaaf71bf97d08da Mon Sep 17 00:00:00 2001 From: Jacopo Date: Fri, 13 Dec 2024 17:19:31 +0100 Subject: [PATCH] Remove debug lines for calibration14 in embObjMotionControl::setCalibrationParametersRaw() Fix tmestamp in log for temperature errors using relative time from construction of embObjMC device Change yError to yWarning when failing to read temmperatures --- .../embObjMotionControl.cpp | 28 ++----------------- .../embObjMotionControl/embObjMotionControl.h | 8 ++++-- 2 files changed, 7 insertions(+), 29 deletions(-) diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp index 18a1a2491b..3e61fb5218 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp @@ -1659,7 +1659,7 @@ 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 @@ -1667,7 +1667,7 @@ bool embObjMotionControl::update(eOprotID32_t id32, double timestamp, void *rxda _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(); } } @@ -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 = " <convertRawToTempCelsius((double)status.mot_temperature); @@ -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; diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h index 015bec8582..93b916ea4e 100644 --- a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h +++ b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h @@ -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; @@ -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;} };