/**
* Adjusts the wall clock.
* Behavior depends on the selected clock adjustment mode - @ref ClockAdjustmentMode.
* Clock adjustment mode can be set only once via constructor.
*
* If the system wide adjustment mode is selected, two ways for performing adjustment exist:
* - Gradual adjustment using adjtime(), if the phase error is less than gradual adjustment limit.
* - Step adjustment using settimeofday(), if the phase error is above gradual adjustment limit.
* The gradual adjustment limit can be configured at any time via the setter method.
*
* @throws uavcan_linux::Exception.
*/
virtual void adjustUtc(const uavcan::UtcDuration adjustment)
{
if (adj_mode_ == ClockAdjustmentMode::PerDriverPrivate)
{
private_adj_ += adjustment;
}
else
{
assert(private_adj_.isZero());
assert(!gradual_adj_limit_.isNegative());
bool success = false;
if (adjustment.getAbs() < gradual_adj_limit_)
{
success = performGradualAdjustment(adjustment);
}
else
{
success = performStepAdjustment(adjustment);
}
if (!success)
{
throw Exception("Clock adjustment failed");
}
}
}
/**
* Sets the maximum phase error to use adjtime().
* If the phase error exceeds this value, settimeofday() will be used instead.
*/
void setGradualAdjustmentLimit(uavcan::UtcDuration limit)
{
if (limit.isNegative())
{
limit = uavcan::UtcDuration();
}
gradual_adj_limit_ = limit;
}
virtual void adjustUtc(uavcan::UtcDuration adjustment)
{
assert(this);
const uint64_t prev_utc = utc;
utc = uint64_t(int64_t(utc) + adjustment.toUSec());
last_adjustment = adjustment;
std::cout << "Clock adjustment " << prev_utc << " --> " << utc << std::endl;
}
bool performGradualAdjustment(const uavcan::UtcDuration adjustment)
{
gradual_adj_cnt_++;
const std::int64_t usec = adjustment.toUSec();
timeval tv;
tv.tv_sec = usec / Int1e6;
tv.tv_usec = usec % Int1e6;
return adjtime(&tv, nullptr) == 0;
}
bool performStepAdjustment(const uavcan::UtcDuration adjustment)
{
step_adj_cnt_++;
const std::int64_t usec = adjustment.toUSec();
timeval tv;
if (gettimeofday(&tv, NULL) != 0)
{
return false;
}
tv.tv_sec += usec / Int1e6;
tv.tv_usec += usec % Int1e6;
return settimeofday(&tv, nullptr) == 0;
}