34 #include <gtest/gtest.h> 107 float baro_data = 122.2f)
110 uint32_t start_time_us =
_t_us;
140 mag_data.copyTo(mag);
179 EXPECT_EQ(0, (
int) control_status.
flags.
gps);
185 EXPECT_EQ(0, (
int) control_status.
flags.
wind);
210 float converged_pos[3];
211 float converged_vel[3];
212 float converged_accel_bias[3];
213 float converged_gyro_bias[3];
220 for(
int i=0; i<3; ++i)
222 EXPECT_NEAR(0.0
f,converged_pos[i],0.001
f);
223 EXPECT_NEAR(0.0
f,converged_vel[i],0.001
f);
224 EXPECT_NEAR(0.0
f,converged_accel_bias[i],0.001
f);
225 EXPECT_NEAR(0.0
f,converged_gyro_bias[i],0.001
f);
240 EXPECT_EQ(1, (
int) control_status.flags.tilt_align);
241 EXPECT_EQ(1, (
int) control_status.flags.yaw_align);
242 EXPECT_EQ(1, (
int) control_status.flags.gps);
243 EXPECT_EQ(0, (
int) control_status.flags.opt_flow);
244 EXPECT_EQ(1, (
int) control_status.flags.mag_hdg);
245 EXPECT_EQ(0, (
int) control_status.flags.mag_3D);
246 EXPECT_EQ(0, (
int) control_status.flags.mag_dec);
247 EXPECT_EQ(0, (
int) control_status.flags.in_air);
248 EXPECT_EQ(0, (
int) control_status.flags.wind);
249 EXPECT_EQ(1, (
int) control_status.flags.baro_hgt);
250 EXPECT_EQ(0, (
int) control_status.flags.rng_hgt);
251 EXPECT_EQ(0, (
int) control_status.flags.gps_hgt);
252 EXPECT_EQ(0, (
int) control_status.flags.ev_pos);
253 EXPECT_EQ(0, (
int) control_status.flags.ev_yaw);
254 EXPECT_EQ(0, (
int) control_status.flags.ev_hgt);
255 EXPECT_EQ(0, (
int) control_status.flags.fuse_beta);
256 EXPECT_EQ(0, (
int) control_status.flags.mag_field_disturbed);
257 EXPECT_EQ(0, (
int) control_status.flags.fixed_wing);
258 EXPECT_EQ(0, (
int) control_status.flags.mag_fault);
259 EXPECT_EQ(0, (
int) control_status.flags.gnd_effect);
260 EXPECT_EQ(0, (
int) control_status.flags.rng_stuck);
261 EXPECT_EQ(0, (
int) control_status.flags.gps_yaw);
262 EXPECT_EQ(0, (
int) control_status.flags.mag_aligned_in_flight);
263 EXPECT_EQ(0, (
int) control_status.flags.ev_vel);
264 EXPECT_EQ(0, (
int) control_status.flags.synthetic_mag_z);
271 Vector3f accel_bias = {0.0f,0.0f,0.1f};
276 float converged_pos[3];
277 float converged_vel[3];
278 float converged_accel_bias[3];
279 float converged_gyro_bias[3];
286 for(
int i=0; i<3; ++i)
288 EXPECT_NEAR(0.0
f,converged_pos[i],0.001
f) <<
"i: " << i;
289 EXPECT_NEAR(0.0
f,converged_vel[i],0.001
f) <<
"i: " << i;
290 EXPECT_NEAR(accel_bias(i),converged_accel_bias[i],0.001
f) <<
"i: " << i;
291 EXPECT_NEAR(0.0
f,converged_gyro_bias[i],0.001
f) <<
"i: " << i;
const uint32_t _imu_dt_us
void setIMUData(const imuSample &imu_sample)
const uint32_t _baro_dt_us
void setMagData(uint64_t time_usec, float(&data)[3])
struct estimator::filter_control_status_u::@60 flags
float eph
GPS horizontal position accuracy in m.
Vector3f delta_ang
delta angle in body frame (integrated gyro measurements) (rad)
float yaw_offset
Heading/Yaw offset for dual antenna GPS - refer to description for GPS_YAW_OFFSET.
uint32_t gps_yaw
22 - true when yaw (not ground course) data from a GPS receiver is being fused
float epv
GPS vertical position accuracy in m.
uint32_t yaw_align
1 - true if the filter yaw alignment is complete
uint8_t nsats
number of satellites used
float vel_m_s
GPS ground speed (m/sec)
const uint32_t _gps_dt_us
uint32_t fixed_wing
17 - true when the vehicle is operating as a fixed wing vehicle
void get_gyro_bias(float bias[3]) override
uint32_t mag_hdg
4 - true if a simple magnetic yaw heading is being fused
uint32_t mag_fault
18 - true when the magnetometer has been declared faulty and is no longer being used ...
float sacc
GPS speed accuracy in m/s.
uint32_t rng_hgt
10 - true when range finder height is being fused as a primary height reference
uint8_t fix_type
0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic ...
uint32_t mag_field_disturbed
16 - true when the mag field does not match the expected strength
void update_with_const_sensors(uint32_t duration_us, Vector3f ang_vel=Vector3f{0.0f, 0.0f, 0.0f}, Vector3f accel=Vector3f{0.0f, 0.0f,-CONSTANTS_ONE_G}, Vector3f mag_data=Vector3f{0.2f, 0.0f, 0.4f}, float baro_data=122.2f)
uint32_t baro_hgt
9 - true when baro height is being fused as a primary height reference
const uint32_t _mag_dt_us
float pdop
position dilution of precision
void get_velocity(float *vel)
void setGpsData(uint64_t time_usec, const gps_message &gps)
float vel_ned[3]
GPS ground speed NED.
static constexpr float CONSTANTS_ONE_G
void get_control_mode(uint32_t *val)
uint32_t synthetic_mag_z
25 - true when we are using a synthesized measurement for the magnetometer Z component ...
TEST_F(EkfInitializationTest, tiltAlign)
void get_accel_bias(float bias[3]) override
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
uint32_t gnd_effect
20 - true when protection from ground effect induced static pressure rise is active ...
bool vel_ned_valid
GPS ground speed is valid.
void setBaroData(uint64_t time_usec, float data)
uint64_t time_us
timestamp of the measurement (uSec)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
int32_t lat
Latitude in 1E-7 degrees.
uint32_t ev_hgt
14 - true when height data from external vision measurements is being fused
uint32_t ev_yaw
13 - true when yaw data from external vision measurements is being fused
uint32_t mag_aligned_in_flight
23 - true when the in-flight mag field alignment has been completed
uint32_t mag_dec
6 - true if synthetic magnetic declination measurements are being fused
Vector3< float > Vector3f
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
uint32_t opt_flow
3 - true if optical flow measurements are being fused
uint32_t mag_3D
5 - true if 3-axis magnetometer measurement are being fused
uint32_t in_air
7 - true when the vehicle is airborne
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
uint32_t wind
8 - true when wind velocity is being estimated
uint32_t ev_pos
12 - true when local position data from external vision is being fused
uint32_t fuse_beta
15 - true when synthetic sideslip measurements are being fused
uint32_t gps_hgt
11 - true when GPS height is being fused as a primary height reference
uint32_t rng_stuck
21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough ...
uint32_t gps
2 - true if GPS measurements are being fused
uint32_t tilt_align
0 - true if the filter tilt alignment is complete
bool init(uint64_t timestamp) override
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
const uint32_t _init_duration_us
float delta_ang_dt
delta angle integration period (sec)
Class for core functions for ekf attitude and position estimator.
float delta_vel_dt
delta velocity integration period (sec)
int32_t lon
Longitude in 1E-7 degrees.
void get_position(float *pos)