Merge branch 'main' into dev/sbgecom_driver

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

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

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 */

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

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

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

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);

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)
 	{

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();

src/modules/battery_status/analog_battery.h

@@ -34,6 +34,7 @@
 #pragma once
 
 #include <battery/battery.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
 #include <parameters/param.h>
 #include <uORB/topics/vehicle_status.h>
 
@@ -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<float> _voltage_filter {BOARD_BATTERY_ADC_VOLTAGE_FILTER_S};
+#endif
+
+#ifdef BOARD_BATTERY_ADC_CURRENT_FILTER_S
+	AlphaFilter<float> _current_filter {BOARD_BATTERY_ADC_CURRENT_FILTER_S};
+#endif
+
 	void updateTopics();
 };

src/modules/commander/Commander.hpp

@@ -44,7 +44,6 @@
 #include "UserModeIntention.hpp"
 #include "worker_thread.hpp"
 
-#include <lib/controllib/blocks.hpp>
 #include <lib/hysteresis/hysteresis.h>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>

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;

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()

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

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;

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(); }

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();
+}

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)

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();
 		}
 	}

src/modules/ekf2/EKF2.cpp

@@ -1556,6 +1556,7 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 
 	// 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);

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)