// probe and initialise the sensor
bool AP_Airspeed_I2C::init()
{
_dev = hal.i2c_mgr->get_device(MS4525D0_I2C_BUS, MS4525D0_I2C_ADDR);
// take i2c bus sempahore
if (!_dev || !_dev->get_semaphore()->take(200)) {
return false;
}
// lots of retries during probe
_dev->set_retries(5);
_measure();
hal.scheduler->delay(10);
_collect();
_dev->get_semaphore()->give();
// drop to 2 retries for runtime
_dev->set_retries(2);
if (_last_sample_time_ms != 0) {
_dev->register_periodic_callback(20000,
FUNCTOR_BIND_MEMBER(&AP_Airspeed_I2C::_timer, bool));
return true;
}
void operator()(T& t, const char* d, size_t s)
{
size_t obj_id = _collect( d, s );
if (obj_id!=0xFF)
{
t.get_aspect().template get<N>()( t, &_object[obj_id][0], _object[obj_id].size() );
amf_head_1* ch1 = reinterpret_cast<amf_head_1*>(&_object[obj_id][0]);
_object[obj_id].resize( ch1->head_size() );
}
}
// probe and initialise the sensor
bool AP_Airspeed_I2C::init(void)
{
// get pointer to i2c bus semaphore
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
// take i2c bus sempahore
if (!i2c_sem->take(200))
return false;
_measure();
hal.scheduler->delay(10);
_collect();
i2c_sem->give();
if (_last_sample_time_ms != 0) {
//hal.scheduler->register_timer_process(AP_HAL_MEMBERPROC(&AP_Airspeed_I2C::_timer));
return true;
}
return false;
}
// 1kHz timer
void AP_Airspeed_I2C::_timer(void)
{
AP_HAL::Semaphore* i2c_sem = hal.i2c->get_semaphore();
if (!i2c_sem->take_nonblocking())
return;
if (_measurement_started_ms == 0) {
_measure();
i2c_sem->give();
return;
}
if ((hal.scheduler->millis() - _measurement_started_ms) > 10) {
_collect();
// start a new measurement
_measure();
}
i2c_sem->give();
}
