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; };