mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 16:43:34 +08:00
move IMU integration out of drivers to sensors hub to handle accel/gyro sync
- IMU integration move from drivers (PX4Accelerometer/PX4Gyroscope) to sensors/vehicle_imu - sensors: voted_sensors_update now consumes vehicle_imu - delete sensor_accel_integrated, sensor_gyro_integrated - merge sensor_accel_status/sensor_gyro_status into vehicle_imu_status - sensors status output minor improvements (ordering, whitespace, show selected sensor device id and instance)
This commit is contained in:
@@ -43,7 +43,7 @@ namespace sensors
|
||||
|
||||
VehicleAngularVelocity::VehicleAngularVelocity() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
_corrections(this, SensorCorrections::SensorType::Gyroscope)
|
||||
{
|
||||
_lp_filter_velocity.set_cutoff_frequency(kInitialRateHz, _param_imu_gyro_cutoff.get());
|
||||
@@ -68,7 +68,10 @@ bool VehicleAngularVelocity::Start()
|
||||
return false;
|
||||
}
|
||||
|
||||
ScheduleNow();
|
||||
if (!SensorSelectionUpdate(true)) {
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -218,6 +221,9 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
|
||||
|
||||
void VehicleAngularVelocity::Run()
|
||||
{
|
||||
// backup schedule
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
// update corrections first to set _selected_sensor
|
||||
bool selection_updated = SensorSelectionUpdate();
|
||||
|
||||
@@ -311,11 +317,9 @@ void VehicleAngularVelocity::Run()
|
||||
|
||||
void VehicleAngularVelocity::PrintStatus()
|
||||
{
|
||||
PX4_INFO("selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor_sub_index);
|
||||
PX4_INFO("bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
|
||||
|
||||
PX4_INFO("sample rate: %.3f Hz", (double)_update_rate_hz);
|
||||
|
||||
PX4_INFO("selected sensor: %d (%d), rate: %.1f Hz",
|
||||
_selected_sensor_device_id, _selected_sensor_sub_index, (double)_update_rate_hz);
|
||||
PX4_INFO("estimated bias: [%.3f %.3f %.3f]", (double)_bias(0), (double)_bias(1), (double)_bias(2));
|
||||
_corrections.PrintStatus();
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user