fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 08:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

IMU: parameterize IMU integration time (IMU_INTEG_RATE)

Browse Source 
- default integration rate now 200 Hz (5000 us interval)
 - set update rate for all drivers and simulators using PX4Accelerometer/PX4Gyroscope
This commit is contained in:
Daniel Agar
2020-05-05 20:34:09 -04:00
committed by GitHub
parent 682aa700bb
commit ca998c1822
24 changed files with 133 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+19 -4
View File
@@ -80,6 +80,9 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation
_sensor_status_pub.advertise();
updateParams();
// set reasonable default, driver should be setting real value
set_update_rate(800);
}
PX4Gyroscope::~PX4Gyroscope()
@@ -130,11 +133,21 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
void PX4Gyroscope::set_update_rate(uint16_t rate)
{
_update_rate = rate;
const uint32_t update_interval = 1000000 / rate;
_update_rate = math::constrain((int)rate, 50, 32000);
// TODO: set this intelligently
_integrator_reset_samples = 2500 / update_interval;
// constrain IMU integration time 1-20 milliseconds (50-1000 Hz)
int32_t imu_integration_rate_hz = math::constrain(_param_imu_integ_rate.get(), 50, 1000);
if (imu_integration_rate_hz != _param_imu_integ_rate.get()) {
_param_imu_integ_rate.set(imu_integration_rate_hz);
_param_imu_integ_rate.commit_no_notification();
}
const float update_interval_us = 1e6f / _update_rate;
const float imu_integration_interval_us = 1e6f / (float)imu_integration_rate_hz;
_integrator_reset_samples = roundf(imu_integration_interval_us / update_interval_us);
_integrator.set_autoreset_interval(_integrator_reset_samples * update_interval_us);
}
void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
@@ -390,8 +403,10 @@ void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
void PX4Gyroscope::print_status()
{
#if !defined(CONSTRAINED_FLASH)
PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
(double)_calibration_offset(2));
#endif // !CONSTRAINED_FLASH
}
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+3 -2
View File
@@ -100,7 +100,7 @@ private:
hrt_abstime _status_last_publish{0};
Integrator _integrator{2500, true};
Integrator _integrator{5000, true}; // 200 Hz default
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
@@ -135,6 +135,7 @@ private:
uint8_t _integrator_fifo_samples{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max,
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
)
};