diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index e47b685a21f7..ef2907deb79d 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -153,8 +153,6 @@ fi param set-default BAT1_N_CELLS 4 -param set-default CBRK_SUPPLY_CHK 894281 - # disable check, no CPU load reported on posix yet param set-default COM_CPU_MAX -1 param set-default COM_RAM_MAX -1 @@ -247,6 +245,8 @@ then battery_simulator start fi +system_power_simulator start + tone_alarm start rc_update start manual_control start diff --git a/boards/ark/can-flow/init/rc.board_sensors b/boards/ark/can-flow/init/rc.board_sensors index 841049dc86bd..56adfca28149 100644 --- a/boards/ark/can-flow/init/rc.board_sensors +++ b/boards/ark/can-flow/init/rc.board_sensors @@ -4,6 +4,7 @@ #------------------------------------------------------------------------------ param set-default IMU_GYRO_RATEMAX 1000 +param set-default SENS_FLOW_RATE 150 param set-default SENS_IMU_CLPNOTI 0 # Internal SPI diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin index 9ca59954b374..d4f13bd42882 100755 Binary files a/boards/ark/fpv/extras/ark_fpv_bootloader.bin and b/boards/ark/fpv/extras/ark_fpv_bootloader.bin differ diff --git a/boards/ark/fpv/src/board_config.h b/boards/ark/fpv/src/board_config.h index 081dbb56698f..7b3c54183ec9 100644 --- a/boards/ark/fpv/src/board_config.h +++ b/boards/ark/fpv/src/board_config.h @@ -176,6 +176,9 @@ #define BOARD_BATTERY1_V_DIV (21.0f) // (20k + 1k) / 1k = 21 +#define BOARD_BATTERY_ADC_VOLTAGE_FILTER_S 0.075f +#define BOARD_BATTERY_ADC_CURRENT_FILTER_S 0.125f + #define ADC_SCALED_PAYLOAD_SENSE ADC_SCALED_12V_CHANNEL /* HW has to large of R termination on ADC todo:change when HW value is chosen */ diff --git a/boards/zeroone/x6/init/rc.board_sensors b/boards/zeroone/x6/init/rc.board_sensors index 9658a6412cce..19038ecd131c 100644 --- a/boards/zeroone/x6/init/rc.board_sensors +++ b/boards/zeroone/x6/init/rc.board_sensors @@ -149,6 +149,9 @@ rm3100 -I -b 4 start # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer) ist8310 -X -b 1 -R 10 start +# Internal compass +ist8310 start -I -a 0x0E -R 12 + # Possible internal Baro if param compare SENS_INT_BARO_EN 1 then diff --git a/msg/EstimatorStatus.msg b/msg/EstimatorStatus.msg index dd62bc4aca3f..3c7985e630bb 100644 --- a/msg/EstimatorStatus.msg +++ b/msg/EstimatorStatus.msg @@ -27,9 +27,9 @@ uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fus uint8 CS_MAG_DEC = 6 # 6 - true if synthetic magnetic declination measurements are being fused uint8 CS_IN_AIR = 7 # 7 - true when thought to be airborne uint8 CS_WIND = 8 # 8 - true when wind velocity is being estimated -uint8 CS_BARO_HGT = 9 # 9 - true when baro height is being fused as a primary height reference -uint8 CS_RNG_HGT = 10 # 10 - true when range finder height is being fused as a primary height reference -uint8 CS_GPS_HGT = 11 # 11 - true when GPS height is being fused as a primary height reference +uint8 CS_BARO_HGT = 9 # 9 - true when baro data is being fused +uint8 CS_RNG_HGT = 10 # 10 - true when range finder data is being fused for height aiding +uint8 CS_GPS_HGT = 11 # 11 - true when GPS altitude is being fused uint8 CS_EV_POS = 12 # 12 - true when local position data from external vision is being fused uint8 CS_EV_YAW = 13 # 13 - true when yaw data from external vision measurements is being fused uint8 CS_EV_HGT = 14 # 14 - true when height data from external vision measurements is being fused diff --git a/msg/EstimatorStatusFlags.msg b/msg/EstimatorStatusFlags.msg index 6b17842c2650..705a5f9a71fd 100644 --- a/msg/EstimatorStatusFlags.msg +++ b/msg/EstimatorStatusFlags.msg @@ -13,9 +13,9 @@ bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fus bool cs_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended bool cs_in_air # 7 - true when the vehicle is airborne bool cs_wind # 8 - true when wind velocity is being estimated -bool cs_baro_hgt # 9 - true when baro height is being fused as a primary height reference -bool cs_rng_hgt # 10 - true when range finder height is being fused as a primary height reference -bool cs_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool cs_baro_hgt # 9 - true when baro data is being fused +bool cs_rng_hgt # 10 - true when range finder data is being fused for height aiding +bool cs_gps_hgt # 11 - true when GPS altitude is being fused bool cs_ev_pos # 12 - true when local position data fusion from external vision is intended bool cs_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended bool cs_ev_hgt # 14 - true when height data from external vision measurements is being fused diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index 87f3110cac7d..5d3ac60b62e2 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -705,11 +705,12 @@ UavcanNode::Run() if (can_init_res < 0) { PX4_ERR("CAN driver init failed %i", can_init_res); - } - _instance->init(node_id, can->driver.updateEvent()); + } else { + _instance->init(node_id, can->driver.updateEvent()); - _node_init = true; + _node_init = true; + } } pthread_mutex_lock(&_node_mutex); diff --git a/src/lib/mathlib/math/filter/AlphaFilter.hpp b/src/lib/mathlib/math/filter/AlphaFilter.hpp index 8536f33976b4..b7a6e86953a8 100644 --- a/src/lib/mathlib/math/filter/AlphaFilter.hpp +++ b/src/lib/mathlib/math/filter/AlphaFilter.hpp @@ -62,8 +62,8 @@ class AlphaFilter * * Both parameters have to be provided in the same units. * - * @param sample_interval interval between two samples - * @param time_constant filter time constant determining convergence + * @param sample_interval interval between two samples in seconds + * @param time_constant filter time constant determining convergence in seconds */ void setParameters(float sample_interval, float time_constant) { diff --git a/src/modules/battery_status/analog_battery.cpp b/src/modules/battery_status/analog_battery.cpp index d46e5de29090..8353b5d5d3fc 100644 --- a/src/modules/battery_status/analog_battery.cpp +++ b/src/modules/battery_status/analog_battery.cpp @@ -76,11 +76,29 @@ AnalogBattery::AnalogBattery(int index, ModuleParams *parent, const int sample_i void AnalogBattery::updateBatteryStatusADC(hrt_abstime timestamp, float voltage_raw, float current_raw) { - const float voltage_v = voltage_raw * _analog_params.v_div; + float voltage_v = voltage_raw * _analog_params.v_div; const bool connected = voltage_v > BOARD_ADC_OPEN_CIRCUIT_V && (BOARD_ADC_OPEN_CIRCUIT_V <= BOARD_VALID_UV || is_valid()); float current_a = (current_raw - _analog_params.v_offs_cur) * _analog_params.a_per_v; +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + + if (_last_timestamp == 0) { + _last_timestamp = timestamp; + } + + const float dt = (timestamp - _last_timestamp) / 1e6f; + _last_timestamp = timestamp; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + voltage_v = _voltage_filter.update(fmaxf(voltage_v, 0.f), dt); +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + current_a = _current_filter.update(fmaxf(current_a, 0.f), dt); +#endif + // Overwrite the measured current if current overwrite is defined and vehicle is unarmed if (_analog_params.i_overwrite > 0) { updateTopics(); diff --git a/src/modules/battery_status/analog_battery.h b/src/modules/battery_status/analog_battery.h index b532f54d0de4..1678f0aa0112 100644 --- a/src/modules/battery_status/analog_battery.h +++ b/src/modules/battery_status/analog_battery.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -96,5 +97,17 @@ class AnalogBattery : public Battery uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uint8_t _arming_state{0}; +#if defined(BOARD_BATTERY_ADC_VOLTAGE_FILTER_S) || defined(BOARD_BATTERY_ADC_CURRENT_FILTER_S) + hrt_abstime _last_timestamp {0}; +#endif + +#ifdef BOARD_BATTERY_ADC_VOLTAGE_FILTER_S + AlphaFilter _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S}; +#endif + +#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S + AlphaFilter _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S}; +#endif + void updateTopics(); }; diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e5bdb947aacb..cf69743af66a 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -44,7 +44,6 @@ #include "UserModeIntention.hpp" #include "worker_thread.hpp" -#include #include #include #include diff --git a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp index 14873831fcee..075401e7a89f 100644 --- a/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/airspeed/airspeed_fusion.cpp @@ -97,7 +97,6 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed) _aid_src_airspeed.innovation_rejected; // TODO: remove this redundant flag const bool continuing_conditions_passing = _control_status.flags.in_air - && _control_status.flags.fixed_wing && !_control_status.flags.fake_pos; const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr; diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 6ab803656076..e6cd566a3fef 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -78,7 +78,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements + // If we are supposed to be using range finder data but have bad range measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air && _range_sensor.isRegularlySendingData() diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 1589047e818d..85d075e0f5cf 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -573,10 +573,10 @@ union filter_control_status_u { uint64_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended uint64_t in_air : 1; ///< 7 - true when the vehicle is airborne uint64_t wind : 1; ///< 8 - true when wind velocity is being estimated - uint64_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference + uint64_t baro_hgt : 1; ///< 9 - true when baro data is being fused uint64_t rng_hgt : - 1; ///< 10 - true when range finder height is being fused as a primary height reference - uint64_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference + 1; ///< 10 - true when range finder data is being fused for height aiding + uint64_t gps_hgt : 1; ///< 11 - true when GPS altitude is being fused uint64_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended uint64_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended uint64_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index a6872b40d257..664613a2b659 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -345,7 +345,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); - // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index e878d100dc9a..121230027831 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -240,7 +240,10 @@ class EstimatorInterface const matrix::Quatf &getQuaternion() const { return _output_predictor.getQuaternion(); } float getUnaidedYaw() const { return _output_predictor.getUnaidedYaw(); } Vector3f getVelocity() const { return _output_predictor.getVelocity(); } - const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } + + // get the mean velocity derivative in earth frame since last reset (see `resetVelocityDerivativeAccumulation()`) + Vector3f getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } + void resetVelocityDerivativeAccumulation() { return _output_predictor.resetVelocityDerivativeAccumulation(); } float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } Vector3f getPosition() const; LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); } diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index 5657d68a2029..05c452b05d80 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -109,7 +109,8 @@ void OutputPredictor::reset() _R_to_earth_now.setIdentity(); _vel_imu_rel_body_ned.setZero(); - _vel_deriv.setZero(); + _delta_vel_sum.setZero(); + _delta_vel_dt = 0.f; _delta_angle_corr.setZero(); @@ -210,9 +211,8 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector delta_vel_earth(2) += CONSTANTS_ONE_G * delta_velocity_dt; // calculate the earth frame velocity derivatives - if (delta_velocity_dt > 0.001f) { - _vel_deriv = delta_vel_earth / delta_velocity_dt; - } + _delta_vel_sum += delta_vel_earth; + _delta_vel_dt += delta_velocity_dt; // save the previous velocity so we can use trapezoidal integration const Vector3f vel_last(_output_new.vel); @@ -389,3 +389,19 @@ void OutputPredictor::applyCorrectionToOutputBuffer(const Vector3f &vel_correcti // update output state to corrected values _output_new = _output_buffer.get_newest(); } + +matrix::Vector3f OutputPredictor::getVelocityDerivative() const +{ + if (_delta_vel_dt > FLT_EPSILON) { + return _delta_vel_sum / _delta_vel_dt; + + } else { + return matrix::Vector3f(0.f, 0.f, 0.f); + } +} + +void OutputPredictor::resetVelocityDerivativeAccumulation() +{ + _delta_vel_dt = 0.f; + _delta_vel_sum.setZero(); +} diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 602993b53309..e90bdf94812e 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -101,8 +101,10 @@ class OutputPredictor // get the velocity of the body frame origin in local NED earth frame matrix::Vector3f getVelocity() const { return _output_new.vel - _vel_imu_rel_body_ned; } - // get the velocity derivative in earth frame - const matrix::Vector3f &getVelocityDerivative() const { return _vel_deriv; } + // get the mean velocity derivative in earth frame since reset (see `resetVelocityDerivativeAccumulation()`) + matrix::Vector3f getVelocityDerivative() const; + + void resetVelocityDerivativeAccumulation(); // get the derivative of the vertical position of the body frame origin in local NED earth frame float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } @@ -178,7 +180,8 @@ class OutputPredictor outputVert _output_vert_new{}; // vertical filter output on the non-delayed time horizon matrix::Matrix3f _R_to_earth_now{}; // rotation matrix from body to earth frame at current time matrix::Vector3f _vel_imu_rel_body_ned{}; // velocity of IMU relative to body origin in NED earth frame - matrix::Vector3f _vel_deriv{}; // velocity derivative at the IMU in NED earth frame (m/s/s) + matrix::Vector3f _delta_vel_sum{}; // accumulation of delta velocity at the IMU in NED earth frame (m/s/s) + float _delta_vel_dt{}; // duration of delta velocity integration (s) // output predictor states matrix::Vector3f _delta_angle_corr{}; ///< delta angle correction vector (rad) diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index afb845a662ec..d00502228964 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -63,13 +63,12 @@ void Ekf::controlTerrainFakeFusion() initTerrain(); } - if (!_control_status.flags.in_air - && !_control_status.flags.rng_terrain - && !_control_status.flags.opt_flow_terrain) { - - bool recent_terrain_aiding = isRecent(_time_last_terrain_fuse, (uint64_t)1e6); + if (!_control_status.flags.in_air) { + bool no_terrain_aiding = !_control_status.flags.rng_terrain + && !_control_status.flags.opt_flow_terrain + && isTimedOut(_time_last_terrain_fuse, (uint64_t)1e6); - if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { + if (no_terrain_aiding && (_height_sensor_ref != HeightSensor::RANGE)) { initTerrain(); } } diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 93c9ed617a0f..c85527b9121a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1556,6 +1556,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) // Acceleration of body origin in local frame const Vector3f vel_deriv{_ekf.getVelocityDerivative()}; + _ekf.resetVelocityDerivativeAccumulation(); lpos.ax = vel_deriv(0); lpos.ay = vel_deriv(1); lpos.az = vel_deriv(2); diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index 7e200c4c54ab..3761758282e4 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -62,3 +62,4 @@ px4_add_unit_gtest(SRC test_EKF_yaw_estimator.cpp LINKLIBS ecl_EKF ecl_sensor_si px4_add_unit_gtest(SRC test_EKF_yaw_fusion_generated.cpp LINKLIBS ecl_EKF ecl_test_helper) px4_add_unit_gtest(SRC test_SensorRangeFinder.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_drag_fusion.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_grounded.cpp LINKLIBS ecl_EKF ecl_sensor_sim) diff --git a/src/modules/ekf2/test/test_EKF_grounded.cpp b/src/modules/ekf2/test/test_EKF_grounded.cpp new file mode 100644 index 000000000000..14f3b6ad52d1 --- /dev/null +++ b/src/modules/ekf2/test/test_EKF_grounded.cpp @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Test EKF altitude estimation while grounded and using Rangefinder as primary alt source + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" + +class EkfGroundedTest : public ::testing::Test +{ +public: + + EkfGroundedTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + void SetUp() override + { + // Init EKF + _ekf->init(0); + _sensor_simulator.runSeconds(0.1); + _ekf->set_in_air_status(false); + _ekf->set_vehicle_at_rest(true); + + _sensor_simulator._rng.setData(0.1f, 100); + _sensor_simulator._rng.setLimits(0.1f, 25.f); + + _sensor_simulator.startGps(); + _sensor_simulator.startBaro(); + _sensor_simulator.startRangeFinder(); + + // Set Range as primary height source + _ekf_wrapper.setRangeHeightRef(); + + // Enable fusion for all height sources + _ekf_wrapper.enableBaroHeightFusion(); + _ekf_wrapper.enableGpsHeightFusion(); + _ekf_wrapper.enableRangeHeightFusion(); + + // Give EKF time for GPS + _sensor_simulator.runSeconds(20); + } +}; + +TEST_F(EkfGroundedTest, rangeFinderOnGround) +{ + EXPECT_TRUE(_ekf->getHeightSensorRef() == HeightSensor::RANGE); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); + + float distance = 0.17f; + _sensor_simulator._rng.setData(distance, 100); + + _sensor_simulator.runSeconds(60); + + EXPECT_NEAR(_ekf->getLatLonAlt().altitude(), 0, 0.5f); +} diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 988853fdeaaf..9057d792b996 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -214,9 +214,6 @@ void FixedwingAttitudeControl::Run() _last_run = time_now_us; } - vehicle_angular_velocity_s angular_velocity{}; - _vehicle_rates_sub.copy(&angular_velocity); - if (_vehicle_status.is_vtol_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -324,22 +321,22 @@ void FixedwingAttitudeControl::Run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) { - const Eulerf setpoint(Quatf(_att_sp.q_d)); - const float roll_body = setpoint.phi(); - const float pitch_body = setpoint.theta(); + const Quatf q_sp(_att_sp.q_d); - if (PX4_ISFINITE(roll_body) && PX4_ISFINITE(pitch_body)) { + if (q_sp.isAllFinite()) { + const Eulerf euler_sp(q_sp); + const float roll_sp = euler_sp.phi(); + const float pitch_sp = euler_sp.theta(); - _roll_ctrl.control_roll(roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _roll_ctrl.control_roll(roll_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _pitch_ctrl.control_pitch(pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _pitch_ctrl.control_pitch(pitch_sp, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta()); - _yaw_ctrl.control_yaw(roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), + _yaw_ctrl.control_yaw(roll_sp, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(), euler_angles.theta(), get_airspeed_constrained()); if (wheel_control) { - Eulerf attitude_setpoint(Quatf(_att_sp.q_d)); - _wheel_ctrl.control_attitude(attitude_setpoint.psi(), euler_angles.psi()); + _wheel_ctrl.control_attitude(euler_sp.psi(), euler_angles.psi()); } else { _wheel_ctrl.reset_integrator(); @@ -394,10 +391,13 @@ void FixedwingAttitudeControl::Run() wheel_u = _manual_control_setpoint.yaw; } else { + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + // XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from // position controller during auto modes _manual_control_setpoint.r gets passed // whenever nudging is enabled, otherwise zero - const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, euler_angles.psi(), _groundspeed, + const float wheel_controller_output = _wheel_ctrl.control_bodyrate(dt, angular_velocity.xyz[2], _groundspeed, groundspeed_scale); wheel_u = wheel_control ? wheel_controller_output + _att_sp.yaw_sp_move_rate : 0.f; } diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index ec8157034baa..4ed4a353ab22 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -71,9 +71,13 @@ bool FixedwingLandDetector::_get_landed_state() } else if (hrt_elapsed_time(&_vehicle_local_position.timestamp) < 1_s) { - // Horizontal velocity complimentary filter. - float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + - _vehicle_local_position.vy * _vehicle_local_position.vy); + float val = 0.0f; + + if (_vehicle_local_position.v_xy_valid) { + // Horizontal velocity complimentary filter. + val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicle_local_position.vx * _vehicle_local_position.vx + + _vehicle_local_position.vy * _vehicle_local_position.vy); + } if (PX4_ISFINITE(val)) { _velocity_xy_filtered = val; @@ -105,15 +109,26 @@ bool FixedwingLandDetector::_get_landed_state() const float acc_hor = matrix::Vector2f(_acceleration).norm(); _xy_accel_filtered = _xy_accel_filtered * 0.8f + acc_hor * 0.18f; + // Check for angular velocity + const float rot_vel_hor = _angular_velocity.norm(); + val = _velocity_rot_filtered * 0.95f + rot_vel_hor * 0.05f; + + if (PX4_ISFINITE(val)) { + _velocity_rot_filtered = val; + } + // make groundspeed threshold tighter if airspeed is invalid - const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : - _param_lndfw_vel_xy_max.get(); + const float vel_xy_max_threshold = airspeed_invalid ? 0.7f * _param_lndfw_vel_xy_max.get() : + _param_lndfw_vel_xy_max.get(); + + const float max_rotation_threshold = math::radians(_param_lndfw_rot_max.get()) ; // Crude land detector for fixedwing. - landDetected = _airspeed_filtered < _param_lndfw_airspd.get() - && _velocity_xy_filtered < vel_xy_max_threshold - && _velocity_z_filtered < _param_lndfw_vel_z_max.get() - && _xy_accel_filtered < _param_lndfw_xyaccel_max.get(); + landDetected = _airspeed_filtered < _param_lndfw_airspd.get() + && _velocity_xy_filtered < vel_xy_max_threshold + && _velocity_z_filtered < _param_lndfw_vel_z_max.get() + && _xy_accel_filtered < _param_lndfw_xyaccel_max.get() + && _velocity_rot_filtered < max_rotation_threshold; } else { // Control state topic has timed out and we need to assume we're landed. diff --git a/src/modules/land_detector/FixedwingLandDetector.h b/src/modules/land_detector/FixedwingLandDetector.h index 981fdc2fb5f1..3f3fa9ad3546 100644 --- a/src/modules/land_detector/FixedwingLandDetector.h +++ b/src/modules/land_detector/FixedwingLandDetector.h @@ -75,6 +75,7 @@ class FixedwingLandDetector final : public LandDetector float _velocity_xy_filtered{0.0f}; float _velocity_z_filtered{0.0f}; float _xy_accel_filtered{0.0f}; + float _velocity_rot_filtered{0.0f}; DEFINE_PARAMETERS_CUSTOM_PARENT( LandDetector, @@ -82,6 +83,7 @@ class FixedwingLandDetector final : public LandDetector (ParamFloat) _param_lndfw_airspd, (ParamFloat) _param_lndfw_vel_xy_max, (ParamFloat) _param_lndfw_vel_z_max, + (ParamFloat) _param_lndfw_rot_max, (ParamFloat) _param_lndfw_trig_time ); }; diff --git a/src/modules/land_detector/land_detector_params_fw.c b/src/modules/land_detector/land_detector_params_fw.c index 02ab4590321e..6ab78fb8678c 100644 --- a/src/modules/land_detector/land_detector_params_fw.c +++ b/src/modules/land_detector/land_detector_params_fw.c @@ -102,3 +102,14 @@ PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 6.00f); * @group Land Detector */ PARAM_DEFINE_FLOAT(LNDFW_TRIG_TIME, 2.f); + +/** + * Fixed-wing land detector: max rotational speed + * + * Maximum allowed norm of the angular velocity in the landed state. + * + * @unit deg/s + * @decimal 1 + * @group Land Detector + */ +PARAM_DEFINE_FLOAT(LNDFW_ROT_MAX, 0.5f); diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c index 121b365bb55a..1d61a7e00bb2 100644 --- a/src/modules/land_detector/land_detector_params_mc.c +++ b/src/modules/land_detector/land_detector_params_mc.c @@ -76,9 +76,9 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f); PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.5f); /** - * Multicopter max rotation + * Multicopter max rotational speed * - * Maximum allowed angular velocity around each axis allowed in the landed state. + * Maximum allowed norm of the angular velocity (roll, pitch) in the landed state. * * @unit deg/s * @decimal 1 diff --git a/src/modules/navigator/mission_base.h b/src/modules/navigator/mission_base.h index b8093ca01cf9..65a021d45d4a 100644 --- a/src/modules/navigator/mission_base.h +++ b/src/modules/navigator/mission_base.h @@ -483,8 +483,8 @@ class MissionBase : public MissionBlock, public ModuleParams mission_item_s _last_camera_trigger_item {}; mission_item_s _last_speed_change_item {}; - DEFINE_PARAMETERS( - (ParamFloat) _param_mis_dist_1wp, + DEFINE_PARAMETERS_CUSTOM_PARENT( + ModuleParams, (ParamInt) _param_mis_mnt_yaw_ctl ) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index b07542984dec..cbcefcd128fb 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -978,7 +978,7 @@ void MissionBlock::startPrecLand(uint16_t land_precision) _navigator->get_precland()->set_mode(PrecLandMode::Opportunistic); _navigator->get_precland()->on_activation(); - } else { //_mission_item.land_precision == 2 + } else if (_mission_item.land_precision == 2) { _navigator->get_precland()->set_mode(PrecLandMode::Required); _navigator->get_precland()->on_activation(); } diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index c264c6fc88b4..88f4da22499a 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -110,7 +110,7 @@ void RtlDirect::on_active() updateAltToAvoidTerrainCollisionAndRepublishTriplet(_mission_item); } - if (_rtl_state == RTLState::LAND && _param_rtl_pld_md.get() > 0) { + if (_rtl_state == RTLState::LAND && _mission_item.land_precision > 0) { // Need to update the position and type on the current setpoint triplet. _navigator->get_precland()->on_active(); @@ -369,7 +369,9 @@ void RtlDirect::set_rtl_item() _mission_item.land_precision = _param_rtl_pld_md.get(); - startPrecLand(_mission_item.land_precision); + if (_mission_item.land_precision > 0) { + startPrecLand(_mission_item.land_precision); + } mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); diff --git a/src/modules/navigator/rtl_mission_fast_reverse.cpp b/src/modules/navigator/rtl_mission_fast_reverse.cpp index 707219b1b37a..b50d49e50a6c 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.cpp +++ b/src/modules/navigator/rtl_mission_fast_reverse.cpp @@ -269,6 +269,13 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type) _mission_item.altitude = _home_pos_sub.get().alt; _mission_item.altitude_is_relative = false; _navigator->reset_position_setpoint(pos_sp_triplet->previous); + + _mission_item.land_precision = _param_rtl_pld_md.get(); + + if (_mission_item.land_precision > 0) { + startPrecLand(_mission_item.land_precision); + new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND; + } } } } diff --git a/src/modules/navigator/rtl_mission_fast_reverse.h b/src/modules/navigator/rtl_mission_fast_reverse.h index 9a30e6d70de5..0351f8dbf889 100644 --- a/src/modules/navigator/rtl_mission_fast_reverse.h +++ b/src/modules/navigator/rtl_mission_fast_reverse.h @@ -74,4 +74,8 @@ class RtlMissionFastReverse : public RtlBase bool _in_landing_phase{false}; uORB::SubscriptionData _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */ + DEFINE_PARAMETERS_CUSTOM_PARENT( + RtlBase, + (ParamInt) _param_rtl_pld_md + ) }; diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 6fd734cc8e41..951993a7248c 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -139,6 +139,7 @@ PARAM_DEFINE_INT32(RTL_CONE_ANG, 45); * RTL precision land mode * * Use precision landing when doing an RTL landing phase. + * This setting does not apply for RTL destinations planned as part of a mission. * * @value 0 No precision landing * @value 1 Opportunistic precision landing diff --git a/src/modules/simulation/Kconfig b/src/modules/simulation/Kconfig index a417092ed94b..576834846b7c 100644 --- a/src/modules/simulation/Kconfig +++ b/src/modules/simulation/Kconfig @@ -9,6 +9,7 @@ menu "Simulation" select MODULES_SIMULATION_SENSOR_BARO_SIM select MODULES_SIMULATION_SENSOR_GPS_SIM select MODULES_SIMULATION_SENSOR_MAG_SIM + select MODULES_SIMULATION_SYSTEM_POWER_SIMULATOR select MODULES_SIMULATION_SIMULATOR_MAVLINK select MODULES_SIMULATION_SIMULATOR_SIH ---help--- diff --git a/src/modules/simulation/battery_simulator/BatterySimulator.hpp b/src/modules/simulation/battery_simulator/BatterySimulator.hpp index a25151b74989..1765c63188be 100644 --- a/src/modules/simulation/battery_simulator/BatterySimulator.hpp +++ b/src/modules/simulation/battery_simulator/BatterySimulator.hpp @@ -74,8 +74,6 @@ class BatterySimulator : public ModuleBase, public ModuleParam static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz static constexpr uint32_t BATTERY_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / BATTERY_SIMLATOR_SAMPLE_FREQUENCY_HZ; - uORB::Publication _battery_pub{ORB_ID(battery_status)}; - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; diff --git a/src/modules/simulation/system_power_simulator/CMakeLists.txt b/src/modules/simulation/system_power_simulator/CMakeLists.txt new file mode 100644 index 000000000000..1168e557cdb5 --- /dev/null +++ b/src/modules/simulation/system_power_simulator/CMakeLists.txt @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2024 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_module( + MODULE modules__simulation__system_power_simulator + MAIN system_power_simulator + COMPILE_FLAGS + SRCS + SystemPowerSimulator.cpp + SystemPowerSimulator.hpp + DEPENDS + px4_work_queue + ) diff --git a/src/modules/simulation/system_power_simulator/Kconfig b/src/modules/simulation/system_power_simulator/Kconfig new file mode 100644 index 000000000000..498e8a989c17 --- /dev/null +++ b/src/modules/simulation/system_power_simulator/Kconfig @@ -0,0 +1,5 @@ +menuconfig MODULES_SIMULATION_SYSTEM_POWER_SIMULATOR + bool "system_power_simulator" + default n + ---help--- + Enable support for system_power_simulator diff --git a/src/modules/simulation/system_power_simulator/SystemPowerSimulator.cpp b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.cpp new file mode 100644 index 000000000000..2c7671c2d2e8 --- /dev/null +++ b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.cpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "SystemPowerSimulator.hpp" + +SystemPowerSimulator::SystemPowerSimulator() : + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default) +{ +} + +SystemPowerSimulator::~SystemPowerSimulator() +{ + perf_free(_loop_perf); +} + +bool SystemPowerSimulator::init() +{ + ScheduleOnInterval(SYSTEM_POWER_SIMLATOR_SAMPLE_INTERVAL_US); + return true; +} + +void SystemPowerSimulator::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + + const hrt_abstime now_us = hrt_absolute_time(); + + // system power + system_power_s sim_system_power{}; + + sim_system_power.timestamp = now_us; + sim_system_power.voltage5v_v = 5.f; + sim_system_power.usb_connected = false; + sim_system_power.hipower_5v_oc = false; + sim_system_power.periph_5v_oc = false; + sim_system_power.brick_valid = 1; + + _system_power_pub.publish(sim_system_power); + + perf_end(_loop_perf); +} + +int SystemPowerSimulator::task_spawn(int argc, char *argv[]) +{ + SystemPowerSimulator *instance = new SystemPowerSimulator(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int SystemPowerSimulator::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + + +int SystemPowerSimulator::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + + +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("system_power_simulation", "system"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); + + return 0; +} + +extern "C" __EXPORT int system_power_simulator_main(int argc, char *argv[]) +{ + return SystemPowerSimulator::main(argc, argv); +} diff --git a/src/modules/simulation/system_power_simulator/SystemPowerSimulator.hpp b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.hpp new file mode 100644 index 000000000000..476eb0afcb11 --- /dev/null +++ b/src/modules/simulation/system_power_simulator/SystemPowerSimulator.hpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2024 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +class SystemPowerSimulator : public ModuleBase, public px4::ScheduledWorkItem +{ +public: + SystemPowerSimulator(); + ~SystemPowerSimulator() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + bool init(); + +private: + void Run() override; + + static constexpr uint32_t SYSTEM_POWER_SIMLATOR_SAMPLE_FREQUENCY_HZ = 100; // Hz + static constexpr uint32_t SYSTEM_POWER_SIMLATOR_SAMPLE_INTERVAL_US = 1_s / SYSTEM_POWER_SIMLATOR_SAMPLE_FREQUENCY_HZ; + + uORB::Publication _system_power_pub{ORB_ID(system_power)}; + + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; +};