dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

init adjustments to ensure used topics are advertised early (primarily for logging) 

- multi-EKF create each instance as soon as IMU & mag are advertised (before device id populated)
This commit is contained in:
Daniel Agar 2022-03-18 15:04:19 -04:00
parent e6ed595651
commit f4c3084c26
22 changed files with 171 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -223,6 +223,13 @@ rc_update start
manual_control start
sensors start
commander start
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
navigator start
# Try to start the micrortps_client with UDP transport if module exists
@ -252,12 +259,6 @@ then
gyro_calibration start
fi
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
#user defined mavlink streams for instances can be in PATH
. px4-rc.mavlink

14
ROMFS/px4fmu_common/init.d/rcS
View File

@ -408,6 +408,13 @@ else
commander start
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
# Pre-takeoff continuous magnetometer calibration
if param compare -s MBE_ENABLE 1
then
@ -458,13 +465,6 @@ else
fi
fi
#
# Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
#
. ${R}etc/init.d/rc.vehicle_setup
#
# Play the startup tune (if not disabled or there is an error)
#

2
src/drivers/heater/heater.cpp
View File

@ -73,6 +73,8 @@ Heater::Heater() :
}
#endif
_heater_status_pub.advertise();
}
Heater::~Heater()

3
src/lib/mixer_module/mixer_module.cpp
View File

@ -103,6 +103,9 @@ _param_prefix(param_prefix)
}
updateParams();
} else {
_control_allocator_status_pub.advertise();
}
_outputs_pub.advertise();

2
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -206,6 +206,8 @@ AirspeedModule::AirspeedModule():
_perf_elapsed = perf_alloc(PC_ELAPSED, MODULE_NAME": elapsed");
_airspeed_validated_pub.advertise();
_wind_est_pub[0].advertise();
_wind_est_pub[1].advertise();
}
AirspeedModule::~AirspeedModule()

