From a82c0091c1a75f034ccf3dd9ba82cb2b9d73bd68 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 12 May 2022 10:57:00 -0400 Subject: [PATCH] [DO NOT MERGE] v5x debug hacks --- boards/px4/fmu-v5x/nuttx-config/nsh/defconfig | 7 +++++++ platforms/nuttx/NuttX/nuttx | 2 +- src/modules/ekf2/EKF2.cpp | 9 +++++++++ src/modules/land_detector/LandDetector.cpp | 9 +++++++++ src/modules/sensors/vehicle_imu/VehicleIMU.cpp | 8 ++++++-- src/modules/sensors/vehicle_imu/VehicleIMU.hpp | 2 +- .../vehicle_magnetometer/VehicleMagnetometer.cpp | 10 ++++++---- 7 files changed, 39 insertions(+), 8 deletions(-) diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig index 21b259b835..a814ccc4e8 100644 --- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig +++ b/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 diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx index c5c7d2b4f2..22dbf796a3 160000 --- a/platforms/nuttx/NuttX/nuttx +++ b/platforms/nuttx/NuttX/nuttx @@ -1 +1 @@ -Subproject commit c5c7d2b4f26f52f1dfb425ebde83328606b65d4f +Subproject commit 22dbf796a3f61e5edbaae0e2716e1794eb10debe diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 166a08f1af..c92e87023b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1878,6 +1878,9 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps) void EKF2::UpdateAccelCalibration(const hrt_abstime ×tamp) { + _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 ×tamp) void EKF2::UpdateGyroCalibration(const hrt_abstime ×tamp) { + _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 ×tamp) void EKF2::UpdateMagCalibration(const hrt_abstime ×tamp) { + _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 diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp index f16e8ed383..a0d66e3f06 100644 --- a/src/modules/land_detector/LandDetector.cpp +++ b/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; } diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp index 12d1f1a637..0ad77e49a1 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp +++ b/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(), diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp index 2caf491b21..a3249c912e 100644 --- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp +++ b/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}; diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index 53c0407b4e..0abd2f4164 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/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 max_var_allowed = magb_vref * 100.f; + // static constexpr float min_var_allowed = magb_vref * 0.01f; + // 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 && - (bias_variance.min() > min_var_allowed) && (bias_variance.max() < max_var_allowed); + estimator_sensor_bias.mag_bias_stable;/* && + (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])",