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

92
libraries/AC_PrecLand/AC_PrecLand.cpp
View File

@ -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_data_frame_s>(_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

12
libraries/AC_PrecLand/AC_PrecLand.h
View File

@ -5,7 +5,7 @@
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <stdint.h>
#include "PosVelEKF.h"
#include <AP_Buffer/AP_Buffer.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_AHRS/AP_AHRS.h>
// 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_data_frame_s,8> _inertial_history;
ObjectArray<inertial_data_frame_s> *_inertial_history;
// backend state
struct precland_state {