From ed9a394029c642427b91e55b7a17a078f67b9535 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 14 Nov 2017 20:14:33 -0500 Subject: [PATCH 1/2] EKF RingBuffer avoid copying --- EKF/RingBuffer.h | 110 +++++++++++++----------------------- EKF/control.cpp | 37 ++++++------ EKF/ekf.cpp | 58 +++++++------------ EKF/ekf_helper.cpp | 88 +++++++++-------------------- EKF/estimator_interface.cpp | 20 +++---- EKF/terrain_estimator.cpp | 2 +- 6 files changed, 111 insertions(+), 204 deletions(-) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 10d18127dc..0bfedef179 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -45,54 +45,51 @@ template 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(sample), static_cast(&_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(_head)) { + if (index == _head) { _tail = _head; _first_write = true; @@ -150,7 +140,7 @@ public: return true; } - if (index == static_cast(_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}; }; diff --git a/EKF/control.cpp b/EKF/control.cpp index 201e01aa59..ec2d79c564 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -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,8 @@ 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); + 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 +528,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 +584,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 +639,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 +691,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 +737,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 diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f8701da0ab..95f41341c7 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -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 diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 6a928c9f11..a001b9ff68 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -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,19 @@ 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); + 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 +429,8 @@ 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); + 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 +650,8 @@ 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++) { + _output_buffer[i].quat_nominal *= _state_reset_status.quat_change; } // apply the change in attitude quaternion to our newest quaternion estimate diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ddee0c0e30..a7969b4e27 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -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; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 7223ba510d..4f18536e48 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -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 From 7c8fcf76289a29c4a90ec3951befb80dc8edcfb6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 16 Nov 2017 11:52:19 +1100 Subject: [PATCH 2/2] EKF: Clarify use of *= operator for quaternions --- EKF/control.cpp | 1 + EKF/ekf_helper.cpp | 3 +++ 2 files changed, 4 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index ec2d79c564..1ef41f3969 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -199,6 +199,7 @@ 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 + // 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; } diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index a001b9ff68..70ba3294f0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -318,6 +318,7 @@ void Ekf::alignOutputFilter() const Vector3f pos_delta = _state.pos - _output_sample_delayed.pos; // loop through the output filter state history and add the deltas + // 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(); @@ -429,6 +430,7 @@ 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 + // 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; } @@ -651,6 +653,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init) // add the reset amount to the output observer buffered data 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; }