Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 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/accelerometer/PX4Accelerometer.cpp
+22 -4
View File
@@ -65,6 +65,7 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit
PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
CDev(nullptr),
ModuleParams(nullptr),
_sensor_pub{ORB_ID(sensor_accel), priority},
_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
_sensor_integrated_pub{ORB_ID(sensor_accel_integrated), priority},
@@ -77,6 +78,11 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R
_sensor_pub.advertise();
_sensor_integrated_pub.advertise();
_sensor_status_pub.advertise();
updateParams();
// set reasonable default, driver should be setting real value
set_update_rate(800);
}
PX4Accelerometer::~PX4Accelerometer()
@@ -128,11 +134,21 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
void PX4Accelerometer::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 PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
@@ -383,10 +399,12 @@ void PX4Accelerometer::UpdateVibrationMetrics(const Vector3f &delta_velocity)
void PX4Accelerometer::print_status()
{
#if !defined(CONSTRAINED_FLASH)
PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
(double)_calibration_scale(2));
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/accelerometer/PX4Accelerometer.hpp
+7 -2
View File
@@ -39,6 +39,7 @@
#include <lib/conversion/rotation.h>
#include <lib/drivers/device/integrator.h>
#include <lib/ecl/geo/geo.h>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_accel.h>
@@ -46,7 +47,7 @@
#include <uORB/topics/sensor_accel_integrated.h>
#include <uORB/topics/sensor_accel_status.h>
class PX4Accelerometer : public cdev::CDev
class PX4Accelerometer : public cdev::CDev, public ModuleParams
{
public:
PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -98,7 +99,7 @@ private:
hrt_abstime _status_last_publish{0};
Integrator _integrator{2500, false};
Integrator _integrator{5000, false}; // 200 Hz default
matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
@@ -131,4 +132,8 @@ private:
uint8_t _integrator_reset_samples{4};
uint8_t _integrator_samples{0};
uint8_t _integrator_fifo_samples{0};
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
)
};
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
)
};