46 #include <mathlib/mathlib.h> 122 if (min_sample_ratio < 5) {
123 min_sample_ratio = 5;
147 float (&delta_ang)[3],
float (&delta_vel)[3])
156 imu_sample_new.
time_us = time_usec;
228 gps_sample_new.
hgt = (float)gps.
alt * 1e-3
f;
230 gps_sample_new.
yaw = gps.
yaw;
242 gps_sample_new.
pos(0) = lpos_x;
243 gps_sample_new.
pos(1) = lpos_y;
246 gps_sample_new.
pos(0) = 0.0f;
247 gps_sample_new.
pos(1) = 0.0f;
308 airspeed_sample_new.
eas2tas = eas2tas;
339 range_sample_new.
quality = quality;
368 float delta_time = 1e-6
f * (float)flow->
dt;
370 bool delta_time_good = delta_time >= delta_time_min;
372 if (!delta_time_good) {
374 delta_time = delta_time_min;
380 float flow_rate_magnitude;
381 bool flow_magnitude_good =
true;
382 if (delta_time_good) {
383 flow_rate_magnitude = flow->
flowdata.norm() / delta_time;
394 bool use_flow_data_to_navigate = delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow);
410 optflow_sample_new.
dt = delta_time;
448 ev_sample_new.
pos = evdata->
pos;
449 ev_sample_new.
vel = evdata->
vel;
510 uint16_t ekf_delay_ms = max_time_delay_ms + (int)(ceilf((
float)max_time_delay_ms * 0.5f));
void setIMUData(const imuSample &imu_sample)
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
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.
uint64_t _time_last_ext_vision
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.
float hgt
gps height measurement (m)
Adapter / shim layer for system calls needed by ECL.
float epv
GPS vertical position accuracy in m.
#define ECL_ERR_TIMESTAMPED
void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata)
#define VDIST_SENSOR_GPS
Use GPS height.
RingBuffer< outputSample > _output_buffer
void setAuxVelData(uint64_t time_usec, float(&data)[2], float(&variance)[2])
RingBuffer< extVisionSample > _ext_vision_buffer
Vector2f pos
NE earth frame gps horizontal position measurement (m)
virtual bool collect_imu(const imuSample &imu)=0
Quatf quat
quaternion defining rotation from body to earth frame
uint64_t _time_last_optflow
float gps_delay_ms
GPS measurement delay relative to the IMU (mSec)
float _drag_sample_time_dt
float min_delay_ms
Maximum time delay of any sensor used to increase buffer length to handle large timing jitter (mSec) ...
float sacc
GPS speed accuracy in m/s.
Vector2f accelXY
measured specific force along the X and Y body axes (m/sec**2)
Vector3f gyrodata
Gyro rates about the XYZ body axes (rad/sec)
Vector3f cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2)
uint64_t time_us
timestamp of the measurement (uSec)
uint8_t fix_type
0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic ...
dragSample _drag_down_sampled
uint8_t _drag_sample_count
RingBuffer< outputVert > _output_vert_buffer
void setGpsData(uint64_t time_usec, const gps_message &gps)
float vacc
1-std vertical position error (m)
float flow_delay_ms
optical flow measurement delay relative to the IMU (mSec) - this is to the middle of the optical flow...
Vector3f gyroXYZ
measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (...
float vel_ned[3]
GPS ground speed NED.
float _flow_max_rate
maximum angular flow rate that the optical flow sensor can measure (rad/s)
int32_t fusion_mode
bitmasked integer that selects which aiding sources will be used
uint32_t dt
integration time of flow samples (sec)
uint8_t _imu_buffer_length
int32_t vdist_sensor_type
selects the primary source for height data
float hgtErr
1-Sigma height accuracy (m)
float dt
amount of integration time (sec)
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
uint8_t quality
Quality of Flow data.
float rng
range (distance to ground) measurement (m)
Vector2f velVarNE
estimated error variance of the NE velocity (m/sec)**2
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
bool allocate(uint8_t size)
uint64_t _time_last_airspeed
float sacc
1-std speed error (m/sec)
unsigned _min_obs_interval_us
RingBuffer< gpsSample > _gps_buffer
bool _airspeed_buffer_fail
int8_t quality
Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
virtual bool collect_gps(const gps_message &gps)=0
float velErr
1-Sigma velocity accuracy (m/sec)
uint64_t time_us
timestamp of the measurement (uSec)
uint32_t ev_vel
24 - true when local earth frame velocity data from external vision measurements are being fused ...
uint64_t time_us
timestamp of the measurement (uSec)
float hgtErr
1-Sigma height accuracy (m)
uint64_t time_us
timestamp of the measurement (uSec)
float posErr
1-Sigma horizontal position accuracy (m)
bool vel_ned_valid
GPS ground speed is valid.
Vector3f vel
NED earth frame gps velocity measurement (m/sec)
void setBaroData(uint64_t time_usec, float data)
float eas2tas
equivalent to true airspeed factor
float auxvel_delay_ms
auxiliary velocity measurement delay relative to the IMU (mSec)
uint64_t time_us
timestamp of the measurement (uSec)
float baro_delay_ms
barometer height measurement delay relative to the IMU (mSec)
uint64_t _time_last_move_detect_us
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Vector2< float > Vector2f
RingBuffer< dragSample > _drag_buffer
uint8_t quality
quality indicator between 0 and 255
Vector3f pos
XYZ position in earth frame (m) - Z must be aligned with down axis.
filter_control_status_u _control_status
virtual bool global_position_is_valid()=0
float mag_delay_ms
magnetometer measurement delay relative to the IMU (mSec)
RingBuffer< baroSample > _baro_buffer
void unallocate_buffers()
int32_t lat
Latitude in 1E-7 degrees.
Vector2f flowRadXY
measured delta angle of the image about the X and Y body axes (rad), RH rotation is positive ...
RingBuffer< magSample > _mag_buffer
fault_status_u _fault_status
uint64_t _time_last_range
uint64_t _time_last_auxvel
float hgt
pressure altitude above sea level (m)
void setAirspeedData(uint64_t time_usec, float true_airspeed, float eas2tas)
RingBuffer< auxVelSample > _auxvel_buffer
float airspeed_delay_ms
airspeed measurement delay relative to the IMU (mSec)
Vector2f velNE
measured NE velocity relative to the local origin (m/sec)
imuSample _imu_sample_delayed
Vector2f flowdata
Optical flow rates about the X and Y body axes (rad/sec)
#define MASK_USE_GPS
set to true to use GPS data
constexpr _Tp min(_Tp a, _Tp b)
bool _deadreckon_time_exceeded
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
#define MASK_USE_DRAG
set to true to use the multi-rotor drag model to estimate wind
Vector3< float > Vector3f
float range_delay_ms
range finder measurement delay relative to the IMU (mSec)
float yaw
yaw angle. NaN if not set (used for dual antenna GPS), (rad, [-PI, PI])
bool local_position_is_valid()
void setRangeData(uint64_t time_usec, float data, int8_t quality)
float true_airspeed
true airspeed measurement (m/sec)
constexpr _Tp max(_Tp a, _Tp b)
uint64_t time_us
timestamp of the measurement (uSec)
virtual bool init(uint64_t timestamp)=0
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
uint64_t time_us
timestamp of the measurement (uSec)
float angErr
1-Sigma angular error (rad)
uint32_t in_air
7 - true when the vehicle is airborne
Definition of base class for attitude estimators.
RingBuffer< rangeSample > _range_buffer
static constexpr unsigned FILTER_UPDATE_PERIOD_MS
RingBuffer< imuSample > _imu_buffer
int32_t alt
Altitude in 1E-3 meters (millimeters) above MSL.
int32_t sensor_interval_min_ms
minimum time of arrival difference between non IMU sensor updates. Sets the size of the observation b...
Quatf quat
quaternion defining rotation from body to earth frame
uint32_t ev_pos
12 - true when local position data from external vision is being fused
RingBuffer< airspeedSample > _airspeed_buffer
float hacc
1-std horizontal position error (m)
Vector3f vel
XYZ velocity in earth frame (m/sec) - Z must be aligned with down axis.
float angErr
1-Sigma angular error (rad)
float is_moving_scaler
gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the...
int32_t flow_qual_min
minimum acceptable quality integer from the flow sensor
uint32_t gps
2 - true if GPS measurements are being fused
RingBuffer< flowSample > _flow_buffer
float velErr
1-Sigma velocity accuracy (m/sec)
Vector3f mag
NED magnetometer body frame measurements (Gauss)
Vector3f delta_vel
delta velocity in body frame (integrated accelerometer measurements) (m/sec)
bool initialise_interface(uint64_t timestamp)
float delta_ang_dt
delta angle integration period (sec)
uint64_t time_us
timestamp of the integration period leading edge (uSec)
uint64_t time_us
timestamp of the measurement (uSec)
uint64_t time_us
timestamp of the measurement (uSec)
float ev_delay_ms
off-board vision measurement delay relative to the IMU (mSec)
float delta_vel_dt
delta velocity integration period (sec)
uint8_t get_length() const
int32_t lon
Longitude in 1E-7 degrees.
float posErr
1-Sigma horizontal position accuracy (m)
uint8_t _obs_buffer_length
void push(const data_type &sample)
void setOpticalFlowData(uint64_t time_usec, flow_message *flow)