5
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -54,6 +54,11 @@ ControlAllocator::ControlAllocator() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
{
_control_allocator_status_pub.advertise();
_actuator_motors_pub.advertise();
_actuator_servos_pub.advertise();
_actuator_servos_trim_pub.advertise();
for (int i = 0; i < MAX_NUM_MOTORS; ++i) {
char buffer[17];
snprintf(buffer, sizeof(buffer), "CA_R%u_SLEW", i);

2
src/modules/ekf2/CMakeLists.txt
View File

@ -42,6 +42,8 @@ px4_add_module(
#-DDEBUG_BUILD
INCLUDES
EKF
PRIORITY
"SCHED_PRIORITY_MAX - 18" # max priority below high priority WQ threads
STACK_MAX
3600
SRCS

74
src/modules/ekf2/EKF2.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2015-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -164,6 +164,18 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_synthetic_mag_z(_params->synthesize_mag_z),
_param_ekf2_gsf_tas_default(_params->EKFGSF_tas_default)
{
// advertise expected minimal topic set immediately to ensure logging
_attitude_pub.advertise();
_local_position_pub.advertise();
_estimator_event_flags_pub.advertise();
_estimator_innovation_test_ratios_pub.advertise();
_estimator_innovation_variances_pub.advertise();
_estimator_innovations_pub.advertise();
_estimator_sensor_bias_pub.advertise();
_estimator_states_pub.advertise();
_estimator_status_flags_pub.advertise();
_estimator_status_pub.advertise();
}
EKF2::~EKF2()
@ -183,13 +195,7 @@ EKF2::~EKF2()
bool EKF2::multi_init(int imu, int mag)
{
// advertise immediately to ensure consistent uORB instance numbering
_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
_wind_pub.advertise();
// advertise all topics to ensure consistent uORB instance numbering
_ekf2_timestamps_pub.advertise();
_estimator_baro_bias_pub.advertise();
_estimator_event_flags_pub.advertise();
@ -205,6 +211,12 @@ bool EKF2::multi_init(int imu, int mag)
_estimator_visual_odometry_aligned_pub.advertise();
_yaw_est_pub.advertise();
_attitude_pub.advertise();
_local_position_pub.advertise();
_global_position_pub.advertise();
_odometry_pub.advertise();
_wind_pub.advertise();
bool changed_instance = _vehicle_imu_sub.ChangeInstance(imu) && _magnetometer_sub.ChangeInstance(mag);
const int status_instance = _estimator_states_pub.get_instance();
@ -320,8 +332,7 @@ void EKF2::Run()
}
if (!_callback_registered) {
PX4_WARN("%d - failed to register callback, retrying", _instance);
ScheduleDelayed(1_s);
ScheduleDelayed(10_ms);
return;
}
}
@ -593,6 +604,9 @@ void EKF2::Run()
// publish ekf2_timestamps
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
// re-schedule as backup timeout
ScheduleDelayed(100_ms);
}
void EKF2::PublishAttitude(const hrt_abstime &timestamp)
@ -692,11 +706,19 @@ void EKF2::PublishEventFlags(const hrt_abstime &timestamp)
event_flags.emergency_yaw_reset_gps_yaw_stopped = _ekf.warning_event_flags().emergency_yaw_reset_gps_yaw_stopped;
event_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.publish(event_flags);
}
_estimator_event_flags_pub.update(event_flags);
_ekf.clear_information_events();
_ekf.clear_warning_events();
_last_event_flags_publish = event_flags.timestamp;
_ekf.clear_information_events();
_ekf.clear_warning_events();
} else if ((_last_event_flags_publish != 0) && (timestamp >= _last_event_flags_publish + 1_s)) {
// continue publishing periodically
_estimator_event_flags_pub.get().timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_event_flags_pub.update();
_last_event_flags_publish = _estimator_event_flags_pub.get().timestamp;
}
}
void EKF2::PublishGlobalPosition(const hrt_abstime &timestamp)
@ -1198,7 +1220,7 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
{
// publish at ~ 1 Hz (or immediately if filter control status or fault status changes)
bool update = (hrt_elapsed_time(&_last_status_flag_update) >= 1_s);
bool update = (timestamp >= _last_status_flags_publish + 1_s);
// filter control status
if (_ekf.control_status().value != _filter_control_status) {
@ -1296,7 +1318,7 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
status_flags.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_status_flags_pub.publish(status_flags);
_last_status_flag_update = status_flags.timestamp;
_last_status_flags_publish = status_flags.timestamp;
}
}
@ -2027,9 +2049,7 @@ int EKF2::task_spawn(int argc, char *argv[])
vehicle_mag_sub.update();
// Mag & IMU data must be valid, first mag can be ignored initially
if ((vehicle_mag_sub.get().device_id != 0 || mag == 0)
&& (vehicle_imu_sub.get().accel_device_id != 0)
&& (vehicle_imu_sub.get().gyro_device_id != 0)) {
if ((vehicle_mag_sub.advertised() || mag == 0) && (vehicle_imu_sub.advertised())) {
if (!ekf2_instance_created[imu][mag]) {
EKF2 *ekf2_inst = new EKF2(true, px4::ins_instance_to_wq(imu), false);
@ -2043,17 +2063,11 @@ int EKF2::task_spawn(int argc, char *argv[])
multi_instances_allocated++;
ekf2_instance_created[imu][mag] = true;
if (actual_instance == 0) {
// force selector to run immediately if first instance started
_ekf2_selector.load()->ScheduleNow();
}
PX4_DEBUG("starting instance %d, IMU:%" PRIu8 " (%" PRIu32 "), MAG:%" PRIu8 " (%" PRIu32 ")", actual_instance,
imu, vehicle_imu_sub.get().accel_device_id,
mag, vehicle_mag_sub.get().device_id);
// sleep briefly before starting more instances
px4_usleep(10000);
_ekf2_selector.load()->ScheduleNow();
} else {
PX4_ERR("instance numbering problem instance: %d", actual_instance);
@ -2063,20 +2077,20 @@ int EKF2::task_spawn(int argc, char *argv[])
} else {
PX4_ERR("alloc and init failed imu: %" PRIu8 " mag:%" PRIu8, imu, mag);
px4_usleep(1000000);
px4_usleep(100000);
break;
}
}
} else {
px4_usleep(50000); // give the sensors extra time to start
continue;
px4_usleep(1000); // give the sensors extra time to start
break;
}
}
}
if (multi_instances_allocated < multi_instances) {
px4_usleep(100000);
px4_usleep(10000);
}
}

6
src/modules/ekf2/EKF2.hpp
View File

@ -267,7 +267,9 @@ private:
bool _callback_registered{false};
hrt_abstime _last_status_flag_update{0};
hrt_abstime _last_event_flags_publish{0};
hrt_abstime _last_status_flags_publish{0};
hrt_abstime _last_range_sensor_update{0};
uint32_t _filter_control_status{0};
@ -282,7 +284,7 @@ private:
uORB::PublicationMulti<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
uORB::PublicationMulti<estimator_baro_bias_s> _estimator_baro_bias_pub{ORB_ID(estimator_baro_bias)};
uORB::PublicationMulti<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMultiData<estimator_event_flags_s> _estimator_event_flags_pub{ORB_ID(estimator_event_flags)};
uORB::PublicationMulti<estimator_gps_status_s> _estimator_gps_status_pub{ORB_ID(estimator_gps_status)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
uORB::PublicationMulti<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};

