Vector getPosition() const
{
return I.get();
}
string toString() const
{
ostringstream str;
str<<I.get().toString(3,3)<<" "<<radius;
return str.str();
}
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();
}
accel_report accel_report = {};
gyro_report gyro_report = {};
accel_report.x = (data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale;
accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale;
accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale;
math::Vector<3> accel_val(accel_report.x,
accel_report.y,
accel_report.z);
math::Vector<3> accel_val_integrated_unused;
_accel_int.put_with_interval(data.fifo_sample_interval_us,
accel_val,
accel_val_integrated_unused,
accel_report.integral_dt);
gyro_report.x = (data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
math::Vector<3> gyro_val(gyro_report.x,
gyro_report.y,
gyro_report.z);
math::Vector<3> gyro_val_integrated_unused;
_gyro_int.put_with_interval(data.fifo_sample_interval_us,
gyro_val,
gyro_val_integrated_unused,
gyro_report.integral_dt);
// 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);
perf_begin(_publish_perf);
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
// 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;
// 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;
// Read and reset.
math::Vector<3> gyro_val_integrated = _gyro_int.get(true, gyro_report.integral_dt);
math::Vector<3> accel_val_integrated = _accel_int.get(true, accel_report.integral_dt);
gyro_report.x_integral = gyro_val_integrated(0);
gyro_report.y_integral = gyro_val_integrated(1);
gyro_report.z_integral = gyro_val_integrated(2);
accel_report.x_integral = accel_val_integrated(0);
accel_report.y_integral = accel_val_integrated(1);
accel_report.z_integral = accel_val_integrated(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);
}
/* 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;
};
