Skip to content

Commit

Permalink
Merge branch 'master' into devel
Browse files Browse the repository at this point in the history
  • Loading branch information
@pattacini
pattacini committed Jan 8, 2025
2 parents 6b76f04 + 5b2afd2 commit c280dbf
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 c280dbf

Please sign in to comment.