22
src/modules/ekf2/EKF2Selector.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -43,6 +43,13 @@ EKF2Selector::EKF2Selector() :
ModuleParams(nullptr),
ScheduledWorkItem("ekf2_selector", px4::wq_configurations::nav_and_controllers)
{
_estimator_selector_status_pub.advertise();
_sensor_selection_pub.advertise();
_vehicle_attitude_pub.advertise();
_vehicle_global_position_pub.advertise();
_vehicle_local_position_pub.advertise();
_vehicle_odometry_pub.advertise();
_wind_pub.advertise();
}
EKF2Selector::~EKF2Selector()
@ -50,12 +57,6 @@ EKF2Selector::~EKF2Selector()
Stop();
}
bool EKF2Selector::Start()
{
ScheduleNow();
return true;
}
void EKF2Selector::Stop()
{
for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
@ -673,9 +674,6 @@ void EKF2Selector::PublishWindEstimate()
void EKF2Selector::Run()
{
// re-schedule as backup timeout
ScheduleDelayed(FILTER_UPDATE_PERIOD);
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
@ -703,6 +701,7 @@ void EKF2Selector::Run()
// if still invalid return early and check again on next scheduled run
if (_selected_instance == INVALID_INSTANCE) {
ScheduleDelayed(100_ms);
return;
}
}
@ -803,6 +802,9 @@ void EKF2Selector::Run()
PublishVehicleGlobalPosition();
PublishVehicleOdometry();
PublishWindEstimate();
// re-schedule as backup timeout
ScheduleDelayed(FILTER_UPDATE_PERIOD);
}
void EKF2Selector::PublishEstimatorSelectorStatus()

1
src/modules/ekf2/EKF2Selector.hpp
View File

@ -70,7 +70,6 @@ public:
EKF2Selector();
~EKF2Selector() override;
bool Start();
void Stop();
void PrintStatus();

2
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -70,6 +70,8 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
// limit to 50 Hz
_local_pos_sub.set_interval_ms(20);
_pos_ctrl_status_pub.advertise();
_pos_ctrl_landing_status_pub.advertise();
_tecs_status_pub.advertise();
_slew_rate_airspeed.setSlewRate(ASPD_SP_SLEW_RATE);

