Skip to content

Commit

Permalink
Improve quality of comments
Browse files Browse the repository at this point in the history
  • Loading branch information
kamilritz authored and bresch committed Dec 12, 2019
1 parent 1bf09fd commit 64652f5
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 22 deletions.
27 changes: 14 additions & 13 deletions EKF/common.h
Original file line number Diff line number Diff line change
Expand Up @@ -80,8 +80,8 @@ struct flow_message {
};

struct ext_vision_message {
Vector3f pos; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
Expand Down Expand Up @@ -153,8 +153,8 @@ struct flowSample {
};

struct extVisionSample {
Vector3f pos; ///< XYZ position in earth frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis
Vector3f pos; ///< XYZ position in external vision's local reference frame (m) - Z must be aligned with down axis
Vector3f vel; ///< XYZ velocity in external vision's local reference frame (m/sec) - Z must be aligned with down axis
Quatf quat; ///< quaternion defining rotation from body to earth frame
float posErr; ///< 1-Sigma horizontal position accuracy (m)
float hgtErr; ///< 1-Sigma height accuracy (m)
Expand Down Expand Up @@ -441,18 +441,18 @@ union filter_control_status_u {
struct {
uint32_t tilt_align : 1; ///< 0 - true if the filter tilt alignment is complete
uint32_t yaw_align : 1; ///< 1 - true if the filter yaw alignment is complete
uint32_t gps : 1; ///< 2 - true if GPS measurements are being fused
uint32_t opt_flow : 1; ///< 3 - true if optical flow measurements are being fused
uint32_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading is being fused
uint32_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement are being fused
uint32_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements are being fused
uint32_t gps : 1; ///< 2 - true if GPS measurement fusion is intended
uint32_t opt_flow : 1; ///< 3 - true if optical flow measurements fusion is intended
uint32_t mag_hdg : 1; ///< 4 - true if a simple magnetic yaw heading fusion is intended
uint32_t mag_3D : 1; ///< 5 - true if 3-axis magnetometer measurement fusion is inteded
uint32_t mag_dec : 1; ///< 6 - true if synthetic magnetic declination measurements fusion is intended
uint32_t in_air : 1; ///< 7 - true when the vehicle is airborne
uint32_t wind : 1; ///< 8 - true when wind velocity is being estimated
uint32_t baro_hgt : 1; ///< 9 - true when baro height is being fused as a primary height reference
uint32_t rng_hgt : 1; ///< 10 - true when range finder height is being fused as a primary height reference
uint32_t gps_hgt : 1; ///< 11 - true when GPS height is being fused as a primary height reference
uint32_t ev_pos : 1; ///< 12 - true when local position data from external vision is being fused
uint32_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements is being fused
uint32_t ev_pos : 1; ///< 12 - true when local position data fusion from external vision is intended
uint32_t ev_yaw : 1; ///< 13 - true when yaw data from external vision measurements fusion is intended
uint32_t ev_hgt : 1; ///< 14 - true when height data from external vision measurements is being fused
uint32_t fuse_beta : 1; ///< 15 - true when synthetic sideslip measurements are being fused
uint32_t mag_field_disturbed : 1; ///< 16 - true when the mag field does not match the expected strength
Expand All @@ -461,14 +461,15 @@ union filter_control_status_u {
uint32_t fuse_aspd : 1; ///< 19 - true when airspeed measurements are being fused
uint32_t gnd_effect : 1; ///< 20 - true when protection from ground effect induced static pressure rise is active
uint32_t rng_stuck : 1; ///< 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data from a GPS receiver is being fused
uint32_t gps_yaw : 1; ///< 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
uint32_t mag_aligned_in_flight : 1; ///< 23 - true when the in-flight mag field alignment has been completed
uint32_t ev_vel : 1; ///< 24 - true when local earth frame velocity data from external vision measurements are being fused
uint32_t ev_vel : 1; ///< 24 - true when local frame velocity data fusion from external vision measurements is intended
uint32_t synthetic_mag_z : 1; ///< 25 - true when we are using a synthesized measurement for the magnetometer Z component
} flags;
uint32_t value;
};

// Mavlink bitmask containing state of estimator solution
union ekf_solution_status {
struct {
uint16_t attitude : 1; ///< 0 - True if the attitude estimate is good
Expand Down
2 changes: 1 addition & 1 deletion EKF/ekf_helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1333,7 +1333,7 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
void Ekf::get_ekf_soln_status(uint16_t *status)
{
ekf_solution_status soln_status;

// TODO: Is this accurate enough?
soln_status.flags.attitude = _control_status.flags.tilt_align && _control_status.flags.yaw_align && (_fault_status.value == 0);
soln_status.flags.velocity_horiz = (_control_status.flags.gps || _control_status.flags.ev_pos|| _control_status.flags.ev_vel || _control_status.flags.opt_flow || (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd)) && (_fault_status.value == 0);
soln_status.flags.velocity_vert = (_control_status.flags.baro_hgt || _control_status.flags.ev_hgt || _control_status.flags.gps_hgt || _control_status.flags.rng_hgt) && (_fault_status.value == 0);
Expand Down
19 changes: 11 additions & 8 deletions EKF/estimator_interface.h
Original file line number Diff line number Diff line change
Expand Up @@ -540,16 +540,19 @@ class EstimatorInterface
bool _drag_buffer_fail{false};
bool _auxvel_buffer_fail{false};

uint64_t _time_last_imu{0}; // timestamp of last imu sample in microseconds
uint64_t _time_last_gps{0}; // timestamp of last gps measurement in microseconds
uint64_t _time_last_mag{0}; // timestamp of last magnetometer measurement in microseconds
uint64_t _time_last_baro{0}; // timestamp of last barometer measurement in microseconds
uint64_t _time_last_range{0}; // timestamp of last range measurement in microseconds
uint64_t _time_last_airspeed{0}; // timestamp of last airspeed measurement in microseconds
uint64_t _time_last_ext_vision{0}; // timestamp of last external vision measurement in microseconds
// timestamps of latest in buffer saved measurement in microseconds
uint64_t _time_last_imu{0};
uint64_t _time_last_gps{0};
uint64_t _time_last_mag{0};
uint64_t _time_last_baro{0};
uint64_t _time_last_range{0};
uint64_t _time_last_airspeed{0};
uint64_t _time_last_ext_vision{0};
uint64_t _time_last_optflow{0};
uint64_t _time_last_gnd_effect_on{0}; //last time the baro ground effect compensation was turned on externally (uSec)
uint64_t _time_last_auxvel{0};
//last time the baro ground effect compensation was turned on externally (uSec)
uint64_t _time_last_gnd_effect_on{0};


fault_status_u _fault_status{};

Expand Down

0 comments on commit 64652f5

Please sign in to comment.