diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index d0ed918409..d46804fdb4 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -302,7 +302,7 @@ void ICM20602::ConfigureGyro() void ICM20602::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index bb3135a80f..b97c3004d5 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -155,7 +155,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index 2aa96bcc44..98cd276779 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -315,7 +315,7 @@ void ICM20608G::ConfigureGyro() void ICM20608G::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index f47254490a..5d16e2cd3e 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -158,7 +158,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.cpp b/src/drivers/imu/invensense/icm20649/ICM20649.cpp index ffa49e6988..7b6089f241 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.cpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.cpp @@ -317,7 +317,7 @@ void ICM20649::ConfigureGyro() void ICM20649::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to ~1 kHz + sample_rate = 800; // default to ~800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/icm20649/ICM20649.hpp b/src/drivers/imu/invensense/icm20649/ICM20649.hpp index d553e2dfc8..7413074821 100644 --- a/src/drivers/imu/invensense/icm20649/ICM20649.hpp +++ b/src/drivers/imu/invensense/icm20649/ICM20649.hpp @@ -169,7 +169,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index 85ea248cca..14aad4fb23 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -315,7 +315,7 @@ void ICM20689::ConfigureGyro() void ICM20689::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index 5be6cbba5d..6762719f09 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -158,7 +158,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp index 333a37b498..c6187ad176 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.cpp @@ -308,7 +308,7 @@ void ICM40609D::ConfigureGyro() void ICM40609D::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp index 336d752248..40e4e23b41 100644 --- a/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp +++ b/src/drivers/imu/invensense/icm40609d/ICM40609D.hpp @@ -158,7 +158,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp index 792917dd12..5716c85b28 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp @@ -310,7 +310,7 @@ void ICM42688P::ConfigureGyro() void ICM42688P::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp index de8547a934..b520e3e6a6 100644 --- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp +++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp @@ -158,7 +158,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index d2b10a69d2..bf4adfd700 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -310,7 +310,7 @@ void MPU6000::ConfigureGyro() void MPU6000::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index a7a3208052..8355b76590 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -155,7 +155,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 21287d6086..6887da69b3 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -310,7 +310,7 @@ void MPU6500::ConfigureGyro() void MPU6500::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 1117fa26ce..21498ecb56 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -155,7 +155,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 52e37b7ff0..11dbc4d176 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -343,7 +343,7 @@ void MPU9250::ConfigureGyro() void MPU9250::ConfigureSampleRate(int sample_rate) { if (sample_rate == 0) { - sample_rate = 1000; // default to 1 kHz + sample_rate = 800; // default to 800 Hz } // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 3048bb7ad6..200fecdcaf 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -167,7 +167,7 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval uint8_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; uint8_t _fifo_accel_samples{static_cast(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))}; diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index 63a4a09d7b..5dc95b84fe 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -124,7 +124,7 @@ void PX4Accelerometer::set_update_rate(uint16_t rate) const uint32_t update_interval = 1000000 / rate; // TODO: set this intelligently - _integrator_reset_samples = 4000 / update_interval; + _integrator_reset_samples = 2500 / update_interval; } void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index 6819ea9433..449cc6d565 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -98,7 +98,7 @@ private: hrt_abstime _status_last_publish{0}; - Integrator _integrator{4000, false}; + Integrator _integrator{2500, false}; matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f}; matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f}; diff --git a/src/lib/drivers/device/integrator.h b/src/lib/drivers/device/integrator.h index 02586aa03d..805476785d 100644 --- a/src/lib/drivers/device/integrator.h +++ b/src/lib/drivers/device/integrator.h @@ -48,7 +48,7 @@ class Integrator { public: - Integrator(uint32_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); + Integrator(uint32_t auto_reset_interval = 2500 /* 400 Hz */, bool coning_compensation = false); ~Integrator() = default; /** diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index b342252962..633738dec1 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -126,7 +126,7 @@ void PX4Gyroscope::set_update_rate(uint16_t rate) const uint32_t update_interval = 1000000 / rate; // TODO: set this intelligently - _integrator_reset_samples = 4000 / update_interval; + _integrator_reset_samples = 2500 / update_interval; } void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index e2785e3d6c..97a1b6ed7c 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -100,7 +100,7 @@ private: hrt_abstime _status_last_publish{0}; - Integrator _integrator{4000, true}; + Integrator _integrator{2500, true}; matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f}; diff --git a/src/lib/ecl b/src/lib/ecl index 47624a0f02..8a9d961f0d 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 47624a0f021e2c187cc4763ad829afe2aa4fb11a +Subproject commit 8a9d961f0d7b0cf6371ab1fcd6d0d2ccb581d3d1 diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36c2c06d72..50de097019 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -318,8 +318,8 @@ MulticopterPositionControl::init() return false; } - // limit to every other vehicle_local_position update (~62.5 Hz) - _local_pos_sub.set_interval_us(16_ms); + // limit to every other vehicle_local_position update (50 Hz) + _local_pos_sub.set_interval_us(20_ms); _time_stamp_last_loop = hrt_absolute_time();