18
src/modules/logger/logged_topics.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -120,10 +120,10 @@ void LoggedTopics::add_default_topics()
// multi topics
add_optional_topic_multi("actuator_outputs", 100, 3);
add_topic_multi("airspeed_wind", 1000, 4);
add_topic_multi("control_allocator_status", 200, 2);
add_optional_topic_multi("airspeed_wind", 1000, 4);
add_optional_topic_multi("control_allocator_status", 200, 2);
add_optional_topic_multi("rate_ctrl_status", 200, 2);
add_topic_multi("sensor_hygrometer", 500, 4);
add_optional_topic_multi("sensor_hygrometer", 500, 4);
add_optional_topic_multi("rpm", 200);
add_optional_topic_multi("telemetry_status", 1000, 4);
@ -132,7 +132,7 @@ void LoggedTopics::add_default_topics()
static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 1;
#else
static constexpr uint8_t MAX_ESTIMATOR_INSTANCES = 6; // artificially limited until PlotJuggler fixed
add_topic("estimator_selector_status");
add_optional_topic("estimator_selector_status");
add_optional_topic_multi("estimator_attitude", 500, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_global_position", 1000, MAX_ESTIMATOR_INSTANCES);
add_optional_topic_multi("estimator_local_position", 500, MAX_ESTIMATOR_INSTANCES);
@ -440,7 +440,7 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins
if (_subscriptions.sub[j].id == static_cast<ORB_ID>(topics[i]->o_id) &&
_subscriptions.sub[j].instance == instance) {
PX4_DEBUG("logging topic %s(%" PRUu8 "), interval: %" PRUu16 ", already added, only setting interval",
PX4_DEBUG("logging topic %s(%" PRIu8 "), interval: %" PRIu16 ", already added, only setting interval",
topics[i]->o_name, instance, interval_ms);
_subscriptions.sub[j].interval_ms = interval_ms;
@ -452,7 +452,11 @@ bool LoggedTopics::add_topic(const char *name, uint16_t interval_ms, uint8_t ins
if (!already_added) {
success = add_topic(topics[i], interval_ms, instance, optional);
PX4_DEBUG("logging topic: %s(%" PRUu8 "), interval: %" PRUu16, topics[i]->o_name, instance, interval_ms);
if (success) {
PX4_DEBUG("logging topic: %s(%" PRIu8 "), interval: %" PRIu16, topics[i]->o_name, instance, interval_ms);
}
break;
}
}

76
src/modules/sensors/sensors.cpp
View File

@ -233,6 +233,11 @@ Sensors::Sensors(bool hil_enabled) :
_loop_perf(perf_alloc(PC_ELAPSED, "sensors")),
_voted_sensors_update(hil_enabled, _vehicle_imu_sub)
{
_sensor_pub.advertise();
_vehicle_angular_velocity.Start();
_vehicle_acceleration.Start();
/* Differential pressure offset */
_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL
@ -250,11 +255,17 @@ Sensors::Sensors(bool hil_enabled) :
param_find("SYS_CAL_TMAX");
param_find("SYS_CAL_TMIN");
_sensor_combined.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
_airspeed_validator.set_timeout(300000);
_airspeed_validator.set_equal_value_threshold(100);
_vehicle_acceleration.Start();
_vehicle_angular_velocity.Start();
parameters_update();
InitializeVehicleAirData();
InitializeVehicleGPSPosition();
InitializeVehicleIMU();
InitializeVehicleMagnetometer();
}
Sensors::~Sensors()
@ -369,6 +380,10 @@ int Sensors::parameters_update()
}
}
InitializeVehicleAirData();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
return PX4_OK;
}
@ -569,14 +584,9 @@ void Sensors::InitializeVehicleIMU()
if (_vehicle_imu_list[i] == nullptr) {
uORB::Subscription accel_sub{ORB_ID(sensor_accel), i};
sensor_accel_s accel{};
accel_sub.copy(&accel);
uORB::Subscription gyro_sub{ORB_ID(sensor_gyro), i};
sensor_gyro_s gyro{};
gyro_sub.copy(&gyro);
if (accel.device_id > 0 && gyro.device_id > 0) {
if (accel_sub.advertised() && gyro_sub.advertised()) {
// if the sensors module is responsible for voting (SENS_IMU_MODE 1) then run every VehicleIMU in the same WQ
// otherwise each VehicleIMU runs in a corresponding INSx WQ
const bool multi_mode = (_param_sens_imu_mode.get() == 0);
@ -627,21 +637,8 @@ void Sensors::Run()
return;
}
// run once
if (_last_config_update == 0) {
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
_voted_sensors_update.init(_sensor_combined);
parameter_update_poll(true);
}
perf_begin(_loop_perf);
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
// check vehicle status for changes to publication state
if (_vcontrol_mode_sub.updated()) {
vehicle_control_mode_s vcontrol_mode{};
@ -651,23 +648,9 @@ void Sensors::Run()
}
}
_voted_sensors_update.sensorsPoll(_sensor_combined);
// check analog airspeed
adc_poll();
diff_pres_poll();
if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
_sensor_pub.publish(_sensor_combined);
_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
}
// keep adding sensors as long as we are not armed,
// when not adding sensors poll for param updates
if (!_armed && hrt_elapsed_time(&_last_config_update) > 1000_ms) {
if ((!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) || (_last_config_update == 0)) {
const int n_accel = orb_group_count(ORB_ID(sensor_accel));
const int n_baro = orb_group_count(ORB_ID(sensor_baro));
@ -683,10 +666,6 @@ void Sensors::Run()
_n_mag = n_mag;
parameters_update();
InitializeVehicleAirData();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
}
// sensor device id (not just orb_group_count) must be populated before IMU init can succeed
@ -700,6 +679,23 @@ void Sensors::Run()
parameter_update_poll();
}
_voted_sensors_update.sensorsPoll(_sensor_combined);
if (_sensor_combined.timestamp != _sensor_combined_prev_timestamp) {
_voted_sensors_update.setRelativeTimestamps(_sensor_combined);
_sensor_pub.publish(_sensor_combined);
_sensor_combined_prev_timestamp = _sensor_combined.timestamp;
}
// check analog airspeed
adc_poll();
diff_pres_poll();
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
perf_end(_loop_perf);
}

4
src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -46,6 +46,8 @@ VehicleAcceleration::VehicleAcceleration() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vehicle_acceleration_pub.advertise();
CheckAndUpdateFilters();
}

