Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Dr.-Ing. Amilcar do Carmo Lucas 2024-02-13 16:29:00 +01:00 committed by Andrew Tridgell
parent 87435473b5
commit 39e7e4bed1
2 changed files with 34 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -223,7 +223,7 @@ void AC_PrecLand::init(uint16_t update_rate_hz)
// create inertial history buffer // create inertial history buffer
// constrain lag parameter to be within bounds // 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 // 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); 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 #if HAL_LOGGING_ENABLED
const uint32_t now = AP_HAL::millis(); const uint32_t now = AP_HAL::millis();
if (now - last_log_ms > 40) { // 25Hz if (now - _last_log_ms > 40) { // 25Hz
last_log_ms = now; _last_log_ms = now;
Write_Precland(); Write_Precland();
} }
#endif #endif
@ -423,7 +423,7 @@ bool AC_PrecLand::get_target_position_cm(Vector2f& ret)
return false; return false;
} }
ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm 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; 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) 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()) { switch ((EstimatorType)_estimator_type.get()) {
case EstimatorType::RAW_SENSOR: { case EstimatorType::RAW_SENSOR: {
@ -506,10 +506,10 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
// Predict // Predict
if (target_acquired()) { if (target_acquired()) {
_target_pos_rel_est_NE.x -= inertial_data_delayed->inertialNavVelocity.x * inertial_data_delayed->dt; _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_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.x = -_inertial_data_delayed->inertialNavVelocity.x;
_target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; _target_vel_rel_est_NE.y = -_inertial_data_delayed->inertialNavVelocity.y;
} }
// Update if a new Line-Of-Sight measurement is available // 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.x = _target_pos_rel_meas_NED.x;
_target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y; _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.x = -_inertial_data_delayed->inertialNavVelocity.x;
_target_vel_rel_est_NE.y = -inertial_data_delayed->inertialNavVelocity.y; _target_vel_rel_est_NE.y = -_inertial_data_delayed->inertialNavVelocity.y;
_last_update_ms = AP_HAL::millis(); _last_update_ms = AP_HAL::millis();
_target_acquired = true; _target_acquired = true;
@ -536,8 +536,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
case EstimatorType::KALMAN_FILTER: { case EstimatorType::KALMAN_FILTER: {
// Predict // Predict
if (target_acquired() || _estimator_initialized) { if (target_acquired() || _estimator_initialized) {
const float& dt = inertial_data_delayed->dt; const float& dt = _inertial_data_delayed->dt;
const Vector3f& vehicleDelVel = inertial_data_delayed->correctedVehicleDeltaVelocityNED; const Vector3f& vehicleDelVel = _inertial_data_delayed->correctedVehicleDeltaVelocityNED;
_ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt); _ekf_x.predict(dt, -vehicleDelVel.x, _accel_noise*dt);
_ekf_y.predict(dt, -vehicleDelVel.y, _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"); 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 // start init of EKF. We will let the filter consume the data for a while before it available for consumption
// reset filter state // reset filter state
if (inertial_data_delayed->inertialNavVelocityValid) { 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_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)); _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -_inertial_data_delayed->inertialNavVelocity.y, sq(2.0f));
} else { } else {
_ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f)); _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)); _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) 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) { const uint32_t los_meas_time_ms = _backend->los_meas_time_ms();
_last_backend_los_meas_ms = _backend->los_meas_time_ms(); if (los_meas_time_ms != _last_backend_los_meas_ms && _backend->get_los_body(target_vec_unit_body)) {
_backend->get_los_body(target_vec_unit_body); _last_backend_los_meas_ms = los_meas_time_ms;
if (!is_zero(_yaw_align)) { if (!is_zero(_yaw_align)) {
// Apply sensor yaw alignment rotation // Apply sensor yaw alignment rotation
target_vec_unit_body.rotate_xy(radians(_yaw_align*0.01f)); 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; return true;
} else {
return false;
} }
return false;
} }
bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid) bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, bool rangefinder_alt_valid)
{ {
Vector3f target_vec_unit_body; Vector3f target_vec_unit_body;
if (retrieve_los_meas(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 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 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 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); const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
if (target_vec_valid && alt_valid) { if (target_vec_valid && alt_valid) {
// distance to target and distance to target along approach vector // 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()) { if (!_cam_offset.get().is_zero()) {
// user has specifed offset for camera // user has specifed offset for camera
// take its height into account while calculating distance // 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) { if (_backend->distance_to_target() > 0.0f) {
// sensor has provided distance to landing target // 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 // 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; const Vector3f cam_pos_ned_rel_imu = cam_pos_ned - accel_pos_ned;
// Compute target position relative to IMU // 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; _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 // 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.x -= cam_pos_horizontal_ned.x;
_target_pos_rel_out_NE.y -= cam_pos_horizontal_ned.y; _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)); UNUSED_RESULT(_ahrs.get_velocity_NED(_last_veh_velocity_NED_ms));
// Apply land offset // 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.x += land_ofs_ned_m.x;
_target_pos_rel_out_NE.y += land_ofs_ned_m.y; _target_pos_rel_out_NE.y += land_ofs_ned_m.y;

13
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -203,17 +203,17 @@ private:
AP_Int8 _retry_max; // PrecLand Maximum number of retires to a failed landing 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_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_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_min_alt; // PrecLand minimum height required for detecting target
AP_Float _sensor_max_alt; // PrecLand maximum height the sensor can detect target AP_Float _sensor_max_alt; // PrecLand maximum height the sensor can detect target
AP_Int16 _options; // Bitmask for extra options AP_Int16 _options; // Bitmask for extra options
AP_Enum<Rotation> _orient; // Orientation of camera/sensor AP_Enum<Rotation> _orient; // Orientation of camera/sensor
uint32_t _last_update_ms; // system time in millisecond when update was last called 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 _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 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 _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_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 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) 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; uint64_t time_usec;
}; };
ObjectArray<inertial_data_frame_s> *_inertial_history; ObjectArray<inertial_data_frame_s> *_inertial_history;
struct inertial_data_frame_s *_inertial_data_delayed;
// backend state // backend state
struct precland_state { struct precland_state {
@ -251,7 +252,7 @@ private:
// write out PREC message to log: // write out PREC message to log:
void Write_Precland(); 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 static AC_PrecLand *_singleton; //singleton
}; };