dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #358 from PX4/pr-ringbuffer_copies 

EKF RingBuffer avoid unnecessary copying
This commit is contained in:
Paul Riseborough 2017-11-16 13:37:36 +11:00 committed by GitHub
parent 44eaa076db 7c8fcf7628
commit f54f1d467f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 115 additions and 204 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
EKF/RingBuffer.h
View File

@ -45,54 +45,51 @@ template <typename data_type>
class RingBuffer
{
public:
RingBuffer()
{
_buffer = NULL;
_head = _tail = _size = 0;
_first_write = true;
}
RingBuffer() = default;
~RingBuffer() { delete[] _buffer; }
bool allocate(int size)
{
if (size <= 0) {
return false;
}
// no copy, assignment, move, move assignment
RingBuffer(const RingBuffer &) = delete;
RingBuffer &operator=(const RingBuffer &) = delete;
RingBuffer(RingBuffer &&) = delete;
RingBuffer &operator=(RingBuffer &&) = delete;
if (_buffer != NULL) {
bool allocate(uint8_t size)
{
if (_buffer != nullptr) {
delete[] _buffer;
}
_buffer = new data_type[size];
if (_buffer == NULL) {
if (_buffer == nullptr) {
return false;
}
_size = size;
_head = 0;
_tail = 0;
// set the time elements to zero so that bad data is not retrieved from the buffers
for (unsigned index = 0; index < _size; index++) {
_buffer[index].time_us = 0;
}
_first_write = true;
return true;
}
void unallocate()
{
if (_buffer != NULL) {
delete[] _buffer;
}
void unallocate() {
delete[] _buffer;
_buffer = nullptr;
}
inline void push(data_type sample)
void push(const data_type& sample)
{
int head_new = _head;
uint8_t head_new = _head;
if (_first_write) {
head_new = _head;
} else {
if (!_first_write) {
head_new = (_head + 1) % _size;
}
@ -108,36 +105,29 @@ public:
}
}
inline data_type get_oldest()
{
return _buffer[_tail];
}
uint8_t get_length() const { return _size; }
unsigned get_oldest_index()
{
return _tail;
}
data_type &operator[](const uint8_t index) { return _buffer[index]; }
inline data_type get_newest()
{
return _buffer[_head];
}
const data_type& get_newest() { return _buffer[_head]; }
const data_type& get_oldest() { return _buffer[_tail]; }
inline bool pop_first_older_than(uint64_t timestamp, data_type *sample)
uint8_t get_oldest_index() const { return _tail; }
bool pop_first_older_than(const uint64_t& timestamp, data_type *sample)
{
// start looking from newest observation data
for (unsigned i = 0; i < _size; i++) {
for (uint8_t i = 0; i < _size; i++) {
int index = (_head - i);
index = index < 0 ? _size + index : index;
if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < (uint64_t)1e5) {
// TODO Re-evaluate the static cast and usage patterns
memcpy(static_cast<void *>(sample), static_cast<void *>(&_buffer[index]), sizeof(*sample));
*sample = _buffer[index];
// Now we can set the tail to the item which comes after the one we removed
// since we don't want to have any older data in the buffer
if (index == static_cast<int>(_head)) {
if (index == _head) {
_tail = _head;
_first_write = true;
@ -150,7 +140,7 @@ public:
return true;
}
if (index == static_cast<int>(_tail)) {
if (index == _tail) {
// we have reached the tail and haven't got a match
return false;
}
@ -159,38 +149,14 @@ public:
return false;
}
data_type &operator[](unsigned index)
{
return _buffer[index];
}
// return data at the specified index
inline data_type get_from_index(unsigned index)
{
if (index >= _size) {
index = _size-1;
}
return _buffer[index];
}
// push data to the specified index
inline void push_to_index(unsigned index, data_type sample)
{
if (index >= _size) {
index = _size-1;
}
_buffer[index] = sample;
}
// return the length of the buffer
unsigned get_length()
{
return _size;
}
private:
data_type *_buffer;
unsigned _head, _tail, _size;
bool _first_write;
data_type *_buffer{nullptr};
uint8_t _head{0};
uint8_t _tail{0};
uint8_t _size{0};
bool _first_write{true};
};

38
EKF/control.cpp
View File

@ -80,11 +80,13 @@ void Ekf::controlFusionModes()
}
// check faultiness (before pop_first_older_than) to see if we can change back to original height sensor
baroSample baro_init = _baro_buffer.get_newest();
const baroSample& baro_init = _baro_buffer.get_newest();
_baro_hgt_faulty = !((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
gpsSample gps_init = _gps_buffer.get_newest();
const gpsSample& gps_init = _gps_buffer.get_newest();
_gps_hgt_faulty = !((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
rangeSample rng_init = _range_buffer.get_newest();
const rangeSample& rng_init = _range_buffer.get_newest();
_rng_hgt_faulty = !((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check for arrival of new sensor data at the fusion time horizon
@ -182,7 +184,7 @@ void Ekf::controlExternalVisionFusion()
Eulerf euler_init(q_init);
// get initial yaw from the observation quaternion
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
const extVisionSample& ev_newest = _ext_vision_buffer.get_newest();
Quatf q_obs(ev_newest.quat);
Eulerf euler_obs(q_obs);
euler_init(2) = euler_obs(2);
@ -197,12 +199,9 @@ void Ekf::controlExternalVisionFusion()
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i=0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
// Note q1 *= q2 is equivalent to q1 = q2 * q1
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
}
// apply the change in attitude quaternion to our newest quaternion estimate
@ -530,8 +529,6 @@ void Ekf::controlGpsFusion()
// set innovation gate size
_posInnovGateNE = fmaxf(_params.posNE_innov_gate, 1.0f);
}
} else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) {
@ -588,10 +585,11 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case where we are using baro for height
if (_control_status.flags.baro_hgt) {
// check if GPS height is available
gpsSample gps_init = _gps_buffer.get_newest();
const gpsSample& gps_init = _gps_buffer.get_newest();
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
baroSample baro_init = _baro_buffer.get_newest();
const baroSample& baro_init = _baro_buffer.get_newest();
bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// check for inertial sensing errors in the last 10 seconds
@ -642,12 +640,12 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case we are using GPS for height
if (_control_status.flags.gps_hgt) {
// check if GPS height is available
gpsSample gps_init = _gps_buffer.get_newest();
const gpsSample& gps_init = _gps_buffer.get_newest();
bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL);
bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);
// check the baro height source for consistency and freshness
baroSample baro_init = _baro_buffer.get_newest();
const baroSample& baro_init = _baro_buffer.get_newest();
bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset);
bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate);
@ -694,11 +692,11 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case we are using range finder for height
if (_control_status.flags.rng_hgt) {
// check if range finder data is available
rangeSample rng_init = _range_buffer.get_newest();
const rangeSample& rng_init = _range_buffer.get_newest();
bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL);
// check if baro data is available
baroSample baro_init = _baro_buffer.get_newest();
const baroSample& baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// reset to baro if we have no range data and baro data is available
@ -740,11 +738,11 @@ void Ekf::controlHeightSensorTimeouts()
// handle the case where we are using external vision data for height
if (_control_status.flags.ev_hgt) {
// check if vision data is available
extVisionSample ev_init = _ext_vision_buffer.get_newest();
const extVisionSample& ev_init = _ext_vision_buffer.get_newest();
bool ev_data_available = ((_time_last_imu - ev_init.time_us) < 2 * EV_MAX_INTERVAL);
// check if baro data is available
baroSample baro_init = _baro_buffer.get_newest();
const baroSample& baro_init = _baro_buffer.get_newest();
bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL);
// reset to baro if we have no vision data and baro data is available

58
EKF/ekf.cpp
View File

@ -138,7 +138,7 @@ bool Ekf::initialiseFilter()
// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
// Sum the IMU delta angle measurements
imuSample imu_init = _imu_buffer.get_newest();
const imuSample& imu_init = _imu_buffer.get_newest();
_delVel_sum += imu_init.delta_vel;
// Sum the magnetometer measurements
@ -281,7 +281,7 @@ bool Ekf::initialiseFilter()
if (_control_status.flags.rng_hgt) {
// if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup
// so it can be used as a backup ad set the initial height using the range finder
baroSample baro_newest = _baro_buffer.get_newest();
const baroSample& baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt;
_state.pos(2) = -math::max(_rng_filt_state * _R_rng_to_earth_2_2, _params.rng_gnd_clearance);
ECL_INFO("EKF using range finder height - commencing alignment");
@ -379,11 +379,7 @@ bool Ekf::collect_imu(imuSample &imu)
// accumulate and downsample IMU data across a period FILTER_UPDATE_PERIOD_MS long
// copy imu data to local variables
_imu_sample_new.delta_ang = imu.delta_ang;
_imu_sample_new.delta_vel = imu.delta_vel;
_imu_sample_new.delta_ang_dt = imu.delta_ang_dt;
_imu_sample_new.delta_vel_dt = imu.delta_vel_dt;
_imu_sample_new.time_us = imu.time_us;
_imu_sample_new = imu;
// accumulate the time deltas
_imu_down_sampled.delta_ang_dt += imu.delta_ang_dt;
@ -393,7 +389,7 @@ bool Ekf::collect_imu(imuSample &imu)
// this quaternion represents the rotation from the start to end of the accumulation period
Quatf delta_q;
delta_q.rotate(imu.delta_ang);
_q_down_sampled = _q_down_sampled * delta_q;
_q_down_sampled = _q_down_sampled * delta_q;
_q_down_sampled.normalize();
// rotate the accumulated delta velocity data forward each time so it is always in the updated rotation frame
@ -406,7 +402,7 @@ bool Ekf::collect_imu(imuSample &imu)
// if the target time delta between filter prediction steps has been exceeded
// write the accumulated IMU data to the ring buffer
float target_dt = (float)(FILTER_UPDATE_PERIOD_MS) / 1000;
const float target_dt = FILTER_UPDATE_PERIOD_MS / 1000.0f;
if (_imu_down_sampled.delta_ang_dt >= target_dt - _imu_collection_time_adj) {
@ -446,7 +442,7 @@ bool Ekf::collect_imu(imuSample &imu)
void Ekf::calculateOutputStates()
{
// Use full rate IMU data at the current time horizon
imuSample imu_new = _imu_sample_new;
const imuSample& imu_new = _imu_sample_new;
// correct delta angles for bias offsets
Vector3f delta_angle;
@ -496,7 +492,7 @@ void Ekf::calculateOutputStates()
_vel_deriv_ned = delta_vel_NED * (1.0f / imu_new.delta_vel_dt);
}
// save the previous velocity so we can use trapezidal integration
// save the previous velocity so we can use trapezoidal integration
Vector3f vel_last = _output_new.vel;
// increment the INS velocity states by the measurement plus corrections
@ -539,7 +535,7 @@ void Ekf::calculateOutputStates()
// calculate the quaternion delta between the INS and EKF quaternions at the EKF fusion time horizon
Quatf quat_inv = _state.quat_nominal.inversed();
Quatf q_error = quat_inv * _output_sample_delayed.quat_nominal;
Quatf q_error = quat_inv * _output_sample_delayed.quat_nominal;
q_error.normalize();
// convert the quaternion delta to a delta angle
@ -579,7 +575,7 @@ void Ekf::calculateOutputStates()
/*
* Loop through the output filter state history and apply the corrections to the velocity and position states.
* This method is too expensive to use for the attitude states due to the quaternion operations required
* but becasue it eliminates the time delay in the 'correction loop' it allows higher tracking gains
* but because it eliminates the time delay in the 'correction loop' it allows higher tracking gains
* to be used and reduces tracking error relative to EKF states.
*/
@ -612,30 +608,24 @@ void Ekf::calculateOutputStates()
// loop through the vertical output filter state history starting at the oldest and apply the corrections to the
// vel_d states and propagate vel_d_integ forward using the corrected vel_d
outputVert current_state;
outputVert next_state;
unsigned index = _output_vert_buffer.get_oldest_index();
unsigned index_next;
unsigned size = _output_vert_buffer.get_length();
uint8_t index = _output_vert_buffer.get_oldest_index();
for (unsigned counter = 0; counter < (size - 1); counter++) {
index_next = (index + 1) % size;
current_state = _output_vert_buffer.get_from_index(index);
next_state = _output_vert_buffer.get_from_index(index_next);
const uint8_t size = _output_vert_buffer.get_length();
for (uint8_t counter = 0; counter < (size - 1); counter++) {
const uint8_t index_next = (index + 1) % size;
outputVert& current_state = _output_vert_buffer[index];
outputVert& next_state = _output_vert_buffer[index_next];
// correct the velocity
if (counter == 0) {
current_state.vel_d += vel_d_correction;
_output_vert_buffer.push_to_index(index, current_state);
}
next_state.vel_d += vel_d_correction;
// position is propagated forward using the corrected velocity and a trapezoidal integrator
next_state.vel_d_integ = current_state.vel_d_integ + (current_state.vel_d + next_state.vel_d) * 0.5f * next_state.dt;
// push the updated data to the buffer
_output_vert_buffer.push_to_index(index_next, next_state);
// advance the index
index = (index + 1) % size;
}
@ -663,20 +653,12 @@ void Ekf::calculateOutputStates()
Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f;
// loop through the output filter state history and apply the corrections to the velocity and position states
outputSample output_states;
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
// a constant velocity correction is applied
output_states.vel += vel_correction;
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
// a constant velocity correction is applied
_output_buffer[index].vel += vel_correction;
// a constant position correction is applied
output_states.pos += pos_correction;
// push the updated data to the buffer
_output_buffer.push_to_index(index, output_states);
_output_buffer[index].pos += pos_correction;
}
// update output state to corrected values

91
EKF/ekf_helper.cpp
View File

@ -68,15 +68,10 @@ bool Ekf::resetVelocity()
}
// calculate the change in velocity and apply to the output predictor state history
Vector3f velocity_change = _state.vel - vel_before_reset;
outputSample output_states = {};
unsigned max_index = _output_buffer.get_length() - 1;
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
output_states.vel += velocity_change;
_output_buffer.push_to_index(index, output_states);
const Vector3f velocity_change = _state.vel - vel_before_reset;
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].vel += velocity_change;
}
// apply the change in velocity to our newest velocity estimate
@ -131,17 +126,11 @@ bool Ekf::resetPosition()
}
// calculate the change in position and apply to the output predictor state history
Vector2f posNE_change;
posNE_change(0) = _state.pos(0) - posNE_before_reset(0);
posNE_change(1) = _state.pos(1) - posNE_before_reset(1);
outputSample output_states = {};
unsigned max_index = _output_buffer.get_length() - 1;
const Vector2f posNE_change{_state.pos(0) - posNE_before_reset(0), _state.pos(1) - posNE_before_reset(1)};
for (unsigned index = 0; index <= max_index; index++) {
output_states = _output_buffer.get_from_index(index);
output_states.pos(0) += posNE_change(0);
output_states.pos(1) += posNE_change(1);
_output_buffer.push_to_index(index, output_states);
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].pos(0) += posNE_change(0);
_output_buffer[index].pos(1) += posNE_change(1);
}
// apply the change in position to our newest position estimate
@ -160,7 +149,7 @@ bool Ekf::resetPosition()
void Ekf::resetHeight()
{
// Get the most recent GPS data
gpsSample gps_newest = _gps_buffer.get_newest();
const gpsSample& gps_newest = _gps_buffer.get_newest();
// store the current vertical position and velocity for reference so we can calculate and publish the reset amount
float old_vert_pos = _state.pos(2);
@ -193,7 +182,7 @@ void Ekf::resetHeight()
vert_pos_reset = true;
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
baroSample baro_newest = _baro_buffer.get_newest();
const baroSample& baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
} else {
@ -203,7 +192,7 @@ void Ekf::resetHeight()
} else if (_control_status.flags.baro_hgt) {
// initialize vertical position with newest baro measurement
baroSample baro_newest = _baro_buffer.get_newest();
const baroSample& baro_newest = _baro_buffer.get_newest();
if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) {
_state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset;
@ -236,7 +225,7 @@ void Ekf::resetHeight()
vert_pos_reset = true;
// reset the baro offset which is subtracted from the baro reading if we need to use it as a backup
baroSample baro_newest = _baro_buffer.get_newest();
const baroSample& baro_newest = _baro_buffer.get_newest();
_baro_hgt_offset = baro_newest.hgt + _state.pos(2);
} else {
@ -245,7 +234,7 @@ void Ekf::resetHeight()
} else if (_control_status.flags.ev_hgt) {
// initialize vertical position with newest measurement
extVisionSample ev_newest = _ext_vision_buffer.get_newest();
const extVisionSample& ev_newest = _ext_vision_buffer.get_newest();
// use the most recent data if it's time offset from the fusion time horizon is smaller
int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us;
@ -306,22 +295,14 @@ void Ekf::resetHeight()
}
// add the reset amount to the output observer buffered data
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
if (vert_pos_reset) {
output_states.pos(2) += _state_reset_status.posD_change;
_output_buffer[i].pos(2) += _state_reset_status.posD_change;
}
if (vert_vel_reset) {
output_states.vel(2) += _state_reset_status.velD_change;
_output_buffer[i].vel(2) += _state_reset_status.velD_change;
}
_output_buffer.push_to_index(i, output_states);
}
}
@ -329,25 +310,20 @@ void Ekf::resetHeight()
void Ekf::alignOutputFilter()
{
// calculate the quaternion delta between the output and EKF quaternions at the EKF fusion time horizon
Quatf quat_inv = _state.quat_nominal.inversed();
Quatf q_delta = quat_inv * _output_sample_delayed.quat_nominal;
Quatf q_delta = _state.quat_nominal.inversed() * _output_sample_delayed.quat_nominal;
q_delta.normalize();
// calculate the velocity and posiiton deltas between the output and EKF at the EKF fusion time horizon
Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
const Vector3f vel_delta = _state.vel - _output_sample_delayed.vel;
const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos;
// loop through the output filter state history and add the deltas
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal = q_delta * output_states.quat_nominal;
output_states.quat_nominal.normalize();
output_states.vel += vel_delta;
output_states.pos += pos_delta;
_output_buffer.push_to_index(i, output_states);
// Note q1 *= q2 is equivalent to q1 = q2 * q1
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal *= q_delta;
_output_buffer[i].quat_nominal.normalize();
_output_buffer[i].vel += vel_delta;
_output_buffer[i].pos += pos_delta;
}
}
@ -454,12 +430,9 @@ bool Ekf::realignYawGPS()
_state_reset_status.quat_change = quat_before_reset.inversed() * _state.quat_nominal;
// add the reset amount to the output observer buffered data
outputSample output_states;
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
// Note q1 *= q2 is equivalent to q1 = q2 * q1
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
}
// apply the change in attitude quaternion to our newest quaternion estimate
@ -679,13 +652,9 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
initialiseQuatCovariances(angle_err_var_vec);
// add the reset amount to the output observer buffered data
outputSample output_states = {};
unsigned output_length = _output_buffer.get_length();
for (unsigned i = 0; i < output_length; i++) {
output_states = _output_buffer.get_from_index(i);
output_states.quat_nominal = _state_reset_status.quat_change * output_states.quat_nominal;
_output_buffer.push_to_index(i, output_states);
for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
// Note q1 *= q2 is equivalent to q1 = q2 * q1
_output_buffer[i].quat_nominal *= _state_reset_status.quat_change;
}
// apply the change in attitude quaternion to our newest quaternion estimate

20
EKF/estimator_interface.cpp
View File

@ -55,9 +55,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
_initialised = true;
}
float dt = (float)(time_usec - _time_last_imu) / 1000 / 1000;
dt = math::max(dt, 1.0e-4f);
dt = math::min(dt, 0.02f);
const float dt = math::constrain((time_usec - _time_last_imu) / 1e6f, 1.0e-4f, 0.02f);
_time_last_imu = time_usec;
@ -66,7 +64,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
}
// copy data
imuSample imu_sample_new = {};
imuSample imu_sample_new;
imu_sample_new.delta_ang = Vector3f(delta_ang);
imu_sample_new.delta_vel = Vector3f(delta_vel);
@ -151,7 +149,7 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
// limit data rate to prevent data being lost
if (time_usec - _time_last_mag > _min_obs_interval_us) {
magSample mag_sample_new = {};
magSample mag_sample_new;
mag_sample_new.time_us = time_usec - _params.mag_delay_ms * 1000;
mag_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
@ -173,7 +171,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
bool need_gps = (_params.fusion_mode & MASK_USE_GPS) || (_params.vdist_sensor_type == VDIST_SENSOR_GPS);
if (((time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps->fix_type > 2) {
gpsSample gps_sample_new = {};
gpsSample gps_sample_new;
gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000;
gps_sample_new.time_us -= FILTER_UPDATE_PERIOD_MS * 1000 / 2;
@ -215,7 +213,7 @@ void EstimatorInterface::setBaroData(uint64_t time_usec, float data)
// limit data rate to prevent data being lost
if (time_usec - _time_last_baro > _min_obs_interval_us) {
baroSample baro_sample_new{};
baroSample baro_sample_new;
baro_sample_new.hgt = data;
baro_sample_new.time_us = time_usec - _params.baro_delay_ms * 1000;
@ -236,7 +234,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
// limit data rate to prevent data being lost
if (time_usec - _time_last_airspeed > _min_obs_interval_us) {
airspeedSample airspeed_sample_new{};
airspeedSample airspeed_sample_new;
airspeed_sample_new.true_airspeed = true_airspeed;
airspeed_sample_new.eas2tas = eas2tas;
airspeed_sample_new.time_us = time_usec - _params.airspeed_delay_ms * 1000;
@ -246,8 +244,7 @@ void EstimatorInterface::setAirspeedData(uint64_t time_usec, float true_airspeed
_airspeed_buffer.push(airspeed_sample_new);
}
}
static float rng;
// set range data
void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
{
if (!_initialised) {
@ -256,9 +253,8 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data)
// limit data rate to prevent data being lost
if (time_usec - _time_last_range > _min_obs_interval_us) {
rangeSample range_sample_new = {};
rangeSample range_sample_new;
range_sample_new.rng = data;
rng = data;
range_sample_new.time_us = time_usec - _params.range_delay_ms * 1000;
_time_last_range = time_usec;

2
EKF/terrain_estimator.cpp
View File

@ -45,7 +45,7 @@
bool Ekf::initHagl()
{
// get most recent range measurement from buffer
rangeSample latest_measurement = _range_buffer.get_newest();
const rangeSample& latest_measurement = _range_buffer.get_newest();
if ((_time_last_imu - latest_measurement.time_us) < (uint64_t)2e5 && _R_rng_to_earth_2_2 > _params.range_cos_max_tilt) {
// if we have a fresh measurement, use it to initialise the terrain estimator