4
src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
View File

@ -48,6 +48,8 @@ VehicleAirData::VehicleAirData() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vehicle_air_data_pub.advertise();
_voter.set_timeout(SENSOR_TIMEOUT);
}
@ -334,7 +336,7 @@ void VehicleAirData::Run()
}
// reschedule timeout
ScheduleDelayed(100_ms);
ScheduleDelayed(50_ms);
perf_end(_cycle_perf);
}

2
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -46,6 +46,8 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
_vehicle_angular_acceleration_pub.advertise();
_vehicle_angular_velocity_pub.advertise();
}
VehicleAngularVelocity::~VehicleAngularVelocity()

8
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -43,6 +43,7 @@ VehicleGPSPosition::VehicleGPSPosition() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vehicle_gps_position_pub.advertise();
}
VehicleGPSPosition::~VehicleGPSPosition()
@ -103,9 +104,6 @@ void VehicleGPSPosition::Run()
perf_begin(_cycle_perf);
ParametersUpdate();
// GPS blending
ScheduleDelayed(500_ms); // backup schedule
// Check all GPS instance
bool any_gps_updated = false;
bool gps_updated = false;
@ -135,6 +133,8 @@ void VehicleGPSPosition::Run()
}
}
ScheduleDelayed(300_ms); // backup schedule
perf_end(_cycle_perf);
}

4
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -650,8 +650,8 @@ void VehicleIMU::UpdateIntegratorConfiguration()
_gyro_integrator.set_reset_interval(roundf((gyro_integral_samples - 0.5f) * _gyro_interval_us));
_gyro_integrator.set_reset_samples(gyro_integral_samples);
_backup_schedule_timeout_us = math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us);
_backup_schedule_timeout_us = math::constrain((int)math::min(sensor_accel_s::ORB_QUEUE_LENGTH * _accel_interval_us,
sensor_gyro_s::ORB_QUEUE_LENGTH * _gyro_interval_us) / 2, 1000, 20000);
// gyro: find largest integer multiple of gyro_integral_samples
for (int n = sensor_gyro_s::ORB_QUEUE_LENGTH; n > 0; n--) {

17
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
View File

@ -36,6 +36,7 @@
#include <px4_platform_common/log.h>
#include <px4_platform_common/events.h>
#include <lib/geo/geo.h>
#include <lib/sensor_calibration/Utilities.hpp>
namespace sensors
{
@ -55,6 +56,20 @@ VehicleMagnetometer::VehicleMagnetometer() :
_voter.set_equal_value_threshold(1000);
ParametersUpdate(true);
_vehicle_magnetometer_pub[0].advertise();
_sensor_preflight_mag_pub.advertise();
// if publishing multiple mags advertise instances immediately for existing calibrations
if (!_param_sens_mag_mode.get()) {
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
uint32_t device_id_mag = calibration::GetCalibrationParamInt32("MAG", "ID", i);
if (device_id_mag != 0) {
_vehicle_magnetometer_pub[i].advertise();
}
}
}
}
VehicleMagnetometer::~VehicleMagnetometer()
@ -547,7 +562,7 @@ void VehicleMagnetometer::Run()
UpdateMagCalibration();
// reschedule timeout
ScheduleDelayed(40_ms);
ScheduleDelayed(50_ms);
perf_end(_cycle_perf);
}

13
src/modules/sensors/voted_sensors_update.cpp
View File

@ -54,22 +54,17 @@ VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled,
_vehicle_imu_sub(vehicle_imu_sub),
_hil_enabled(hil_enabled)
{
_sensor_selection_pub.advertise();
_sensors_status_imu_pub.advertise();
if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit
_gyro.voter.set_timeout(500000);
_accel.voter.set_timeout(500000);
}
}
int VotedSensorsUpdate::init(sensor_combined_s &raw)
{
raw.accelerometer_timestamp_relative = sensor_combined_s::RELATIVE_TIMESTAMP_INVALID;
raw.timestamp = 0;
initializeSensors();
_selection_changed = true;
return 0;
parametersUpdate();
}
void VotedSensorsUpdate::initializeSensors()

6
src/modules/sensors/voted_sensors_update.h
View File

@ -78,12 +78,6 @@ public:
*/
VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]);
/**
* initialize subscriptions etc.
* @return 0 on success, <0 otherwise
*/
int init(sensor_combined_s &raw);
/**
* This tries to find new sensor instances. This is called from init(), then it can be called periodically.
*/