/*
* BusEvent
*/
bool BusEvent::wait(uavcan::MonotonicDuration duration)
{
static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
const uavcan::int64_t msec = duration.toMSec();
msg_t ret = msg_t();
if (msec <= 0)
{
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout(TIME_IMMEDIATE);
# else // ChibiOS 3
ret = sem_.wait(TIME_IMMEDIATE);
# endif
}
else
{
# if (CH_KERNEL_MAJOR == 2)
ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
# else // ChibiOS 3
ret = sem_.wait((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
# endif
}
# if (CH_KERNEL_MAJOR == 2)
return ret == RDY_OK;
# else // ChibiOS 3
return ret == MSG_OK;
# endif
}
msg_t main() override
{
zubax_chibios::watchdog::Timer wdt;
wdt.startMSec(1000);
setName("mag");
::usleep(500000); // Startup delay
wdt.reset();
node::markComponentInitialized(node::ComponentID::Magnetometer);
while (!tryInit() && !node::hasPendingRestartRequest())
{
setStatus(uavcan::protocol::NodeStatus::HEALTH_ERROR);
lowsyslog("Mag init failed, will retry...\n");
::usleep(500000);
wdt.reset();
}
wdt.reset();
const float variance = param_variance.get();
const uint64_t period_usec = param_period_usec.get();
systime_t sleep_until = chibios_rt::System::getTime();
while (!node::hasPendingRestartRequest())
{
sleep_until += US2ST(period_usec);
float vector[3] = {0, 0, 0};
if (tryRead(vector))
{
transformToNEDFrame(vector);
publish(vector, variance);
setStatus(estimateStatusFromMeasurement(vector));
}
else
{
setStatus(uavcan::protocol::NodeStatus::HEALTH_ERROR);
}
sysSleepUntilChTime(sleep_until);
wdt.reset();
}
lowsyslog("Mag driver terminated\n");
return msg_t();
}
T get ()
{
if (this->first_node_ == nullptr)
return msg_t();
container<T>* item = this->first_node_;
this->first_node_ = item->next_;
this->llist_size--;
T value = item->current_;
delete item;
return value;
}
/*
* BusEvent
*/
bool BusEvent::wait(uavcan::MonotonicDuration duration)
{
static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
const uavcan::int64_t msec = duration.toMSec();
msg_t ret = msg_t();
if (msec <= 0)
{
ret = sem_.waitTimeout(TIME_IMMEDIATE);
}
else
{
ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
}
return ret == RDY_OK;
}