[DO NOT MERGE] v5x debug hacks

boards/px4/fmu-v5x/nuttx-config/nsh/defconfig

@@ -54,9 +54,16 @@ CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3185
 CONFIG_CDCACM_VENDORSTR="Auterion"
 CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_ASSERTIONS=y
+CONFIG_DEBUG_ERROR=y
+CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SCHED=y
+CONFIG_DEBUG_SCHED_ERROR=y
+CONFIG_DEBUG_SCHED_WARN=y
 CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEBUG_WARN=y
 CONFIG_DEFAULT_SMALL=y
 CONFIG_DEV_FIFO_SIZE=0
 CONFIG_DEV_GPIO=y

src/modules/ekf2/EKF2.cpp

@@ -1878,6 +1878,9 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 
 void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 {
+	_accel_cal.cal_available = true;
+	return;
+
 	if (_param_ekf2_aid_mask.get() & MASK_INHIBIT_ACC_BIAS) {
 		_accel_cal.cal_available = false;
 		return;
@@ -1918,6 +1921,9 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 {
+	_gyro_cal.cal_available = true;
+	return;
+
 	static constexpr float max_var_allowed = 1e-3f;
 	static constexpr float max_var_ratio = 1e2f;
 
@@ -1948,6 +1954,9 @@ void EKF2::UpdateGyroCalibration(const hrt_abstime &timestamp)
 
 void EKF2::UpdateMagCalibration(const hrt_abstime &timestamp)
 {
+	_mag_cal.cal_available = true;
+	return;
+
 	// Check if conditions are OK for learning of magnetometer bias values
 	// the EKF is operating in the correct mode and there are no filter faults
 

src/modules/land_detector/LandDetector.cpp

@@ -93,6 +93,15 @@ void LandDetector::Run()
 	actuator_armed_s actuator_armed;
 
 	if (_actuator_armed_sub.update(&actuator_armed)) {
+		if (!_armed && actuator_armed.armed) {
+			// force in air briefly
+			_freefall_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_ground_contact_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_maybe_landed_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_landed_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+			_ground_effect_hysteresis.set_state_and_update(false, actuator_armed.timestamp);
+		}
+
 		_armed = actuator_armed.armed;
 	}
 

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -794,7 +794,9 @@ void VehicleIMU::SensorCalibrationSaveAccel()
 			}
 		}
 
-		if (initialised && ((cal_orig - offset_estimate).longerThan(0.05f) || !_accel_calibration.calibrated())) {
+		if (initialised) {
+			_accel_calibration.set_offset({0.1f, 0.2f, 0.3f}); // hack
+
 			if (_accel_calibration.set_offset(offset_estimate)) {
 				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
 					 _accel_calibration.SensorString(), _instance, _accel_calibration.device_id(),
@@ -846,7 +848,9 @@ void VehicleIMU::SensorCalibrationSaveGyro()
 			}
 		}
 
-		if (initialised && ((cal_orig - offset_estimate).longerThan(0.01f) || !_gyro_calibration.calibrated())) {
+		if (initialised) {
+			_gyro_calibration.set_offset({0.1f, 0.2f, 0.3f}); // hack
+
 			if (_gyro_calibration.set_offset(offset_estimate)) {
 				PX4_INFO("%s %d (%" PRIu32 ") offset committed: [%.3f %.3f %.3f]->[%.3f %.3f %.3f])",
 					 _gyro_calibration.SensorString(), _instance, _gyro_calibration.device_id(),

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -177,7 +177,7 @@ private:
 	InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
 	InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
 
-	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
+	static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{1_s};
 
 	hrt_abstime _in_flight_calibration_check_timestamp_last{0};
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -234,8 +234,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 	// This is a reference variance used to calculate the fraction of learned magnetometer bias that will be used to update the stored value.
 	// Larger values cause a larger fraction of the learned biases to be used.
 	static constexpr float magb_vref = 2.5e-7f;
-	static constexpr float min_var_allowed = magb_vref * 0.01f;
+	// static constexpr float min_var_allowed = magb_vref * 0.01f;
-	static constexpr float max_var_allowed = magb_vref * 100.f;
+	// static constexpr float max_var_allowed = magb_vref * 100.f;
 
 	if (_armed) {
 		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
@@ -251,8 +251,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 				const bool valid = (hrt_elapsed_time(&estimator_sensor_bias.timestamp) < 1_s)
 						   && (estimator_sensor_bias.mag_device_id != 0) &&
 						   estimator_sensor_bias.mag_bias_valid &&
-						   estimator_sensor_bias.mag_bias_stable &&
+						   estimator_sensor_bias.mag_bias_stable;/* &&
-						   (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);
+						   (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed);*/
 
 				if (valid) {
 					// find corresponding mag calibration
@@ -302,6 +302,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 						state_variance(axis_index) = fmaxf(state_variance(axis_index) * (1.f - kalman_gain(axis_index)), 0.f);
 					}
 
+					_calibration[mag_index].set_offset({0.1f, 0.2f, 0.3f}); // hack
+
 					if (_calibration[mag_index].set_offset(mag_cal_offset)) {
 
 						PX4_INFO("%d (%" PRIu32 ") EST:%d offset: [%.2f, %.2f, %.2f]->[%.2f, %.2f, %.2f] (full [%.3f, %.3f, %.3f])",