diff --git a/EKF/common.h b/EKF/common.h index f8840b5267..f0b40bd611 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -71,6 +71,13 @@ struct flow_message { uint32_t dt; // integration time of flow samples }; +struct ext_vision_message { + Vector3f posNED; // measured NED position relative to the local origin (m) + Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame + float posErr; // 1-Sigma spherical position accuracy (m) + float angErr; // 1-Sigma angular error (rad) +}; + struct outputSample { Quaternion quat_nominal; // nominal quaternion describing vehicle attitude Vector3f vel; // NED velocity estimate in earth frame in m/s @@ -126,6 +133,14 @@ struct flowSample { uint64_t time_us; // timestamp in microseconds of the integration period mid-point }; +struct extVisionSample { + Vector3f posNED; // measured NED position relative to the local origin (m) + Quaternion quat; // measured quaternion orientation defining rotation from NED to body frame + float posErr; // 1-Sigma spherical position accuracy (m) + float angErr; // 1-Sigma angular error (rad) + uint64_t time_us; // timestamp of the measurement in microseconds +}; + // Integer definitions for vdist_sensor_type #define VDIST_SENSOR_BARO 0 // Use baro height #define VDIST_SENSOR_GPS 1 // Use GPS height @@ -140,6 +155,8 @@ struct flowSample { #define MASK_USE_GPS (1<<0) // set to true to use GPS data #define MASK_USE_OF (1<<1) // set to true to use optical flow data #define MASK_INHIBIT_ACC_BIAS (1<<2) // set to true to inhibit estimation of accelerometer delta velocity bias +#define MASK_USE_EVPOS (1<<3) // set to true to use external vision NED position data +#define MASK_USE_EVYAW (1<<4) // set to true to use exernal vision quaternion data for yaw // Integer definitions for mag_fusion_type #define MAG_FUSE_TYPE_AUTO 0 // The selection of either heading or 3D magnetometer fusion will be automatic @@ -163,6 +180,7 @@ struct parameters { float airspeed_delay_ms; // airspeed measurement delay relative to the IMU (msec) float flow_delay_ms; // optical flow measurement delay relative to the IMU (msec) - this is to the middle of the optical flow integration interval float range_delay_ms; // range finder measurement delay relative to the IMU (msec) + float ev_delay_ms; // off-board vision measurement delay relative to the IMU (msec) // input noise float gyro_noise; // IMU angular rate noise used for covariance prediction (rad/sec) @@ -247,6 +265,7 @@ struct parameters { airspeed_delay_ms = 200.0f; flow_delay_ms = 5.0f; range_delay_ms = 5.0f; + ev_delay_ms = 100.0f; // input noise gyro_noise = 1.5e-2f; @@ -383,6 +402,8 @@ union filter_control_status_u { uint16_t baro_hgt : 1; // 9 - true when baro height is being fused as a primary height reference uint16_t rng_hgt : 1; // 10 - true when range finder height is being fused as a primary height reference uint16_t gps_hgt : 1; // 11 - true when range finder height is being fused as a primary height reference + uint16_t ev_pos : 1; // 12 - true when local position data from external vision is being fused + uint16_t ev_yaw : 1; // 13 - true when yaw data from external vision measurements is being fused } flags; uint16_t value; }; diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 65bfa295c7..3fa6858e8f 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -65,6 +65,7 @@ EstimatorInterface::EstimatorInterface(): _time_last_baro(0), _time_last_range(0), _time_last_airspeed(0), + _time_last_ext_vision(0), _mag_declination_gps(0.0f), _mag_declination_to_save_deg(0.0f) { @@ -284,6 +285,30 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl } } +// set attitude and position data derived from an external vision system +void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message *evdata) +{ + if (!_initialised) { + return; + } + + // if data passes checks, push to buffer + if (time_usec > _time_last_ext_vision) { + extVisionSample ev_sample_new; + // calculate the system time-stamp for the mid point of the integration period + ev_sample_new.time_us = time_usec - _params.ev_delay_ms * 1000; + // copy required data + ev_sample_new.angErr = evdata->angErr; + ev_sample_new.posErr = evdata->posErr; + ev_sample_new.quat = evdata->quat; + ev_sample_new.posNED = evdata->posNED; + // record time for comparison next measurement + _time_last_ext_vision = time_usec; + // push to buffer + _ext_vision_buffer.push(ev_sample_new); + } +} + bool EstimatorInterface::initialise_interface(uint64_t timestamp) { @@ -294,6 +319,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _range_buffer.allocate(OBS_BUFFER_LENGTH) && _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && _flow_buffer.allocate(OBS_BUFFER_LENGTH) && + _ext_vision_buffer.allocate(OBS_BUFFER_LENGTH) && _output_buffer.allocate(IMU_BUFFER_LENGTH))) { printf("EKF buffer allocation failed!"); unallocate_buffers(); @@ -314,6 +340,8 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _airspeed_buffer.push(airspeed_sample_init); flowSample flow_sample_init = {}; _flow_buffer.push(flow_sample_init); + extVisionSample ext_vision_sample_init = {}; + _ext_vision_buffer.push(ext_vision_sample_init); } // zero the data in the imu data and output observer state buffers @@ -357,6 +385,7 @@ void EstimatorInterface::unallocate_buffers() _range_buffer.unallocate(); _airspeed_buffer.unallocate(); _flow_buffer.unallocate(); + _ext_vision_buffer.unallocate(); _output_buffer.unallocate(); } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index bd2ecf40a0..1789323c56 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -145,6 +145,9 @@ public: // set optical flow data void setOpticalFlowData(uint64_t time_usec, flow_message *flow); + // set external vision position and attitude data + void setExtVisionData(uint64_t time_usec, ext_vision_message *evdata); + // return a address to the parameters struct // in order to give access to the application parameters *getParamHandle() {return &_params;} @@ -277,6 +280,7 @@ protected: RingBuffer _range_buffer; RingBuffer _airspeed_buffer; RingBuffer _flow_buffer; + RingBuffer _ext_vision_buffer; RingBuffer _output_buffer; uint64_t _time_last_imu; // timestamp of last imu sample in microseconds @@ -285,6 +289,7 @@ protected: uint64_t _time_last_baro; // timestamp of last barometer measurement in microseconds uint64_t _time_last_range; // timestamp of last range measurement in microseconds uint64_t _time_last_airspeed; // timestamp of last airspeed measurement in microseconds + uint64_t _time_last_ext_vision; // timestamp of last external vision measurement in microseconds uint64_t _time_last_optflow; fault_status_u _fault_status;