diff --git a/src/drivers/dshot/dshot.cpp b/src/drivers/dshot/dshot.cpp index de6af99301..98160cd6e7 100644 --- a/src/drivers/dshot/dshot.cpp +++ b/src/drivers/dshot/dshot.cpp @@ -220,7 +220,6 @@ private: bool _outputs_initialized{false}; perf_counter_t _cycle_perf; - perf_counter_t _cycle_interval_perf; void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); @@ -247,8 +246,7 @@ px4::atomic_bool DShotOutput::_request_telemetry_init{false}; DShotOutput::DShotOutput() : CDev("/dev/dshot"), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, "dshot: cycle")), - _cycle_interval_perf(perf_alloc(PC_INTERVAL, "dshot: cycle interval")) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _mixing_output.setAllDisarmedValues(DISARMED_VALUE); _mixing_output.setAllMinValues(DISARMED_VALUE + 1); @@ -265,7 +263,6 @@ DShotOutput::~DShotOutput() unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); perf_free(_cycle_perf); - perf_free(_cycle_interval_perf); delete _telemetry; } @@ -756,7 +753,6 @@ DShotOutput::Run() } perf_begin(_cycle_perf); - perf_count(_cycle_interval_perf); _mixing_output.update(); @@ -1667,7 +1663,6 @@ int DShotOutput::print_status() PX4_INFO("Outputs initialized: %s", _outputs_initialized ? "yes" : "no"); PX4_INFO("Outputs on: %s", _outputs_on ? "yes" : "no"); perf_print_counter(_cycle_perf); - perf_print_counter(_cycle_interval_perf); _mixing_output.printStatus(); if (_telemetry) { PX4_INFO("telemetry on: %s", _telemetry_device); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4b347b33ac..11d49abcae 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -187,7 +187,6 @@ private: unsigned _num_disarmed_set{0}; perf_counter_t _cycle_perf; - perf_counter_t _cycle_interval_perf; void capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow); @@ -212,8 +211,7 @@ private: PX4FMU::PX4FMU() : CDev(PX4FMU_DEVICE_PATH), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default), - _cycle_perf(perf_alloc(PC_ELAPSED, "px4fmu: cycle")), - _cycle_interval_perf(perf_alloc(PC_INTERVAL, "px4fmu: cycle interval")) + _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")) { _mixing_output.setAllMinValues(PWM_DEFAULT_MIN); _mixing_output.setAllMaxValues(PWM_DEFAULT_MAX); @@ -229,7 +227,6 @@ PX4FMU::~PX4FMU() unregister_class_devname(PWM_OUTPUT_BASE_DEVICE_PATH, _class_instance); perf_free(_cycle_perf); - perf_free(_cycle_interval_perf); } int @@ -755,7 +752,6 @@ PX4FMU::Run() } perf_begin(_cycle_perf); - perf_count(_cycle_interval_perf); _mixing_output.update(); @@ -2305,7 +2301,6 @@ int PX4FMU::print_status() } perf_print_counter(_cycle_perf); - perf_print_counter(_cycle_interval_perf); _mixing_output.printStatus(); return 0; diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp index a0d658b795..2911eda659 100644 --- a/src/drivers/rc_input/RCInput.cpp +++ b/src/drivers/rc_input/RCInput.cpp @@ -46,7 +46,7 @@ constexpr char const *RCInput::RC_SCAN_STRING[]; RCInput::RCInput(bool run_as_task, char *device) : _cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")), - _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) + _publish_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": publish interval")) { // rc input, published to ORB _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_PX4FMU_PPM; diff --git a/src/lib/drivers/accelerometer/CMakeLists.txt b/src/lib/drivers/accelerometer/CMakeLists.txt index 0ed79b55eb..fd6ca3efcb 100644 --- a/src/lib/drivers/accelerometer/CMakeLists.txt +++ b/src/lib/drivers/accelerometer/CMakeLists.txt @@ -32,4 +32,8 @@ ############################################################################ px4_add_library(drivers_accelerometer PX4Accelerometer.cpp) -target_link_libraries(drivers_accelerometer PRIVATE drivers__device) +target_link_libraries(drivers_accelerometer + PRIVATE + drivers__device + mathlib + ) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index cfa77509b5..dd0e9e6849 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -171,8 +171,6 @@ private: uint64_t _start_time_us = 0; ///< system time at EKF start (uSec) int64_t _last_time_slip_us = 0; ///< Last time slip (uSec) - perf_counter_t _cycle_perf; - perf_counter_t _interval_perf; perf_counter_t _ekf_update_perf; // Initialise time stamps used to send sensor data to the EKF and for logging @@ -549,8 +547,6 @@ Ekf2::Ekf2(bool replay_mode): ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), _replay_mode(replay_mode), - _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")), - _interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval")), _ekf_update_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": update")), _params(_ekf.getParamHandle()), _param_ekf2_min_obs_dt(_params->sensor_interval_min_ms), @@ -656,8 +652,6 @@ Ekf2::Ekf2(bool replay_mode): Ekf2::~Ekf2() { - perf_free(_cycle_perf); - perf_free(_interval_perf); perf_free(_ekf_update_perf); } @@ -679,8 +673,6 @@ int Ekf2::print_status() PX4_INFO("time slip: %" PRId64 " us", _last_time_slip_us); - perf_print_counter(_cycle_perf); - perf_print_counter(_interval_perf); perf_print_counter(_ekf_update_perf); return 0; @@ -728,9 +720,6 @@ void Ekf2::Run() return; } - perf_begin(_cycle_perf); - perf_count(_interval_perf); - sensor_combined_s sensors; if (_sensors_sub.update(&sensors)) { @@ -1751,8 +1740,6 @@ void Ekf2::Run() // publish ekf2_timestamps _ekf2_timestamps_pub.publish(ekf2_timestamps); } - - perf_end(_cycle_perf); } int Ekf2::getRangeSubIndex() 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 c36978eb0d..c0bfbd8f82 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -195,7 +195,6 @@ private: WeatherVane *_wv_controller{nullptr}; perf_counter_t _cycle_perf; - perf_counter_t _interval_perf; /** * Update our local parameter cache. @@ -289,8 +288,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _vel_y_deriv(this, "VELD"), _vel_z_deriv(this, "VELD"), _control(this), - _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")), - _interval_perf(perf_alloc_once(PC_INTERVAL, MODULE_NAME": interval")) + _cycle_perf(perf_alloc_once(PC_ELAPSED, MODULE_NAME": cycle time")) { // fetch initial parameter values parameters_update(true); @@ -306,7 +304,6 @@ MulticopterPositionControl::~MulticopterPositionControl() } perf_free(_cycle_perf); - perf_free(_interval_perf); } bool @@ -515,7 +512,6 @@ MulticopterPositionControl::print_status() } perf_print_counter(_cycle_perf); - perf_print_counter(_interval_perf); return 0; } @@ -530,7 +526,6 @@ MulticopterPositionControl::Run() } perf_begin(_cycle_perf); - perf_count(_interval_perf); if (_local_pos_sub.update(&_local_pos)) { diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 0fd7573333..d62ffeadcf 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -40,18 +40,13 @@ using namespace time_literals; VehicleAcceleration::VehicleAcceleration() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl), - _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: cycle time")), - _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_acceleration: sensor latency")) + WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl) { } VehicleAcceleration::~VehicleAcceleration() { Stop(); - - perf_free(_cycle_perf); - perf_free(_sensor_latency_perf); } bool @@ -175,16 +170,12 @@ VehicleAcceleration::ParametersUpdate(bool force) void VehicleAcceleration::Run() { - perf_begin(_cycle_perf); - // update corrections first to set _selected_sensor SensorCorrectionsUpdate(); sensor_accel_s sensor_data; if (_sensor_sub[_selected_sensor].update(&sensor_data)) { - perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); - ParametersUpdate(); SensorBiasUpdate(); @@ -207,15 +198,10 @@ VehicleAcceleration::Run() _vehicle_acceleration_pub.publish(out); } - - perf_end(_cycle_perf); } void VehicleAcceleration::PrintStatus() { PX4_INFO("selected sensor: %d", _selected_sensor); - - perf_print_counter(_cycle_perf); - perf_print_counter(_sensor_latency_perf); } diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp index 9cd4c9d496..ecb29bb238 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp @@ -36,7 +36,6 @@ #include #include #include -#include #include #include #include @@ -101,9 +100,6 @@ private: matrix::Vector3f _scale; matrix::Vector3f _bias; - perf_counter_t _cycle_perf; - perf_counter_t _sensor_latency_perf; - uint8_t _selected_sensor{0}; }; diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 09228c2d07..6cd2e52399 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -40,18 +40,13 @@ using namespace time_literals; VehicleAngularVelocity::VehicleAngularVelocity() : ModuleParams(nullptr), - WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl), - _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")), - _sensor_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor latency")) + WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl) { } VehicleAngularVelocity::~VehicleAngularVelocity() { Stop(); - - perf_free(_cycle_perf); - perf_free(_sensor_latency_perf); } bool @@ -214,8 +209,6 @@ VehicleAngularVelocity::ParametersUpdate(bool force) void VehicleAngularVelocity::Run() { - perf_begin(_cycle_perf); - // update corrections first to set _selected_sensor SensorCorrectionsUpdate(); @@ -224,8 +217,6 @@ VehicleAngularVelocity::Run() sensor_gyro_control_s sensor_data; if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) { - perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); - ParametersUpdate(); SensorBiasUpdate(); @@ -251,8 +242,6 @@ VehicleAngularVelocity::Run() sensor_gyro_s sensor_data; if (_sensor_sub[_selected_sensor].update(&sensor_data)) { - perf_set_elapsed(_sensor_latency_perf, hrt_elapsed_time(&sensor_data.timestamp)); - ParametersUpdate(); SensorBiasUpdate(); @@ -276,8 +265,6 @@ VehicleAngularVelocity::Run() _vehicle_angular_velocity_pub.publish(angular_velocity); } } - - perf_end(_cycle_perf); } void @@ -291,7 +278,4 @@ VehicleAngularVelocity::PrintStatus() } else { PX4_WARN("sensor_gyro_control unavailable for selected sensor: %d (%d)", _selected_sensor_device_id, _selected_sensor); } - - perf_print_counter(_cycle_perf); - perf_print_counter(_sensor_latency_perf); } diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index 4269ba5775..d22b5d0e3f 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -36,7 +36,6 @@ #include #include #include -#include #include #include #include @@ -109,9 +108,6 @@ private: matrix::Vector3f _scale; matrix::Vector3f _bias; - perf_counter_t _cycle_perf; - perf_counter_t _sensor_latency_perf; - uint32_t _selected_sensor_device_id{0}; uint8_t _selected_sensor{0}; uint8_t _selected_sensor_control{0};