aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_PrecLand: Add PLND_BUFFER parameter and move to AP_HAL::utility::RingBuffer, #6297

Browse Source 
AC_PrecLand: Add PLND_BUFFER parameter and move to AP_HAL::utility::RingBuffer, ##6297

AC_PrecLand: Fixup int type, #8892

AC_PrecLand: Add PLND_BUFFER parameter and move to AP_HAL::utility::RingBuffer, ##6297

Update screwed up submodules from rebase
This commit is contained in:
fnoop 2018-07-11 13:58:38 +01:00 committed by Randy Mackay
parent c02863e001
commit 02da34a246
2 changed files with 74 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -90,6 +90,15 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("BUS", 8, AC_PrecLand, _bus, -1), 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 AP_GROUPEND
}; };
@ -115,6 +124,13 @@ void AC_PrecLand::init()
_backend = nullptr; _backend = nullptr;
_backend_state.healthy = false; _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 // instantiate backend based on type parameter
switch ((enum PrecLandType)(_type.get())) { switch ((enum PrecLandType)(_type.get())) {
// no type defined // 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_data_frame_s>(_inertial_buffer_size);
}
}
// update - give chance to driver to get updates from sensor // update - give chance to driver to get updates from sensor
void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) 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 // append current velocity and attitude correction into history buffer
struct inertial_data_frame_s inertial_data_newest; struct inertial_data_frame_s inertial_data_newest;
const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); 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 curr_vel.z = -curr_vel.z; // NED to NEU
inertial_data_newest.inertialNavVelocity = curr_vel; 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 // update estimator of target position
if (_backend != nullptr && _enabled) { 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) 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) { switch (_estimator_type) {
case ESTIMATOR_TYPE_RAW_SENSOR: { case ESTIMATOR_TYPE_RAW_SENSOR: {
// Return if there's any invalid velocity data // Return if there's any invalid velocity data
for (uint8_t i=0; i<_inertial_history.size(); i++) { for (uint8_t i=0; i<_inertial_history->available(); i++) {
const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i); const struct inertial_data_frame_s *inertial_data = (*_inertial_history)[i];
if (!inertial_data.inertialNavVelocityValid) { if (!inertial_data->inertialNavVelocityValid) {
_target_acquired = false; _target_acquired = false;
return; return;
} }
@ -246,18 +284,18 @@ 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 LOS measurement is available // Update if a new LOS measurement is available
if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { 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.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;
@ -272,8 +310,8 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va
case ESTIMATOR_TYPE_KALMAN_FILTER: { case ESTIMATOR_TYPE_KALMAN_FILTER: {
// Predict // Predict
if (target_acquired()) { if (target_acquired()) {
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);
@ -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); 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()) { if (!target_acquired()) {
// 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));
@ -348,9 +386,9 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
{ {
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.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 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); 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) {
@ -365,7 +403,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
Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); 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 // 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; _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; _target_vel_rel_out_NE = _target_vel_rel_est_NE;
// Predict forward from delayed time horizon // Predict forward from delayed time horizon
for (uint8_t i=1; i<_inertial_history.size(); i++) { for (uint8_t i=1; i<_inertial_history->available(); i++) {
const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(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.x -= inertial_data->correctedVehicleDeltaVelocityNED.x;
_target_vel_rel_out_NE.y -= inertial_data.correctedVehicleDeltaVelocityNED.y; _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.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; _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data->dt;
} }
const AP_AHRS &_ahrs = AP::ahrs(); 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()); Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(_ahrs.get_primary_accel_index());
// Apply position correction for CG offset from IMU // Apply position correction for CG offset from IMU

12
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -5,7 +5,7 @@
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdint.h> #include <stdint.h>
#include "PosVelEKF.h" #include "PosVelEKF.h"
#include <AP_Buffer/AP_Buffer.h> #include <AP_HAL/utility/RingBuffer.h>
#include <AP_AHRS/AP_AHRS.h> #include <AP_AHRS/AP_AHRS.h>
// declare backend classes // declare backend classes
@ -59,6 +59,9 @@ public:
// returns time of last update // returns time of last update
uint32_t last_update_ms() const { return _last_update_ms; } 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 // returns time of last time target was seen
uint32_t last_backend_los_meas_ms() const { return _last_backend_los_meas_ms; } 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 _type; // precision landing sensor type
AP_Int8 _bus; // which sensor bus AP_Int8 _bus; // which sensor bus
AP_Int8 _estimator_type; // precision landing estimator type 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 _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_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 _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_Float _accel_noise; // accelometer process noise
AP_Vector3f _cam_offset; // Position of the camera relative to the CG 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 uint32_t _last_update_ms; // system time in millisecond when update was last called
bool _target_acquired; // true if target has been seen recently bool _target_acquired; // true if target has been seen recently
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
@ -140,15 +145,16 @@ private:
Vector2f _target_pos_rel_out_NE; // target's position relative to the camera, fed into position controller 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 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 { struct inertial_data_frame_s {
Matrix3f Tbn; // dcm rotation matrix to rotate body frame to north Matrix3f Tbn; // dcm rotation matrix to rotate body frame to north
Vector3f correctedVehicleDeltaVelocityNED; Vector3f correctedVehicleDeltaVelocityNED;
Vector3f inertialNavVelocity; Vector3f inertialNavVelocity;
bool inertialNavVelocityValid; bool inertialNavVelocityValid;
float dt; float dt;
uint64_t time_usec;
}; };
AP_Buffer<inertial_data_frame_s,8> _inertial_history; ObjectArray<inertial_data_frame_s> *_inertial_history;
// backend state // backend state
struct precland_state { struct precland_state {