rate limit most parameter_update subscriptions

- parameter updates can be quite expensive because they trigger nearly all modules to reload all of their parameters immediately
 - limit modules from updating faster than once per second
This commit is contained in:
Daniel Agar
2021-01-10 21:09:15 -05:00
committed by GitHub
parent 70e503cb91
commit 967d35a6b6
73 changed files with 212 additions and 122 deletions
+2 -2
View File
@@ -574,7 +574,7 @@ void DShot::Run()
}
}
if (_param_sub.updated()) {
if (_parameter_update_sub.updated()) {
update_params();
}
@@ -603,7 +603,7 @@ void DShot::Run()
void DShot::update_params()
{
parameter_update_s pupdate;
_param_sub.update(&pupdate);
_parameter_update_sub.copy(&pupdate);
updateParams();
+1 -1
View File
@@ -223,7 +223,7 @@ private:
Mode _mode{MODE_NONE};
uORB::Subscription _param_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
DEFINE_PARAMETERS(
(ParamInt<px4::params::DSHOT_CONFIG>) _param_dshot_config,
+4 -2
View File
@@ -46,12 +46,14 @@
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <mathlib/mathlib.h>
using namespace time_literals;
#define CONTROLLER_PERIOD_DEFAULT 100000
/**
@@ -174,7 +176,7 @@ private:
float _integrator_value = 0.0f;
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
float _proportional_value = 0.0f;
+5 -2
View File
@@ -49,8 +49,11 @@
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
#define ADDR 0x55 /**< I2C adress of TCA62724FMG */
#define SUB_ADDR_START 0x01 /**< write everything (with auto-increment) */
#define SUB_ADDR_PWM0 0x81 /**< blue (without auto-increment) */
@@ -61,6 +64,7 @@
#define SETTING_NOT_POWERSAVE 0x01 /**< power-save mode not off */
#define SETTING_ENABLE 0x02 /**< on */
class RGBLED : public device::I2C, public I2CSPIDriver<RGBLED>
{
public:
@@ -88,11 +92,10 @@ private:
uint8_t _b{0};
bool _leds_enabled{true};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
LedController _led_controller;
int send_led_enable(bool enable);
int send_led_rgb();
int get(bool &on, bool &powersave, uint8_t &r, uint8_t &g, uint8_t &b);
@@ -47,9 +47,10 @@
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
#define ADDR 0x39 /**< I2C adress of NCP5623C */
@@ -89,7 +90,7 @@ private:
volatile bool _should_run{true};
bool _leds_enabled{true};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
LedController _led_controller;
+4 -1
View File
@@ -43,6 +43,7 @@
#include <px4_platform_common/posix.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -63,6 +64,8 @@
#include "ocpoc_mmap.h"
#include "bbblue_pwm_rc.h"
using namespace time_literals;
namespace linux_pwm_out
{
static px4_task_t _task_handle = -1;
@@ -260,7 +263,7 @@ void task_main(int argc, char *argv[])
Mixer::Airmode airmode = Mixer::Airmode::disabled;
update_params(airmode);
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
int rc_channels_sub = -1;
+2 -1
View File
@@ -53,6 +53,7 @@
#define PCA9685_DEFAULT_ADDRESS (0x40)
using namespace drv_pca9685_pwm;
using namespace time_literals;
class PCA9685Wrapper : public cdev::CDev, public ModuleBase<PCA9685Wrapper>, public OutputModuleInterface
{
@@ -112,7 +113,7 @@ protected:
PCA9685 *pca9685 = nullptr; // driver handle.
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; // param handle
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
MixingOutput _mixing_output{PCA9685_PWM_CHANNEL_COUNT, *this, MixingOutput::SchedulingPolicy::Disabled, true};
};
+2 -2
View File
@@ -45,8 +45,8 @@
#include <lib/perf/perf_counter.h>
#include <battery/battery.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <px4_platform_common/i2c_spi_buses.h>
@@ -207,7 +207,7 @@ private:
Battery _battery;
uORB::Subscription _actuators_sub{ORB_ID(actuator_controls_0)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
int read(uint8_t address, int16_t &data);
int write(uint8_t address, uint16_t data);
+2 -1
View File
@@ -89,6 +89,7 @@
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/power_monitor.h>
@@ -253,7 +254,7 @@ private:
perf_counter_t _comms_errors;
uORB::PublicationMulti<power_monitor_s> _pm_pub_topic{ORB_ID(power_monitor)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
power_monitor_s _pm_status{};
+1 -2
View File
@@ -169,8 +169,7 @@ private:
unsigned _current_update_rate{0};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
unsigned _num_outputs{0};
int _class_instance{-1};
+3 -2
View File
@@ -44,6 +44,8 @@
#include <px4_platform_common/time.h>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
class PWMSim : public cdev::CDev, public ModuleBase<PWMSim>, public OutputModuleInterface
{
public:
@@ -76,7 +78,6 @@ private:
static constexpr uint16_t PWM_SIM_PWM_MAX_MAGIC = 2000;
MixingOutput _mixing_output{MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
};
+3 -2
View File
@@ -80,8 +80,8 @@
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -241,9 +241,10 @@ private:
uORB::Subscription _t_actuator_controls_3{ORB_ID(actuator_controls_3)};; ///< actuator controls group 3 topic
uORB::Subscription _t_actuator_armed{ORB_ID(actuator_armed)}; ///< system armed control topic
uORB::Subscription _t_vehicle_control_mode{ORB_ID(vehicle_control_mode)}; ///< vehicle control mode topic
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< parameter update topic
uORB::Subscription _t_vehicle_command{ORB_ID(vehicle_command)}; ///< vehicle command topic
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
hrt_abstime _last_status_publish{0};
bool _param_update_force; ///< force a parameter update
@@ -333,7 +333,7 @@ void task_main(int argc, char *argv[])
Mixer::Airmode airmode = Mixer::Airmode::disabled;
update_params(airmode);
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
// Start disarmed
_armed.armed = false;
+4 -1
View File
@@ -48,6 +48,7 @@
#include <perf/perf_counter.h>
#include <px4_platform_common/module_params.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -78,6 +79,8 @@
# define TAP_ESC_CTRL_UORB_UPDATE_INTERVAL 2 // [ms] min: 2, max: 100
#endif
using namespace time_literals;
/*
* This driver connects to TAP ESCs via serial.
*/
@@ -115,7 +118,7 @@ private:
int _armed_sub = -1;
int _test_motor_sub = -1;
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
orb_advert_t _outputs_pub = nullptr;
actuator_outputs_s _outputs = {};
+4 -1
View File
@@ -68,8 +68,11 @@
#include <lib/perf/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
class UavcanNode;
/**
@@ -215,7 +218,7 @@ private:
bool _idle_throttle_when_armed{false};
int32_t _idle_throttle_when_armed_param{0};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
perf_counter_t _cycle_perf;
perf_counter_t _interval_perf;
+3 -2
View File
@@ -65,7 +65,6 @@
#include <uavcan/equipment/power/BatteryInfo.hpp>
#include <uavcan/equipment/range_sensor/Measurement.hpp>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@@ -79,6 +78,8 @@
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_gps_position.h>
using namespace time_literals;
/**
* A UAVCAN node.
*/
@@ -179,7 +180,7 @@ private:
hrt_abstime _last_static_temperature_publish{0};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _battery_status_sub{this, ORB_ID(battery_status)};
uORB::SubscriptionCallbackWorkItem _diff_pressure_sub{this, ORB_ID(differential_pressure)};
+4 -2
View File
@@ -55,6 +55,7 @@
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
@@ -65,7 +66,8 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
using namespace time_literals;
/* Prototypes */
@@ -315,7 +317,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
int global_sp_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
/* Setup of loop */
-1
View File
@@ -38,7 +38,6 @@
#include <mathlib/math/Functions.hpp>
using namespace matrix;
using namespace time_literals;
using math::radians;
GyroFFT::GyroFFT() :
+4 -1
View File
@@ -54,6 +54,8 @@
#include "arm_math.h"
#include "arm_const_structs.h"
using namespace time_literals;
class GyroFFT : public ModuleBase<GyroFFT>, public ModuleParams, public px4::ScheduledWorkItem
{
public:
@@ -86,7 +88,8 @@ private:
uORB::Publication<sensor_gyro_fft_s> _sensor_gyro_fft_pub{ORB_ID(sensor_gyro_fft)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status)};
+8 -5
View File
@@ -52,6 +52,7 @@
#include <time.h>
#include <drivers/drv_hrt.h>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -70,6 +71,8 @@
/* process-specific header files */
#include "params.h"
using namespace time_literals;
/* Prototypes */
/**
@@ -101,7 +104,7 @@ int parameters_init(struct param_handles *h);
* Update all parameters
*
*/
int parameters_update(const struct param_handles *h, struct params *p);
int parameter_update(const struct param_handles *h, struct params *p);
/**
* Mainloop of daemon.
@@ -141,7 +144,7 @@ int parameters_init(struct param_handles *h)
return OK;
}
int parameters_update(const struct param_handles *h, struct params *p)
int parameter_update(const struct param_handles *h, struct params *p)
{
param_get(h->yaw_p, &(p->yaw_p));
@@ -201,7 +204,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
/* initialize parameters, first the handles, then the values */
parameters_init(&ph);
parameters_update(&ph, &pp);
parameter_update(&ph, &pp);
/*
@@ -271,7 +274,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval parameter_update_sub{ORB_ID(parameter_update), 1_s};
/* Setup of loop */
@@ -313,7 +316,7 @@ int rover_steering_control_thread_main(int argc, char *argv[])
parameter_update_sub.copy(&pupdate);
// if a param update occured, re-read our parameters
parameters_update(&ph, &pp);
parameter_update(&ph, &pp);
}
/* only run controller if attitude changed */
@@ -42,6 +42,8 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_angular_velocity.h>
using namespace time_literals;
/**
* Airship attitude control app start / stop handling function
*/
@@ -80,7 +82,7 @@ private:
void publish_actuator_controls();
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< parameter updates subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
@@ -106,9 +106,10 @@ private:
uORB::PublicationMulti<wind_estimate_s> _wind_est_pub[MAX_NUM_AIRSPEED_SENSORS + 1] {{ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}, {ORB_ID(wind_estimate)}}; /**< wind estimate topic (for each airspeed validator + purely sideslip fusion) */
orb_advert_t _mavlink_log_pub {nullptr}; /**< mavlink log topic*/
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
uORB::Subscription _param_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -283,7 +284,7 @@ AirspeedModule::Run()
parameter_update_s update;
if (_param_sub.update(&update)) {
if (_parameter_update_sub.update(&update)) {
update_params();
}
@@ -108,7 +108,7 @@ private:
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)};
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vision_odom_sub{ORB_ID(vehicle_visual_odometry)};
@@ -101,7 +101,7 @@ private:
void Run() override;
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)}; /**< attitude controls sub */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; /**< notification of parameter updates */
uORB::SubscriptionCallbackWorkItem _adc_report_sub{this, ORB_ID(adc_report)};
static constexpr uint32_t SAMPLE_FREQUENCY_HZ = 100;
+2 -1
View File
@@ -403,7 +403,6 @@ private:
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _safety_sub{ORB_ID(safety)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
@@ -411,6 +410,8 @@ private:
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionMultiArray<battery_status_s, battery_status_s::MAX_INSTANCES> _battery_status_subs{ORB_ID::battery_status};
uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
+2 -1
View File
@@ -208,6 +208,8 @@ private:
float _airspeed_scale_factor{1.0f}; ///< scale factor correction applied to airspeed measurements
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _airdata_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
uORB::Subscription _distance_sensor_sub{ORB_ID(distance_sensor)};
@@ -215,7 +217,6 @@ private:
uORB::Subscription _landing_target_pose_sub{ORB_ID(landing_target_pose)};
uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _optical_flow_sub{ORB_ID(optical_flow)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+1 -1
View File
@@ -181,7 +181,7 @@ private:
uint8_t _lat_lon_reset_counter{0};
uint8_t _alt_reset_counter{0};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};
// Publications
+1 -1
View File
@@ -71,7 +71,7 @@ private:
void parameters_updated();
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};
uORB::SubscriptionCallbackWorkItem _esc_status_sub{this, ORB_ID(esc_status)};
@@ -94,8 +94,9 @@ private:
perf_counter_t _loop_perf; ///< loop duration performance counter
hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionData<home_position_s> _home_position_sub{ORB_ID(home_position)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::SubscriptionData<vehicle_control_mode_s> _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::SubscriptionData<vehicle_land_detected_s> _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
@@ -72,6 +72,8 @@ using matrix::Quatf;
using uORB::SubscriptionData;
using namespace time_literals;
class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public ModuleParams,
public px4::WorkItem
{
@@ -95,11 +97,12 @@ private:
uORB::SubscriptionCallbackWorkItem _att_sub{this, ORB_ID(vehicle_attitude)}; /**< vehicle attitude */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)}; /**< vehicle attitude setpoint */
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */
uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)}; /**< local position subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint */
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
@@ -148,10 +148,11 @@ private:
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; ///< notification of parameter updates
uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription
+2 -1
View File
@@ -169,8 +169,9 @@ private:
uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
@@ -225,14 +225,10 @@ void LandingTargetEstimator::update()
void LandingTargetEstimator::_check_params(const bool force)
{
bool updated = _parameterSub.updated();
if (_parameter_update_sub.updated() || force) {
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
if (updated) {
parameter_update_s paramUpdate;
_parameterSub.copy(&paramUpdate);
}
if (updated || force) {
_update_params();
}
}
@@ -47,6 +47,7 @@
#include <parameters/param.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -59,6 +60,7 @@
#include <matrix/Matrix.hpp>
#include "KalmanFilter.h"
using namespace time_literals;
namespace landing_target_estimator
{
@@ -96,7 +98,7 @@ protected:
uORB::Publication<landing_target_innovations_s> _targetInnovationsPub{ORB_ID(landing_target_innovations)};
landing_target_innovations_s _target_innovations{};
uORB::Subscription _parameterSub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
private:
@@ -241,7 +241,7 @@ void BlockLocalPositionEstimator::Run()
_lastArmedState = armedState;
// see which updates are available
bool paramsUpdated = _sub_param_update.update();
const bool paramsUpdated = _parameter_update_sub.updated();
_baroUpdated = false;
if ((_param_lpe_fusion.get() & FUSE_BARO) && _sub_airdata.update()) {
@@ -266,6 +266,10 @@ void BlockLocalPositionEstimator::Run()
// update parameters
if (paramsUpdated) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
SuperBlock::updateParams();
ModuleParams::updateParams();
updateSSParams();
@@ -263,12 +263,13 @@ private:
// subscriptions
uORB::SubscriptionCallbackWorkItem _sensors_sub{this, ORB_ID(sensor_combined)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionData<actuator_armed_s> _sub_armed{ORB_ID(actuator_armed)};
uORB::SubscriptionData<vehicle_land_detected_s> _sub_land{ORB_ID(vehicle_land_detected)};
uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
uORB::SubscriptionData<vehicle_angular_velocity_s> _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionData<optical_flow_s> _sub_flow{ORB_ID(optical_flow)};
uORB::SubscriptionData<parameter_update_s> _sub_param_update{ORB_ID(parameter_update)};
uORB::SubscriptionData<vehicle_gps_position_s> _sub_gps{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_odometry_s> _sub_visual_odom{ORB_ID(vehicle_visual_odometry)};
uORB::SubscriptionData<vehicle_odometry_s> _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)};
+2 -2
View File
@@ -353,8 +353,8 @@ private:
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _log_message_sub{ORB_ID(log_message), 20};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
DEFINE_PARAMETERS(
(ParamInt<px4::params::SDLOG_UTC_OFFSET>) _param_sdlog_utc_offset,
+2 -5
View File
@@ -52,7 +52,6 @@
#include <lib/mathlib/mathlib.h>
#include <lib/systemlib/mavlink_log.h>
#include <lib/version/version.h>
#include <uORB/Publication.hpp>
#include "mavlink_receiver.h"
#include "mavlink_main.h"
@@ -2136,8 +2135,6 @@ Mavlink::task_main(int argc, char *argv[])
pthread_mutex_init(&_message_buffer_mutex, nullptr);
}
uORB::Subscription parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
// ensure topic exists, otherwise we might lose first queued commands (leading to printf error's below)
orb_advertise_queue(ORB_ID(vehicle_command), nullptr, vehicle_command_s::ORB_QUEUE_LENGTH);
@@ -2247,10 +2244,10 @@ Mavlink::task_main(int argc, char *argv[])
update_rate_mult();
// check for parameter updates
if (parameter_update_sub.updated()) {
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
parameter_update_sub.copy(&pupdate);
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
mavlink_update_parameters();
+5 -1
View File
@@ -69,8 +69,10 @@
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <systemlib/uthash/utlist.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/mission_result.h>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/radio_status.h>
#include <uORB/topics/telemetry_status.h>
@@ -530,6 +532,8 @@ private:
uORB::PublicationMulti<telemetry_status_s> _telem_status_pub{ORB_ID(telemetry_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
bool _task_running{true};
static bool _boot_complete;
static constexpr int MAVLINK_MAX_INSTANCES{4};
+5 -5
View File
@@ -354,14 +354,14 @@ MavlinkParametersManager::send_untransmitted()
{
bool sent_one = false;
if (_mavlink_parameter_sub.updated()) {
// Clear the ready flag
parameter_update_s value;
_mavlink_parameter_sub.update(&value);
if (_parameter_update_sub.updated()) {
// clear the update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// Schedule an update if not already the case
if (_param_update_time == 0) {
_param_update_time = value.timestamp;
_param_update_time = pupdate.timestamp;
_param_update_index = 0;
}
}
+4 -1
View File
@@ -47,12 +47,15 @@
#include "mavlink_bridge_header.h"
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/uavcan_parameter_request.h>
#include <uORB/topics/uavcan_parameter_value.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
class Mavlink;
class MavlinkParametersManager
@@ -147,7 +150,7 @@ protected:
uORB::Subscription _uavcan_parameter_value_sub{ORB_ID(uavcan_parameter_value)};
uORB::Subscription _mavlink_parameter_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
hrt_abstime _param_update_time{0};
int _param_update_index{0};
+5 -1
View File
@@ -55,6 +55,7 @@
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/PublicationMulti.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
@@ -107,6 +108,8 @@
# include <uORB/topics/debug_vect.h>
#endif // !CONSTRAINED_FLASH
using namespace time_literals;
class Mavlink;
class MavlinkReceiver : public ModuleParams
@@ -304,11 +307,12 @@ private:
// ORB subscriptions
uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
// hil_sensor and hil_state_quaternion
enum SensorSource {
ACCEL = 0b111,
@@ -46,6 +46,7 @@
#include <uORB/topics/estimator_selector_status.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/wind_estimate.h>
@@ -58,6 +58,8 @@
#include <AttitudeControl.hpp>
using namespace time_literals;
/**
* Multicopter attitude control app start / stop handling function
*/
@@ -98,10 +100,11 @@ private:
AttitudeControl _attitude_control; ///< class for attitude control calculations
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_attitude_setpoint_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)}; /**< vehicle rates setpoint subscription */
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< manual control setpoint subscription */
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
@@ -228,10 +228,10 @@ MulticopterAttitudeControl::Run()
perf_begin(_loop_perf);
// Check if parameters have changed
if (_params_sub.updated()) {
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_params_sub.copy(&param_update);
_parameter_update_sub.copy(&param_update);
updateParams();
parameters_updated();
@@ -61,6 +61,8 @@
#include "zero_order_hover_thrust_ekf.hpp"
using namespace time_literals;
class MulticopterHoverThrustEstimator : public ModuleBase<MulticopterHoverThrustEstimator>, public ModuleParams,
public px4::WorkItem
{
@@ -97,7 +99,8 @@ private:
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
@@ -92,8 +92,9 @@ private:
uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)}; /**< vehicle local position */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle control mode subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< notification of parameter updates */
uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
@@ -60,6 +60,8 @@
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
using namespace time_literals;
class MulticopterRateControl : public ModuleBase<MulticopterRateControl>, public ModuleParams, public px4::WorkItem
{
public:
@@ -91,13 +93,14 @@ private:
uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
uORB::Subscription _vehicle_angular_acceleration_sub{ORB_ID(vehicle_angular_acceleration)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
uORB::Publication<actuator_controls_s> _actuators_0_pub;
+5 -2
View File
@@ -60,6 +60,7 @@
#include <px4_platform_common/module_params.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/mission.h>
@@ -78,12 +79,13 @@
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
using namespace time_literals;
/**
* Number of navigation modes that need on_active/on_inactive calls
*/
#define NAVIGATOR_MODE_ARRAY_SIZE 9
class Navigator : public ModuleBase<Navigator>, public ModuleParams
{
public:
@@ -332,11 +334,12 @@ private:
int _local_pos_sub{-1}; /**< local position subscription */
int _vehicle_status_sub{-1}; /**< local position subscription */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; /**< global position subscription */
uORB::Subscription _gps_pos_sub{ORB_ID(vehicle_gps_position)}; /**< gps position subscription */
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; /**< param update subscription */
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
+4 -1
View File
@@ -60,6 +60,8 @@
#include <uORB/topics/rc_parameter_map.h>
#include <uORB/topics/parameter_update.h>
using namespace time_literals;
namespace RCUpdate
{
@@ -156,7 +158,8 @@ private:
uORB::SubscriptionCallbackWorkItem _input_rc_sub{this, ORB_ID(input_rc)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _rc_parameter_map_sub{ORB_ID(rc_parameter_map)};
uORB::Publication<rc_channels_s> _rc_channels_pub{ORB_ID(rc_channels)};

Some files were not shown because too many files have changed in this diff Show More