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

EKF: replace reset event times with event counters 

Using a 64bit integer was unnecessary given it was only being used to detect a new reset event.
This commit is contained in:
Paul Riseborough 2016-05-25 18:24:31 +10:00
parent 70f76e1a6c
commit 6a55d908c5
4 changed files with 33 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
EKF/control.cpp
View File

@ -126,7 +126,7 @@ void Ekf::controlExternalVisionAiding()
}
// capture the reset event
_state_reset_status.quat_time_us = _imu_sample_delayed.time_us;
_state_reset_status.quat_counter++;
// flag the yaw as aligned
_control_status.flags.yaw_align = true;

32
EKF/ekf.h
View File

@ -127,23 +127,23 @@ public:
// get GPS check status
void get_gps_check_status(uint16_t *_gps_check_fail_status);
// return the amount the local vertical position changed in the last reset and the time of the reset
void get_posD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.posD_change; *time_us = _state_reset_status.posD_time_us;}
// return the amount the local vertical position changed in the last reset and the number of reset events
void get_posD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.posD_change; *counter = _state_reset_status.posD_counter;}
// return the amount the local vertical velocity changed in the last reset and the time of the reset
void get_velD_reset(float *delta, uint64_t *time_us) {*delta = _state_reset_status.velD_change; *time_us = _state_reset_status.velD_time_us;}
// return the amount the local vertical velocity changed in the last reset and the number of reset events
void get_velD_reset(float *delta, uint8_t *counter) {*delta = _state_reset_status.velD_change; *counter = _state_reset_status.velD_counter;}
// return the amount the local horizontal position changed in the last reset and the time of the reset
void get_posNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.posNE_change; *time_us = _state_reset_status.posNE_time_us;}
// return the amount the local horizontal position changed in the last reset and the number of reset events
void get_posNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.posNE_change; *counter = _state_reset_status.posNE_counter;}
// return the amount the local horizontal velocity changed in the last reset and the time of the reset
void get_velNE_reset(Vector2f *delta, uint64_t *time_us) {*delta = _state_reset_status.velNE_change; *time_us = _state_reset_status.velNE_time_us;}
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
void get_velNE_reset(Vector2f *delta, uint8_t *counter) {*delta = _state_reset_status.velNE_change; *counter = _state_reset_status.velNE_counter;}
// return the amount the quaternion has changed in the last reset and the time of the reset
void get_quat_reset(Quaternion *delta, uint64_t *time_us)
// return the amount the quaternion has changed in the last reset and the number of reset events
void get_quat_reset(Quaternion *delta, uint8_t *counter)
{
*delta = _state_reset_status.quat_change;
*time_us = _state_reset_status.quat_time_us;
*counter = _state_reset_status.quat_counter;
}
private:
@ -155,11 +155,11 @@ private:
// reset event monitoring
// structure containing velocity, position, height and yaw reset information
struct {
uint64_t velNE_time_us; // time stamp of the last horizontal velocity reset event (us)
uint64_t velD_time_us; // time stamp of the last vertical velocity reset event (us)
uint64_t posNE_time_us; // time stamp of the last horizontal position reset event (us)
uint64_t posD_time_us; // time stamp of the last vertical position reset event (us)
uint64_t quat_time_us; // time stamp of the last quaternion reset event (us)
uint8_t velNE_counter; // number of horizontal position reset events (allow to wrap if count exceeds 255)
uint8_t velD_counter; // number of vertical velocity reset events (allow to wrap if count exceeds 255)
uint8_t posNE_counter; // number of horizontal position reset events (allow to wrap if count exceeds 255)
uint8_t posD_counter; // number of vertical position reset events (allow to wrap if count exceeds 255)
uint8_t quat_counter; // number of quaternion reset events (allow to wrap if count exceeds 255)
Vector2f velNE_change; // North East velocity change due to last reset (m)
float velD_change; // Down velocity change due to last reset (m/s)
Vector2f posNE_change; // North, East position change due to last reset (m)

12
EKF/ekf_helper.cpp
View File

@ -88,8 +88,8 @@ bool Ekf::resetVelocity()
_state_reset_status.velNE_change(0) = velocity_change(0);
_state_reset_status.velNE_change(1) = velocity_change(1);
_state_reset_status.velD_change = velocity_change(2);
_state_reset_status.velNE_time_us = _imu_sample_delayed.time_us;
_state_reset_status.velD_time_us = _imu_sample_delayed.time_us;
_state_reset_status.velNE_counter++;
_state_reset_status.velD_counter++;
}
@ -162,7 +162,7 @@ bool Ekf::resetPosition()
// capture the reset event
_state_reset_status.posNE_change = posNE_change;
_state_reset_status.posNE_time_us = _imu_sample_delayed.time_us;
_state_reset_status.posNE_counter++;
}
@ -290,12 +290,12 @@ void Ekf::resetHeight()
// store the reset amount and time to be published
if (vert_pos_reset) {
_state_reset_status.posD_change = _state.pos(2) - old_vert_pos;
_state_reset_status.posD_time_us = _imu_sample_delayed.time_us;
_state_reset_status.posD_counter++;
}
if (vert_vel_reset) {
_state_reset_status.velD_change = _state.vel(2) - old_vert_vel;
_state_reset_status.velD_time_us = _imu_sample_delayed.time_us;
_state_reset_status.velD_counter++;
}
// add the reset amount to the output observer states
@ -497,7 +497,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
}
// capture the reset event
_state_reset_status.quat_time_us = _imu_sample_delayed.time_us;
_state_reset_status.quat_counter++;
return true;
}

20
EKF/estimator_interface.h
View File

@ -217,20 +217,20 @@ public:
// get GPS check status
virtual void get_gps_check_status(uint16_t *val) = 0;
// return the amount the local vertical position changed in the last reset and the time of the reset
virtual void get_posD_reset(float *delta, uint64_t *time_us) = 0;
// return the amount the local vertical position changed in the last reset and the number of reset events
virtual void get_posD_reset(float *delta, uint8_t *counter) = 0;
// return the amount the local vertical velocity changed in the last reset and the time of the reset
virtual void get_velD_reset(float *delta, uint64_t *time_us) = 0;
// return the amount the local vertical velocity changed in the last reset and the number of reset events
virtual void get_velD_reset(float *delta, uint8_t *counter) = 0;
// return the amount the local horizontal position changed in the last reset and the time of the reset
virtual void get_posNE_reset(Vector2f *delta, uint64_t *time_us) = 0;
// return the amount the local horizontal position changed in the last reset and the number of reset events
virtual void get_posNE_reset(Vector2f *delta, uint8_t *counter) = 0;
// return the amount the local horizontal velocity changed in the last reset and the time of the reset
virtual void get_velNE_reset(Vector2f *delta, uint64_t *time_us) = 0;
// return the amount the local horizontal velocity changed in the last reset and the number of reset events
virtual void get_velNE_reset(Vector2f *delta, uint8_t *counter) = 0;
// return the amount the quaternion has changed in the last reset and the time of the reset
virtual void get_quat_reset(Quaternion *delta, uint64_t *time_us) = 0;
// return the amount the quaternion has changed in the last reset and the number of reset events
virtual void get_quat_reset(Quaternion *delta, uint8_t *counter) = 0;
// get EKF innovation consistency check status
virtual void get_innovation_test_status(uint16_t *val)