diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 652eff1262..a85db23f6c 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -89,7 +89,16 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // @Values: -1:DefaultBus,0:InternalI2C,1:ExternalI2C // @User: Advanced AP_GROUPINFO("BUS", 8, AC_PrecLand, _bus, -1), - + + // @Param: BUFFER + // @DisplayName: Inertial History Buffer + // @Description: Length of inertial history buffer in ms, to cope with variable landing_target latency + // @Range: 1 250 + // @Increment: 1 + // @User: Advanced + // @Units: ms + AP_GROUPINFO("BUFFER", 9, AC_PrecLand, _inertial_buffer_length, 20), // 20ms is the old default buffer size (8 frames @ 400hz/2.5ms) + AP_GROUPEND }; @@ -115,6 +124,13 @@ void AC_PrecLand::init() _backend = nullptr; _backend_state.healthy = false; + // Create inertial history buffer + inertial_buffer(); + /* NOTE: If this check below returns without creating buffer and backend then SITL crashes */ + if (_inertial_history == nullptr) { + return; // allocation failed + } + // instantiate backend based on type parameter switch ((enum PrecLandType)(_type.get())) { // no type defined @@ -145,9 +161,30 @@ void AC_PrecLand::init() } } +// inertial_buffer - create/update inertial buffer +void AC_PrecLand::inertial_buffer() +{ + // If PLND_LATENCY parameter is not within bounds, return without doing anything + if ( _inertial_buffer_length <= 0 || _inertial_buffer_length > 250 ) { + return; + } + + // calculate inertial buffer size from delay length (loop runs at 400hz) + _inertial_buffer_size = (uint16_t)roundf(_inertial_buffer_length / (1000/400.0f)); + + // Create buffer if it doesn't exist or if latency parameter has changed + if (_inertial_history == nullptr || _inertial_buffer_size != _inertial_history->size()) { + // instantiate ring buffer to hold inertial history + _inertial_history = new ObjectArray(_inertial_buffer_size); + } +} + // update - give chance to driver to get updates from sensor void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) { + // Recreate inertial buffer if latency parameter has changed + inertial_buffer(); + // append current velocity and attitude correction into history buffer struct inertial_data_frame_s inertial_data_newest; const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); @@ -163,7 +200,8 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) curr_vel.z = -curr_vel.z; // NED to NEU inertial_data_newest.inertialNavVelocity = curr_vel; - _inertial_history.push_back(inertial_data_newest); + inertial_data_newest.time_usec = AP_HAL::micros64(); + _inertial_history->push_force(inertial_data_newest); // update estimator of target position if (_backend != nullptr && _enabled) { @@ -231,14 +269,14 @@ void AC_PrecLand::handle_msg(mavlink_message_t* msg) void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid) { - const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front(); + const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; switch (_estimator_type) { case ESTIMATOR_TYPE_RAW_SENSOR: { // Return if there's any invalid velocity data - for (uint8_t i=0; i<_inertial_history.size(); i++) { - const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i); - if (!inertial_data.inertialNavVelocityValid) { + for (uint8_t i=0; i<_inertial_history->available(); i++) { + const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i]; + if (!inertial_data->inertialNavVelocityValid) { _target_acquired = false; return; } @@ -246,18 +284,18 @@ 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 LOS measurement is available if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { _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; @@ -272,8 +310,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va case ESTIMATOR_TYPE_KALMAN_FILTER: { // Predict if (target_acquired()) { - 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); @@ -284,9 +322,9 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); if (!target_acquired()) { // 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)); @@ -348,9 +386,9 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, { Vector3f target_vec_unit_body; if (retrieve_los_meas(target_vec_unit_body)) { - const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front(); + const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0]; - Vector3f target_vec_unit_ned = inertial_data_delayed.Tbn * target_vec_unit_body; + Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body; bool target_vec_valid = target_vec_unit_ned.z > 0.0f; bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f); if (target_vec_valid && alt_valid) { @@ -365,7 +403,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, // Compute camera position relative to IMU Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); - Vector3f cam_pos_ned = inertial_data_delayed.Tbn * (_cam_offset.get() - accel_body_offset); + Vector3f cam_pos_ned = inertial_data_delayed->Tbn * (_cam_offset.get() - accel_body_offset); // Compute target position relative to IMU _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt) + cam_pos_ned; @@ -381,17 +419,17 @@ void AC_PrecLand::run_output_prediction() _target_vel_rel_out_NE = _target_vel_rel_est_NE; // Predict forward from delayed time horizon - for (uint8_t i=1; i<_inertial_history.size(); i++) { - const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i); - _target_vel_rel_out_NE.x -= inertial_data.correctedVehicleDeltaVelocityNED.x; - _target_vel_rel_out_NE.y -= inertial_data.correctedVehicleDeltaVelocityNED.y; - _target_pos_rel_out_NE.x += _target_vel_rel_out_NE.x * inertial_data.dt; - _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data.dt; + for (uint8_t i=1; 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; + _target_pos_rel_out_NE.x += _target_vel_rel_out_NE.x * inertial_data->dt; + _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data->dt; } const AP_AHRS &_ahrs = AP::ahrs(); - const Matrix3f& Tbn = _inertial_history.peek(_inertial_history.size()-1).Tbn; + const Matrix3f& Tbn = (*_inertial_history)[_inertial_history->available()-1]->Tbn; Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(_ahrs.get_primary_accel_index()); // Apply position correction for CG offset from IMU diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index ed2ccb2858..e0b3bdc73a 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -5,7 +5,7 @@ #include #include #include "PosVelEKF.h" -#include +#include #include // declare backend classes @@ -59,6 +59,9 @@ public: // returns time of last update uint32_t last_update_ms() const { return _last_update_ms; } + // create or update the inertial buffer + void inertial_buffer(); + // returns time of last time target was seen uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; } @@ -119,12 +122,14 @@ private: AP_Int8 _type; // precision landing sensor type AP_Int8 _bus; // which sensor bus AP_Int8 _estimator_type; // precision landing estimator type + AP_Int16 _inertial_buffer_length; // inertial buffer length in ms AP_Float _yaw_align; // Yaw angle from body x-axis to sensor x-axis. AP_Float _land_ofs_cm_x; // Desired landing position of the camera forward of the target in vehicle body frame AP_Float _land_ofs_cm_y; // Desired landing position of the camera right of the target in vehicle body frame AP_Float _accel_noise; // accelometer process noise AP_Vector3f _cam_offset; // Position of the camera relative to the CG + uint16_t _inertial_buffer_size; // inertial buffer queue length, calculated from _inertial_buffer_length uint32_t _last_update_ms; // system time in millisecond when update was last called bool _target_acquired; // true if target has been seen recently uint32_t _last_backend_los_meas_ms; // system time target was last seen @@ -140,15 +145,16 @@ private: Vector2f _target_pos_rel_out_NE; // target's position relative to the camera, fed into position controller Vector2f _target_vel_rel_out_NE; // target's velocity relative to the CG, fed into position controller - // structure and buffer to hold a short history of vehicle velocity + // structure and buffer to hold a history of vehicle velocity struct inertial_data_frame_s { Matrix3f Tbn; // dcm rotation matrix to rotate body frame to north Vector3f correctedVehicleDeltaVelocityNED; Vector3f inertialNavVelocity; bool inertialNavVelocityValid; float dt; + uint64_t time_usec; }; - AP_Buffer _inertial_history; + ObjectArray *_inertial_history; // backend state struct precland_state {