int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
{
/* Check if calibration values are still up-to-date. */
bool updated;
orb_check(_param_update_sub, &updated);
if (updated) {
parameter_update_s parameter_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update);
_update_accel_calibration();
_update_gyro_calibration();
_update_mag_calibration();
}
math::Vector<3> vec_integrated_unused;
uint64_t integral_dt_unused;
math::Vector<3> accel_val((data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale,
(data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale,
(data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale);
// apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val;
_accel_int.put_with_interval(data.fifo_sample_interval_us,
accel_val,
vec_integrated_unused,
integral_dt_unused);
math::Vector<3> gyro_val(data.gyro_rad_s_x,
data.gyro_rad_s_y,
data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
// Apply calibration after rotation.
gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
_gyro_int.put_with_interval(data.fifo_sample_interval_us,
gyro_val,
vec_integrated_unused,
integral_dt_unused);
// If we are not receiving the last sample from the FIFO buffer yet, let's stop here
// and wait for more packets.
if (!data.is_last_fifo_sample) {
return 0;
}
// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
// Therefore, only publish every forth time.
++_publish_count;
if (_publish_count < 4) {
return 0;
}
_publish_count = 0;
// Update all the counters.
perf_set_count(_read_counter, data.read_counter);
perf_set_count(_error_counter, data.error_counter);
perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
if (_mag_enabled) {
perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter);
}
perf_begin(_publish_perf);
accel_report accel_report = {};
gyro_report gyro_report = {};
mag_report mag_report = {};
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
if (_mag_enabled) {
mag_report.timestamp = accel_report.timestamp;
}
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.range_rad_s = -1.0f;
gyro_report.device_id = m_id.dev_id;
accel_report.scaling = -1.0f;
accel_report.range_m_s2 = -1.0f;
accel_report.device_id = m_id.dev_id;
if (_mag_enabled) {
mag_report.scaling = -1.0f;
mag_report.range_ga = -1.0f;
mag_report.device_id = m_id.dev_id;
}
// TODO: remove these (or get the values)
gyro_report.x_raw = NAN;
gyro_report.y_raw = NAN;
gyro_report.z_raw = NAN;
accel_report.x_raw = NAN;
accel_report.y_raw = NAN;
accel_report.z_raw = NAN;
if (_mag_enabled) {
mag_report.x_raw = NAN;
mag_report.y_raw = NAN;
mag_report.z_raw = NAN;
}
math::Vector<3> gyro_val_filt;
math::Vector<3> accel_val_filt;
// Read and reset.
math::Vector<3> gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
math::Vector<3> accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
// Use the filtered (by integration) values to get smoother / less noisy data.
gyro_report.x = gyro_val_filt(0);
gyro_report.y = gyro_val_filt(1);
gyro_report.z = gyro_val_filt(2);
accel_report.x = accel_val_filt(0);
accel_report.y = accel_val_filt(1);
accel_report.z = accel_val_filt(2);
if (_mag_enabled) {
math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
mag_val = _rotation_matrix * mag_val;
mag_report.x = mag_val(0);
mag_report.y = mag_val(1);
mag_report.z = mag_val(2);
}
gyro_report.x_integral = gyro_val_integ(0);
gyro_report.y_integral = gyro_val_integ(1);
gyro_report.z_integral = gyro_val_integ(2);
accel_report.x_integral = accel_val_integ(0);
accel_report.y_integral = accel_val_integ(1);
accel_report.z_integral = accel_val_integ(2);
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
if (_gyro_topic != nullptr) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
if (_mag_enabled) {
if (_mag_topic == nullptr) {
_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
&_mag_orb_class_instance, ORB_PRIO_LOW);
} else {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
}
/* Notify anyone waiting for data. */
DevMgr::updateNotify(*this);
// Report if there are high vibrations, every 10 times it happens.
const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
// Report every 5s.
const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
if (threshold_reached && due_to_report) {
mavlink_log_critical(&_mavlink_log_pub,
"High accelerations, range exceeded %llu times",
data.accel_range_hit_counter);
_last_accel_range_hit_time = hrt_absolute_time();
_last_accel_range_hit_count = data.accel_range_hit_counter;
}
}
perf_end(_publish_perf);
// TODO: check the return codes of this function
return 0;
};
int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
{
/* Check if calibration values are still up-to-date. */
bool updated;
orb_check(_param_update_sub, &updated);
if (updated) {
parameter_update_s parameter_update;
orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update);
_update_accel_calibration();
_update_gyro_calibration();
}
matrix::Vector3f vec_integrated_unused;
uint32_t integral_dt_unused;
matrix::Vector3f accel_val(data.accel_m_s2_x,
data.accel_m_s2_y,
data.accel_m_s2_z);
// apply sensor rotation on the accel measurement
accel_val = _rotation_matrix * accel_val;
// Apply calibration after rotation
accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale;
accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale;
accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale;
_accel_int.put_with_interval(data.fifo_sample_interval_us,
accel_val,
vec_integrated_unused,
integral_dt_unused);
matrix::Vector3f gyro_val(data.gyro_rad_s_x,
data.gyro_rad_s_y,
data.gyro_rad_s_z);
// apply sensor rotation on the gyro measurement
gyro_val = _rotation_matrix * gyro_val;
// Apply calibration after rotation
gyro_val(0) = (gyro_val(0) - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
gyro_val(1) = (gyro_val(1) - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
gyro_val(2) = (gyro_val(2) - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
_gyro_int.put_with_interval(data.fifo_sample_interval_us,
gyro_val,
vec_integrated_unused,
integral_dt_unused);
// The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz.
// Therefore, only publish every forth time.
++_publish_count;
if (_publish_count < 4) {
return 0;
}
_publish_count = 0;
// Update all the counters.
perf_set_count(_read_counter, data.read_counter);
perf_set_count(_error_counter, data.error_counter);
perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter);
perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter);
perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter);
perf_begin(_publish_perf);
sensor_accel_s accel_report = {};
sensor_gyro_s gyro_report = {};
mag_report mag_report = {};
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
mag_report.timestamp = accel_report.timestamp;
mag_report.is_external = false;
// TODO: get these right
gyro_report.scaling = -1.0f;
gyro_report.device_id = m_id.dev_id;
accel_report.scaling = -1.0f;
accel_report.device_id = m_id.dev_id;
if (_mag_enabled) {
mag_report.scaling = -1.0f;
mag_report.device_id = m_id.dev_id;
}
// TODO: remove these (or get the values)
gyro_report.x_raw = 0;
gyro_report.y_raw = 0;
gyro_report.z_raw = 0;
accel_report.x_raw = 0;
accel_report.y_raw = 0;
accel_report.z_raw = 0;
if (_mag_enabled) {
mag_report.x_raw = 0;
mag_report.y_raw = 0;
mag_report.z_raw = 0;
}
matrix::Vector3f gyro_val_filt;
matrix::Vector3f accel_val_filt;
// Read and reset.
matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt);
matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt);
// Use the filtered (by integration) values to get smoother / less noisy data.
gyro_report.x = gyro_val_filt(0);
gyro_report.y = gyro_val_filt(1);
gyro_report.z = gyro_val_filt(2);
accel_report.x = accel_val_filt(0);
accel_report.y = accel_val_filt(1);
accel_report.z = accel_val_filt(2);
if (_mag_enabled) {
matrix::Vector3f mag_val(data.mag_ga_x,
data.mag_ga_y,
data.mag_ga_z);
mag_val = _rotation_matrix * mag_val;
mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_val(2) = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
mag_report.x = mag_val(0);
mag_report.y = mag_val(1);
mag_report.z = mag_val(2);
}
gyro_report.x_integral = gyro_val_integ(0);
gyro_report.y_integral = gyro_val_integ(1);
gyro_report.z_integral = gyro_val_integ(2);
accel_report.x_integral = accel_val_integ(0);
accel_report.y_integral = accel_val_integ(1);
accel_report.z_integral = accel_val_integ(2);
// TODO: when is this ever blocked?
if (!(m_pub_blocked)) {
if (_gyro_topic != nullptr) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
}
if (_mag_topic != nullptr) {
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
}
// Report if there are high vibrations, every 10 times it happens.
const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10);
// Report every 5s.
const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000);
if (threshold_reached && due_to_report) {
mavlink_log_critical(&_mavlink_log_pub,
"High accelerations, range exceeded %llu times",
data.accel_range_hit_counter);
_last_accel_range_hit_time = hrt_absolute_time();
_last_accel_range_hit_count = data.accel_range_hit_counter;
}
}
perf_end(_publish_perf);
// TODO: check the return codes of this function
return 0;
};
