From 39e7e4bed1371be0ad6b3d8fc84474ff4bb76b89 Mon Sep 17 00:00:00 2001 From: "Dr.-Ing. Amilcar do Carmo Lucas" Date: Tue, 13 Feb 2024 16:29:00 +0100 Subject: [PATCH] AC_PrecLand: NFC move two local instances of inertial_data_delayed to single _inertial_data_delayed member variable - improve code alignment - simplify a return - improve initialization of vectors --- libraries/AC_PrecLand/AC_PrecLand.cpp | 55 +++++++++++++-------------- libraries/AC_PrecLand/AC_PrecLand.h | 13 ++++--- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index ece8653436..8a64f5625f 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -223,7 +223,7 @@ void AC_PrecLand::init(uint16_t update_rate_hz) // create inertial history buffer // constrain lag parameter to be within bounds - _lag.set(constrain_float(_lag, 0.02f, 0.25f)); + _lag.set(constrain_float(_lag, 0.02f, 0.25f)); // must match LAG parameter range at line 124 // calculate inertial buffer size from lag and minimum of main loop rate and update_rate_hz argument const uint16_t inertial_buffer_size = MAX((uint16_t)roundf(_lag * MIN(update_rate_hz, AP::scheduler().get_loop_rate_hz())), 1); @@ -312,8 +312,8 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) #if HAL_LOGGING_ENABLED const uint32_t now = AP_HAL::millis(); - if (now - last_log_ms > 40) { // 25Hz - last_log_ms = now; + if (now - _last_log_ms > 40) { // 25Hz + _last_log_ms = now; Write_Precland(); } #endif @@ -423,7 +423,7 @@ bool AC_PrecLand::get_target_position_cm(Vector2f& ret) return false; } ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm - ret.y = (_target_pos_rel_out_NE.y + curr_pos.y) * 100.0f; // m to cm + ret.y = (_target_pos_rel_out_NE.y + curr_pos.y) * 100.0f; // m to cm return true; } @@ -491,7 +491,7 @@ void AC_PrecLand::handle_msg(const mavlink_landing_target_t &packet, uint32_t ti void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid) { - const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; + _inertial_data_delayed = (*_inertial_history)[0]; switch ((EstimatorType)_estimator_type.get()) { case EstimatorType::RAW_SENSOR: { @@ -506,10 +506,10 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va // Predict if (target_acquired()) { - _target_pos_rel_est_NE.x -= inertial_data_delayed->inertialNavVelocity.x * inertial_data_delayed->dt; - _target_pos_rel_est_NE.y -= inertial_data_delayed->inertialNavVelocity.y * inertial_data_delayed->dt; - _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x; - _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; + _target_pos_rel_est_NE.x -= _inertial_data_delayed->inertialNavVelocity.x * _inertial_data_delayed->dt; + _target_pos_rel_est_NE.y -= _inertial_data_delayed->inertialNavVelocity.y * _inertial_data_delayed->dt; + _target_vel_rel_est_NE.x = -_inertial_data_delayed->inertialNavVelocity.x; + _target_vel_rel_est_NE.y = -_inertial_data_delayed->inertialNavVelocity.y; } // Update if a new Line-Of-Sight measurement is available @@ -520,8 +520,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va } _target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x; _target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y; - _target_vel_rel_est_NE.x = -inertial_data_delayed->inertialNavVelocity.x; - _target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; + _target_vel_rel_est_NE.x = -_inertial_data_delayed->inertialNavVelocity.x; + _target_vel_rel_est_NE.y = -_inertial_data_delayed->inertialNavVelocity.y; _last_update_ms = AP_HAL::millis(); _target_acquired = true; @@ -536,8 +536,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va case EstimatorType::KALMAN_FILTER: { // Predict if (target_acquired() || _estimator_initialized) { - const float& dt = inertial_data_delayed->dt; - const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED; + const float& dt = _inertial_data_delayed->dt; + const Vector3f& vehicleDelVel = _inertial_data_delayed->correctedVehicleDeltaVelocityNED; _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt); _ekf_y.predict(dt, -vehicleDelVel.y, _accel_noise*dt); @@ -551,9 +551,9 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va gcs().send_text(MAV_SEVERITY_INFO, "PrecLand: Target Found"); // start init of EKF. We will let the filter consume the data for a while before it available for consumption // reset filter state - if (inertial_data_delayed->inertialNavVelocityValid) { - _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.x, sq(2.0f)); - _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed->inertialNavVelocity.y, sq(2.0f)); + if (_inertial_data_delayed->inertialNavVelocityValid) { + _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -_inertial_data_delayed->inertialNavVelocity.x, sq(2.0f)); + _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -_inertial_data_delayed->inertialNavVelocity.y, sq(2.0f)); } else { _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f)); _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f)); @@ -615,9 +615,9 @@ void AC_PrecLand::check_ekf_init_timeout() bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body) { - if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) { - _last_backend_los_meas_ms = _backend->los_meas_time_ms(); - _backend->get_los_body(target_vec_unit_body); + const uint32_t los_meas_time_ms = _backend->los_meas_time_ms(); + if (los_meas_time_ms != _last_backend_los_meas_ms && _backend->get_los_body(target_vec_unit_body)) { + _last_backend_los_meas_ms = los_meas_time_ms; if (!is_zero(_yaw_align)) { // Apply sensor yaw alignment rotation target_vec_unit_body.rotate_xy(radians(_yaw_align*0.01f)); @@ -638,20 +638,19 @@ bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body) } return true; - } else { - return false; } + return false; } bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid) { Vector3f target_vec_unit_body; if (retrieve_los_meas(target_vec_unit_body)) { - const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; + _inertial_data_delayed = (*_inertial_history)[0]; const bool target_vec_valid = target_vec_unit_body.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f; - const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; - const Vector3f approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body; + const Vector3f target_vec_unit_ned = _inertial_data_delayed->Tbn * target_vec_unit_body; + const Vector3f approach_vector_NED = _inertial_data_delayed->Tbn * _approach_vector_body; const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); if (target_vec_valid && alt_valid) { // distance to target and distance to target along approach vector @@ -661,7 +660,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, if (!_cam_offset.get().is_zero()) { // user has specifed offset for camera // take its height into account while calculating distance - cam_pos_ned = inertial_data_delayed->Tbn * _cam_offset; + cam_pos_ned = _inertial_data_delayed->Tbn * _cam_offset; } if (_backend->distance_to_target() > 0.0f) { // sensor has provided distance to landing target @@ -674,7 +673,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, } // Compute camera position relative to IMU - const Vector3f accel_pos_ned = inertial_data_delayed->Tbn * AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); + const Vector3f accel_pos_ned = _inertial_data_delayed->Tbn * AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); const Vector3f cam_pos_ned_rel_imu = cam_pos_ned - accel_pos_ned; // Compute target position relative to IMU @@ -718,7 +717,7 @@ void AC_PrecLand::run_output_prediction() _target_pos_rel_out_NE.y += imu_pos_ned.y; // Apply position correction for body-frame horizontal camera offset from CG, so that vehicle lands lens-to-target - Vector3f cam_pos_horizontal_ned = Tbn * Vector3f(_cam_offset.get().x, _cam_offset.get().y, 0); + Vector3f cam_pos_horizontal_ned = Tbn * Vector3f{_cam_offset.get().x, _cam_offset.get().y, 0}; _target_pos_rel_out_NE.x -= cam_pos_horizontal_ned.x; _target_pos_rel_out_NE.y -= cam_pos_horizontal_ned.y; @@ -731,7 +730,7 @@ void AC_PrecLand::run_output_prediction() UNUSED_RESULT(_ahrs.get_velocity_NED(_last_veh_velocity_NED_ms)); // Apply land offset - Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f; + Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f{_land_ofs_cm_x, _land_ofs_cm_y, 0} * 0.01f; _target_pos_rel_out_NE.x += land_ofs_ned_m.x; _target_pos_rel_out_NE.y += land_ofs_ned_m.y; diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index e11a51f81d..24ebdd2a49 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -203,17 +203,17 @@ private: AP_Int8 _retry_max; // PrecLand Maximum number of retires to a failed landing AP_Float _retry_timeout_sec; // Time for which vehicle continues descend even if target is lost. After this time period, vehicle will attempt a landing retry depending on PLND_STRICT param. AP_Int8 _retry_behave; // Action to do when trying a landing retry - AP_Float _sensor_min_alt; // PrecLand minimum height required for detecting target - AP_Float _sensor_max_alt; // PrecLand maximum height the sensor can detect target - AP_Int16 _options; // Bitmask for extra options - AP_Enum _orient; // Orientation of camera/sensor + AP_Float _sensor_min_alt; // PrecLand minimum height required for detecting target + AP_Float _sensor_max_alt; // PrecLand maximum height the sensor can detect target + AP_Int16 _options; // Bitmask for extra options + AP_Enum _orient; // Orientation of camera/sensor uint32_t _last_update_ms; // system time in millisecond when update was last called bool _target_acquired; // true if target has been seen recently after estimator is initialized bool _estimator_initialized; // true if estimator has been initialized after few seconds of the target being detected by sensor uint32_t _estimator_init_ms; // system time in millisecond when EKF was init uint32_t _last_backend_los_meas_ms; // system time target was last seen - uint32_t _last_valid_target_ms; // last time PrecLand library had a output of the landing target position + uint32_t _last_valid_target_ms; // last time PrecLand library had a output of the landing target position PosVelEKF _ekf_x, _ekf_y; // Kalman Filter for x and y axis uint32_t _outlier_reject_count; // mini-EKF's outlier counter (3 consecutive outliers lead to EKF accepting updates) @@ -242,6 +242,7 @@ private: uint64_t time_usec; }; ObjectArray *_inertial_history; + struct inertial_data_frame_s *_inertial_data_delayed; // backend state struct precland_state { @@ -251,7 +252,7 @@ private: // write out PREC message to log: void Write_Precland(); - uint32_t last_log_ms; // last time we logged + uint32_t _last_log_ms; // last time we logged static AC_PrecLand *_singleton; //singleton };