void AP_Compass_PX4::_compass_timer(uint32_t now)
{
// try to accumulate samples at 100Hz
if (now - _last_timer < 10000) {
return;
}
_last_timer = hal.scheduler->micros();
_accumulate();
}
bool AP_Compass_PX4::read(void)
{
hal.scheduler->suspend_timer_procs();
// try to accumulate one more sample, so we have the latest data
_accumulate();
// consider the compass healthy if we got a reading in the last 0.2s
healthy = (hrt_absolute_time() - _last_timestamp < 200000);
if (!healthy || _count == 0) {
hal.scheduler->resume_timer_procs();
return healthy;
}
_sum /= _count;
_sum *= 1000;
_sum.rotate(_orientation);
_sum.rotate(_board_orientation);
_sum += _offset.get();
// apply motor compensation
if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
_sum += _motor_offset;
} else {
_motor_offset.x = 0;
_motor_offset.y = 0;
_motor_offset.z = 0;
}
mag_x = _sum.x;
mag_y = _sum.y;
mag_z = _sum.z;
_sum.zero();
_count = 0;
hal.scheduler->resume_timer_procs();
last_update = _last_timestamp;
return true;
}
// Read the sensor
uint8_t AP_Baro_VRBRAIN::read(void)
{
// try to accumulate one more sample, so we have the latest data
_accumulate();
// consider the baro healthy if we got a reading in the last 0.2s
_flags.healthy = (hrt_absolute_time() - _last_timestamp < 200000);
if (!_flags.healthy || _sum_count == 0) {
return _flags.healthy;
}
_pressure = (_pressure_sum / _sum_count) * 100.0f;
_temperature = _temperature_sum / _sum_count;
_pressure_samples = _sum_count;
_last_update = (uint32_t)_last_timestamp/1000;
_pressure_sum = 0;
_temperature_sum = 0;
_sum_count = 0;
return 1;
}
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_VRBRAIN::init(void)
{
if (_baro_fd <= 0) {
_baro_fd = open(BARO_DEVICE_PATH, O_RDONLY);
if (_baro_fd < 0) {
hal.scheduler->panic("Unable to open " BARO_DEVICE_PATH);
}
/* set the driver to poll at 150Hz */
ioctl(_baro_fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX);
// average over up to 20 samples
ioctl(_baro_fd, SENSORIOCSQUEUEDEPTH, 20);
// give the timer a chance to run and gather one sample
hal.scheduler->delay(40);
_accumulate();
}
return true;
}
bool AP_InertialSensor_Flymaple::sample_available(void)
{
_accumulate();
return min(_accel_samples, _gyro_samples) / _sample_divider > 0;
}
