Skip to content

Commit

Permalink
AC_PrecLand: Use sensor timestamp to match inertial frame corrections
Browse files Browse the repository at this point in the history 
Co-authored-by: Dr.-Ing. Amilcar do Carmo Lucas <amilcar.lucas@iav.de>

If the sensor sets the timestamp with the sensor frame then it attempts to match it against inertial frame timestamps.
Sensor frames can have widely varying latencies so this is essential to safely correlate the attitude compensation against any given sensor reading.
This effectively auto-detects the sensor latency and makes the PLND_LAG manual setting unnecessary.
The only caveat is that PLND_LAG must be set large enough to hold the biggest sensor latency.
So if used with a sensor that supports time-stamping PLND_LAG should be set to something large like 0.250 (s).

- Move inertial_data_delayed to _inertial_data_delayed
- Separate out sync_frames logic, make _inertial_data_delayed class struct
- Run forward prediction from synced frame
- Remove _ prefixes in non-member (local) variable's names
  • Loading branch information
@amilcarlucas
amilcarlucas committed Feb 13, 2024
1 parent e58dd0d commit b3248ed
Show file tree
Hide file tree
Showing 2 changed files with 65 additions and 27 deletions.
87 changes: 60 additions & 27 deletions libraries/AC_PrecLand/AC_PrecLand.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -491,7 +491,9 @@ 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];
// Set inertial_data_delayed to the oldest sensor data in the buffer, by default.
// construct_pos_meas_using_rangefinder() is then called which attempts to match timestamped
// sensor frame and overwrites _target_pos_rel_meas_NED calculated from matched inertial frame, if available

switch ((EstimatorType)_estimator_type.get()) {
case EstimatorType::RAW_SENSOR: {
Expand All @@ -506,10 +508,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
Expand All @@ -520,8 +522,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;
Expand All @@ -536,8 +538,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);
Expand All @@ -551,9 +553,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));
Expand Down Expand Up @@ -615,9 +617,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));
Expand All @@ -638,20 +640,20 @@ 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];
// Match sensor and inertial frames or push front of buffer, into _inertial_data_delayed
// If frame sync fails (rather than just no sensor timestamp) then skip the update
if (retrieve_los_meas(target_vec_unit_body) && sync_frames(_last_backend_los_meas_ms)) {

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
Expand All @@ -661,7 +663,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
Expand All @@ -674,7 +676,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
Expand All @@ -693,13 +695,44 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
return false;
}

// Set the active inertial frame, based on either sensor timestamp (higher priority) or PLND_LAG parameter
// Return true if sensor does not send timestamp and front of buffer is used, or if sensor timestamp is successfully synced
// Return false otherwise, so invalid sync is not propagated
bool AC_PrecLand::sync_frames(const uint32_t los_meas_time_ms)
{
// Retrieve timestamp from backend measurement. If not zero, attempt to correlate with inertial history.
uint64_t nearest_timestamp = 0;
uint16_t nearest_index = 0;
const uint64_t los_meas_time_us = los_meas_time_ms * 1000;
if (los_meas_time_us != 0) {
// Search through inertial history for the nearest frame to the measurement timestamp
for (uint8_t i=0; i<_inertial_history->available(); i++) {
uint64_t delta_usec = labs((*_inertial_history)[i]->time_usec - los_meas_time_us);
if (!nearest_timestamp || delta_usec < nearest_timestamp) {
nearest_timestamp = delta_usec;
nearest_index = i;
}
}
// If the nearest timestamp delta is longer than a frame, it's after the last frame or before the first, so reject
if (nearest_timestamp > 1000000/AP::scheduler().get_loop_rate_hz()) {
return false;
}
_inertial_frame_index = nearest_index;
} else {
// If timestamp isn't set, use the oldest inertial buffer frame
_inertial_frame_index = 0;
}
_inertial_data_delayed = (*_inertial_history)[_inertial_frame_index];
return true;
}

void AC_PrecLand::run_output_prediction()
{
_target_pos_rel_out_NE = _target_pos_rel_est_NE;
_target_vel_rel_out_NE = _target_vel_rel_est_NE;

// Predict forward from delayed time horizon
for (uint8_t i=1; i<_inertial_history->available(); i++) {
for (uint8_t i=1+_inertial_frame_index; i<_inertial_history->available(); i++) {
const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
_target_vel_rel_out_NE.x -= inertial_data->correctedVehicleDeltaVelocityNED.x;
_target_vel_rel_out_NE.y -= inertial_data->correctedVehicleDeltaVelocityNED.y;
Expand All @@ -718,7 +751,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;

Expand All @@ -728,7 +761,7 @@ void AC_PrecLand::run_output_prediction()
_target_vel_rel_out_NE.y -= vel_ned_rel_imu.y;

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

Expand Down
5 changes: 5 additions & 0 deletions libraries/AC_PrecLand/AC_PrecLand.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,9 @@ class AC_PrecLand
// results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_NE
void run_output_prediction();

// Attempt to match sensor frame with inertial frame, otherwise use the front of the inertial buffer (PLND_LAG parameter)
bool sync_frames(const uint32_t los_meas_time_ms);

// parameters
AP_Int8 _enabled; // enabled/disabled
AP_Enum<Type> _type; // precision landing sensor type
Expand Down Expand Up @@ -228,6 +231,8 @@ class AC_PrecLand
uint64_t time_usec;
};
ObjectArray<inertial_data_frame_s> *_inertial_history;
inertial_data_frame_s *_inertial_data_delayed;
uint16_t _inertial_frame_index;

// backend state
struct precland_state {
Expand Down

0 comments on commit b3248ed

Please sign in to comment.