From 532c9abd4a9ae8849bf751fa49595d7519ade557 Mon Sep 17 00:00:00 2001 From: kritz Date: Tue, 17 Dec 2019 11:35:45 +0100 Subject: [PATCH] Expand test framework and test cases (#685) * Fix comment * Ekf wrapper for testing Add utility function for accessing information in the ekf object * Add step function for Gps sensor * Add RangeFinder and Flow to simulated sensors * Add first fusion logic tests * Add units to function name * Use EXPECT_TRUE * Adding missing qualifiers * Improve EXPECT_ calls * Improve naming --- EKF/common.h | 2 +- EKF/control.cpp | 1 + EKF/estimator_interface.cpp | 5 +- test/CMakeLists.txt | 1 + test/sensor_simulator/CMakeLists.txt | 3 + test/sensor_simulator/baro.h | 2 +- test/sensor_simulator/ekf_wrapper.cpp | 71 ++++++++ test/sensor_simulator/ekf_wrapper.h | 74 +++++++++ test/sensor_simulator/flow.cpp | 38 +++++ test/sensor_simulator/flow.h | 64 +++++++ test/sensor_simulator/gps.cpp | 45 ++++- test/sensor_simulator/gps.h | 6 +- test/sensor_simulator/imu.cpp | 3 +- test/sensor_simulator/imu.h | 2 +- test/sensor_simulator/mag.cpp | 2 +- test/sensor_simulator/mag.h | 2 +- test/sensor_simulator/range_finder.cpp | 28 ++++ test/sensor_simulator/range_finder.h | 65 ++++++++ test/sensor_simulator/sensor.h | 2 +- test/sensor_simulator/sensor_simulator.cpp | 86 +++++----- test/sensor_simulator/sensor_simulator.h | 36 ++-- test/test_EKF_basics.cpp | 71 ++++---- test/test_EKF_fusionLogic.cpp | 185 +++++++++++++++++++++ 23 files changed, 679 insertions(+), 115 deletions(-) create mode 100644 test/sensor_simulator/ekf_wrapper.cpp create mode 100644 test/sensor_simulator/ekf_wrapper.h create mode 100644 test/sensor_simulator/flow.cpp create mode 100644 test/sensor_simulator/flow.h create mode 100644 test/sensor_simulator/range_finder.cpp create mode 100644 test/sensor_simulator/range_finder.h create mode 100644 test/test_EKF_fusionLogic.cpp diff --git a/EKF/common.h b/EKF/common.h index 3ce773ad8d..e8a27685bc 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -76,7 +76,7 @@ struct flow_message { uint8_t quality; ///< Quality of Flow data Vector2f flowdata; ///< Optical flow rates about the X and Y body axes (rad/sec) Vector3f gyrodata; ///< Gyro rates about the XYZ body axes (rad/sec) - uint32_t dt; ///< integration time of flow samples (sec) + uint32_t dt; ///< integration time of flow samples (microseconds) }; struct ext_vision_message { diff --git a/EKF/control.cpp b/EKF/control.cpp index 0943af3304..5ae4da6ba6 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -401,6 +401,7 @@ void Ekf::controlExternalVisionFusion() void Ekf::controlOpticalFlowFusion() { + // TODO: These motion checks run all the time. Pull them out of this function // Check if on ground motion is un-suitable for use of optical flow if (!_control_status.flags.in_air) { // When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index ceb45b597d..b472727021 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -344,7 +344,7 @@ void EstimatorInterface::setRangeData(uint64_t time_usec, float data, int8_t qua } } -// set optical flow data +// TODO: Change pointer to constant reference void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *flow) { if (!_initialised || _flow_buffer_fail) { @@ -366,7 +366,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl if ((time_usec - _time_last_optflow) > _min_obs_interval_us) { // check if enough integration time and fail if integration time is less than 50% // of min arrival interval because too much data is being lost - float delta_time = 1e-6f * (float)flow->dt; + float delta_time = 1e-6f * (float)flow->dt; // in seconds float delta_time_min = 5e-7f * (float)_min_obs_interval_us; bool delta_time_good = delta_time >= delta_time_min; @@ -407,7 +407,6 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl optflow_sample_new.gyroXYZ = - flow->gyrodata; optflow_sample_new.flowRadXY = -flow->flowdata; - // convert integration interval to seconds optflow_sample_new.dt = delta_time; _time_last_optflow = time_usec; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index abef338342..aa3ca08a58 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -41,6 +41,7 @@ set(SRCS test_EKF_ringbuffer.cpp test_EKF_imuSampling.cpp test_AlphaFilter.cpp + test_EKF_fusionLogic.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/sensor_simulator/CMakeLists.txt b/test/sensor_simulator/CMakeLists.txt index 7818feebb3..9c7dc79f44 100644 --- a/test/sensor_simulator/CMakeLists.txt +++ b/test/sensor_simulator/CMakeLists.txt @@ -34,11 +34,14 @@ set(SRCS sensor_simulator.cpp + ekf_wrapper.cpp sensor.cpp imu.cpp mag.cpp baro.cpp gps.cpp + flow.cpp + range_finder.cpp ) add_library(ecl_sensor_sim ${SRCS}) diff --git a/test/sensor_simulator/baro.h b/test/sensor_simulator/baro.h index 225c18c39b..6e55181a99 100644 --- a/test/sensor_simulator/baro.h +++ b/test/sensor_simulator/baro.h @@ -60,4 +60,4 @@ private: }; } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/ekf_wrapper.cpp b/test/sensor_simulator/ekf_wrapper.cpp new file mode 100644 index 0000000000..9658e78f00 --- /dev/null +++ b/test/sensor_simulator/ekf_wrapper.cpp @@ -0,0 +1,71 @@ +#include "ekf_wrapper.h" + +EkfWrapper::EkfWrapper(std::shared_ptr ekf): +_ekf{ekf} +{ + _ekf_params = _ekf->getParamHandle(); +} + +EkfWrapper::~EkfWrapper() +{ +} + +void EkfWrapper::enableGpsFusion() +{ + _ekf_params->fusion_mode |= MASK_USE_GPS; +} + +void EkfWrapper::disableGpsFusion() +{ + _ekf_params->fusion_mode &= ~MASK_USE_GPS; +} + +bool EkfWrapper::isIntendingGpsFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.gps; +} + +void EkfWrapper::enableFlowFusion() +{ + _ekf_params->fusion_mode |= MASK_USE_OF; +} + +void EkfWrapper::disableFlowFusion() +{ + _ekf_params->fusion_mode &= ~MASK_USE_OF; +} + +bool EkfWrapper::isIntendingFlowFusion() const +{ + filter_control_status_u control_status; + _ekf->get_control_mode(&control_status.value); + return control_status.flags.opt_flow; +} + +Vector3f EkfWrapper::getPosition() const +{ + float temp[3]; + _ekf->get_position(temp); + return Vector3f(temp); +} +Vector3f EkfWrapper::getVelocity() const +{ + float temp[3]; + _ekf->get_velocity(temp); + return Vector3f(temp); +} +Vector3f EkfWrapper::getAccelBias() const +{ + float temp[3]; + _ekf->get_accel_bias(temp); + return Vector3f(temp); +} + +Vector3f EkfWrapper::getGyroBias() const +{ + float temp[3]; + _ekf->get_gyro_bias(temp); + return Vector3f(temp); +} diff --git a/test/sensor_simulator/ekf_wrapper.h b/test/sensor_simulator/ekf_wrapper.h new file mode 100644 index 0000000000..b8bf2cae5a --- /dev/null +++ b/test/sensor_simulator/ekf_wrapper.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Wrapper class for Ekf object to set behavior or check status + * @author Kamil Ritz + */ +#pragma once + +#include +#include "EKF/ekf.h" +#include "EKF/estimator_interface.h" + +class EkfWrapper +{ +public: + EkfWrapper(std::shared_ptr ekf); + ~EkfWrapper(); + + void enableGpsFusion(); + + void disableGpsFusion(); + + bool isIntendingGpsFusion() const; + + void enableFlowFusion(); + + void disableFlowFusion(); + + bool isIntendingFlowFusion() const; + + Vector3f getPosition() const; + Vector3f getVelocity() const; + Vector3f getAccelBias() const; + Vector3f getGyroBias() const; + + +private: + std::shared_ptr _ekf; + + // Pointer to Ekf internal param struct + parameters* _ekf_params; + +}; diff --git a/test/sensor_simulator/flow.cpp b/test/sensor_simulator/flow.cpp new file mode 100644 index 0000000000..a6689c1d66 --- /dev/null +++ b/test/sensor_simulator/flow.cpp @@ -0,0 +1,38 @@ +#include "flow.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +Flow::Flow(std::shared_ptr ekf):Sensor(ekf) +{ +} + +Flow::~Flow() +{ +} + +void Flow::send(uint32_t time) +{ + _ekf->setOpticalFlowData(time, &_flow_data); +} + +void Flow::setData(const flow_message& flow) +{ + _flow_data = flow; + +} + +flow_message Flow::dataAtRest() +{ + flow_message _flow_at_rest; + _flow_at_rest.dt = 20000; + _flow_at_rest.flowdata = Vector2f{0.0f, 0.0f}; + _flow_at_rest.gyrodata = Vector3f{0.0f, 0.0f, 0.0f}; + _flow_at_rest.quality = 255; + return _flow_at_rest; +} + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/flow.h b/test/sensor_simulator/flow.h new file mode 100644 index 0000000000..b212efd2ba --- /dev/null +++ b/test/sensor_simulator/flow.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Feeds Ekf with optical flow data + * @author Kamil Ritz + */ +#pragma once + +#include "sensor.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +class Flow: public Sensor +{ +public: + Flow(std::shared_ptr ekf); + ~Flow(); + + void setData(const flow_message& flow); + flow_message dataAtRest(); + +private: + flow_message _flow_data; + + void send(uint32_t time) override; + +}; + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index 4b2776956b..0c36e2a3c3 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -24,5 +24,48 @@ void Gps::setData(const gps_message& gps) _gps_data = gps; } +void Gps::stepHeightByMeters(float hgt_change) +{ + _gps_data.alt += hgt_change * 1e3f; +} + +void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change) +{ + float hposN_curr; + float hposE_curr; + map_projection_global_project((float)_gps_data.lat, (float)_gps_data.lon, &hposN_curr, &hposE_curr); + Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change; + double lat_new; + double lon_new; + map_projection_global_reproject(hpos_new(0), hpos_new(1), &lat_new, &lon_new); + _gps_data.lon = (uint32_t)lon_new; + _gps_data.lat = (uint32_t)lat_new; +} + + +gps_message Gps::getDefaultGpsData() +{ + gps_message gps_data{}; + gps_data.time_usec = 0; + gps_data.lat = 473566094; + gps_data.lon = 85190237; + gps_data.alt = 422056; + gps_data.yaw = 0.0f; + gps_data.yaw_offset = 0.0f; + gps_data.fix_type = 3; + gps_data.eph = 0.5f; + gps_data.epv = 0.8f; + gps_data.sacc = 0.2f; + gps_data.vel_m_s = 0.0; + gps_data.vel_ned[0] = 0.0f; + gps_data.vel_ned[1] = 0.0f; + gps_data.vel_ned[2] = 0.0f; + gps_data.vel_ned_valid = 1; + gps_data.nsats = 16; + gps_data.pdop = 0.0f; + + return gps_data; +} + } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index ed7a426b2b..6686c53c1c 100644 --- a/test/sensor_simulator/gps.h +++ b/test/sensor_simulator/gps.h @@ -51,6 +51,10 @@ public: ~Gps(); void setData(const gps_message& gps); + void stepHeightByMeters(float hgt_change); + void stepHorizontalPositionByMeters(Vector2f hpos_change); + + gps_message getDefaultGpsData(); private: gps_message _gps_data; @@ -60,4 +64,4 @@ private: }; } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/imu.cpp b/test/sensor_simulator/imu.cpp index 0360a64fdb..f80cc7ee5d 100644 --- a/test/sensor_simulator/imu.cpp +++ b/test/sensor_simulator/imu.cpp @@ -15,7 +15,6 @@ Imu::~Imu() void Imu::send(uint32_t time) { - // fill imu sample with stored data imuSample imu_sample; imu_sample.time_us = time; imu_sample.delta_ang_dt = (time - _time_last_data_sent) * 1.e-6f; @@ -43,4 +42,4 @@ void Imu::setGyroData(const Vector3f& gyro) } } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/imu.h b/test/sensor_simulator/imu.h index afad7662c2..f088c000c9 100644 --- a/test/sensor_simulator/imu.h +++ b/test/sensor_simulator/imu.h @@ -63,4 +63,4 @@ private: }; } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/mag.cpp b/test/sensor_simulator/mag.cpp index 07af125b09..3e34e654bb 100644 --- a/test/sensor_simulator/mag.cpp +++ b/test/sensor_simulator/mag.cpp @@ -26,4 +26,4 @@ void Mag::setData(const Vector3f& mag) } } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/mag.h b/test/sensor_simulator/mag.h index 08f1d4b115..6d4fb1dc92 100644 --- a/test/sensor_simulator/mag.h +++ b/test/sensor_simulator/mag.h @@ -60,4 +60,4 @@ private: }; } // namespace sensor -} // namespace sensor_simulator::sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/range_finder.cpp b/test/sensor_simulator/range_finder.cpp new file mode 100644 index 0000000000..4796b8bcf3 --- /dev/null +++ b/test/sensor_simulator/range_finder.cpp @@ -0,0 +1,28 @@ +#include "range_finder.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +RangeFinder::RangeFinder(std::shared_ptr ekf):Sensor(ekf) +{ +} + +RangeFinder::~RangeFinder() +{ +} + +void RangeFinder::send(uint32_t time) +{ + _ekf->setRangeData(time, _range_data, _range_quality); +} + +void RangeFinder::setData(float range_data_meters, int8_t range_quality) +{ + _range_data = range_data_meters; + _range_quality = range_quality; +} + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/range_finder.h b/test/sensor_simulator/range_finder.h new file mode 100644 index 0000000000..6afc87236c --- /dev/null +++ b/test/sensor_simulator/range_finder.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Feeds Ekf with range finder data + * @author Kamil Ritz + */ +#pragma once + +#include "sensor.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +class RangeFinder: public Sensor +{ +public: + RangeFinder(std::shared_ptr ekf); + ~RangeFinder(); + + void setData(float range_data, int8_t range_quality); + flow_message dataAtRest(); + +private: + float _range_data; + int8_t _range_quality; + + void send(uint32_t time) override; + +}; + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/sensor.h b/test/sensor_simulator/sensor.h index 6690851913..55440f889a 100644 --- a/test/sensor_simulator/sensor.h +++ b/test/sensor_simulator/sensor.h @@ -54,7 +54,7 @@ public: void update(uint32_t time); - void setRate(uint32_t rate){ _update_period = uint32_t(1000000)/rate; } + void setRateHz(uint32_t rate){ _update_period = uint32_t(1000000)/rate; } bool isRunning() const { return _is_running; } diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 12e6de7673..4134156a36 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -3,29 +3,16 @@ SensorSimulator::SensorSimulator(std::shared_ptr ekf): _ekf{ekf}, -_imu{ekf}, -_mag{ekf}, -_baro{ekf}, -_gps{ekf} +_imu(ekf), +_mag(ekf), +_baro(ekf), +_gps(ekf), +_flow(ekf), +_rng(ekf) { - - // set default sensor rate in Hz - _imu.setRate(250); - _mag.setRate(80); - _baro.setRate(80); - _gps.setRate(5); - - // set default sensor data - _imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}, - Vector3f{0.0f,0.0f,0.0f}); - _mag.setData(Vector3f{0.2f, 0.0f, 0.4f}); - _baro.setData(122.2f); - _gps.setData(getDefaultGpsData()); - - // start default sensor - _imu.start(); - _mag.start(); - _baro.start(); + setSensorDataToDefault(); + setSensorRateToDefault(); + startBasicSensor(); } SensorSimulator::~SensorSimulator() @@ -33,40 +20,41 @@ SensorSimulator::~SensorSimulator() } -gps_message SensorSimulator::getDefaultGpsData() +void SensorSimulator::setSensorDataToDefault() { - // setup gps message to reasonable default values - gps_message gps_data{}; - gps_data.time_usec = 0; - gps_data.lat = 473566094; - gps_data.lon = 85190237; - gps_data.alt = 422056; - gps_data.yaw = 0.0f; - gps_data.yaw_offset = 0.0f; - gps_data.fix_type = 3; - gps_data.eph = 0.5f; - gps_data.epv = 0.8f; - gps_data.sacc = 0.2f; - gps_data.vel_m_s = 0.0; - gps_data.vel_ned[0] = 0.0f; - gps_data.vel_ned[1] = 0.0f; - gps_data.vel_ned[2] = 0.0f; - gps_data.vel_ned_valid = 1; - gps_data.nsats = 16; - gps_data.pdop = 0.0f; - - return gps_data; + _imu.setRateHz(250); + _mag.setRateHz(80); + _baro.setRateHz(80); + _gps.setRateHz(5); + _flow.setRateHz(50); + _rng.setRateHz(30); +} +void SensorSimulator::setSensorRateToDefault() +{ + _imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}, + Vector3f{0.0f,0.0f,0.0f}); + _mag.setData(Vector3f{0.2f, 0.0f, 0.4f}); + _baro.setData(122.2f); + _gps.setData(_gps.getDefaultGpsData()); + _flow.setData(_flow.dataAtRest()); + _rng.setData(0.2f, 100); +} +void SensorSimulator::startBasicSensor() +{ + _imu.start(); + _mag.start(); + _baro.start(); } -void SensorSimulator::run_seconds(float duration_seconds) +void SensorSimulator::runSeconds(float duration_seconds) { - run_microseconds( uint32_t(duration_seconds * 1e6f) ); + runMicroseconds( uint32_t(duration_seconds * 1e6f) ); } -void SensorSimulator::run_microseconds(uint32_t duration) +void SensorSimulator::runMicroseconds(uint32_t duration) { // simulate in 1000us steps - uint32_t start_time = _time; + const uint32_t start_time = _time; for(;_time < start_time + duration; _time+=1000) { @@ -74,6 +62,8 @@ void SensorSimulator::run_microseconds(uint32_t duration) _mag.update(_time); _baro.update(_time); _gps.update(_time); + _flow.update(_time); + _rng.update(_time); _ekf->update(); } diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h index 92d00a9818..8078c8791c 100644 --- a/test/sensor_simulator/sensor_simulator.h +++ b/test/sensor_simulator/sensor_simulator.h @@ -48,39 +48,47 @@ #include "mag.h" #include "baro.h" #include "gps.h" +#include "flow.h" +#include "range_finder.h" #include "EKF/ekf.h" using namespace sensor_simulator::sensor; class SensorSimulator { + +private: + std::shared_ptr _ekf; + + uint32_t _time {0}; // in microseconds + + void setSensorDataToDefault(); + void setSensorRateToDefault(); + void startBasicSensor(); + public: SensorSimulator(std::shared_ptr ekf); ~SensorSimulator(); - void setImuRate(uint32_t rate){ _imu.setRate(rate); } - void setMagRate(uint32_t rate){ _mag.setRate(rate); } - void setBaroRate(uint32_t rate){ _baro.setRate(rate); } - void setGpsRate(uint32_t rate){ _gps.setRate(rate); } - - void run_seconds(float duration_seconds); - void run_microseconds(uint32_t duration); + void runSeconds(float duration_seconds); + void runMicroseconds(uint32_t duration); void startGps(){ _gps.start(); } void stopGps(){ _gps.stop(); } - void setImuBias(Vector3f accel_bias, Vector3f gyro_bias); + void startFlow(){ _flow.start(); } + void stopFlow(){ _flow.stop(); } -private: - std::shared_ptr _ekf; + void startRangeFinder(){ _rng.start(); } + void stopRangeFinder(){ _rng.stop(); } + + void setImuBias(Vector3f accel_bias, Vector3f gyro_bias); Imu _imu; Mag _mag; Baro _baro; Gps _gps; - - uint32_t _time {0}; // in microseconds - - gps_message getDefaultGpsData(); + Flow _flow; + RangeFinder _rng; }; diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index 615f68f81d..3743456b7a 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -36,16 +36,19 @@ #include #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" - +#include "sensor_simulator/ekf_wrapper.h" class EkfInitializationTest : public ::testing::Test { public: EkfInitializationTest(): ::testing::Test(), _ekf{std::make_shared()}, - _sensor_simulator(_ekf) {}; + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; std::shared_ptr _ekf; SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + // Duration of initalization with only providing baro,mag and IMU const uint32_t _init_duration_s{2}; @@ -54,7 +57,7 @@ class EkfInitializationTest : public ::testing::Test { void SetUp() override { _ekf->init(0); - _sensor_simulator.run_seconds(_init_duration_s); + _sensor_simulator.runSeconds(_init_duration_s); } // Use this method to clean up any memory, network etc. after each test @@ -68,7 +71,7 @@ TEST_F(EkfInitializationTest, tiltAlign) { // GIVEN: reasonable static sensor data for some duration // THEN: EKF should tilt align - EXPECT_EQ(true,_ekf->attitude_valid()); + EXPECT_TRUE(_ekf->attitude_valid()); } TEST_F(EkfInitializationTest, initialControlMode) @@ -108,25 +111,19 @@ TEST_F(EkfInitializationTest, initialControlMode) TEST_F(EkfInitializationTest, convergesToZero) { // GIVEN: initialized EKF with default IMU, baro and mag input - _sensor_simulator.run_seconds(4); + _sensor_simulator.runSeconds(4); - float converged_pos[3]; - float converged_vel[3]; - float converged_accel_bias[3]; - float converged_gyro_bias[3]; - _ekf->get_position(converged_pos); - _ekf->get_velocity(converged_vel); - _ekf->get_accel_bias(converged_accel_bias); - _ekf->get_gyro_bias(converged_gyro_bias); + Vector3f pos = _ekf_wrapper.getPosition(); + Vector3f vel = _ekf_wrapper.getVelocity(); + Vector3f accel_bias = _ekf_wrapper.getAccelBias(); + Vector3f gyro_bias = _ekf_wrapper.getGyroBias(); + Vector3f ref{0.0f, 0.0f, 0.0f}; // THEN: EKF should stay or converge to zero - for(int i=0; i<3; ++i) - { - EXPECT_NEAR(0.0f,converged_pos[i],0.001f); - EXPECT_NEAR(0.0f,converged_vel[i],0.001f); - EXPECT_NEAR(0.0f,converged_accel_bias[i],0.001f); - EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f); - } + EXPECT_TRUE(matrix::isEqual(pos, ref, 0.001f)); + EXPECT_TRUE(matrix::isEqual(vel, ref, 0.001f)); + EXPECT_TRUE(matrix::isEqual(accel_bias, ref, 0.001f)); + EXPECT_TRUE(matrix::isEqual(gyro_bias, ref, 0.001f)); } TEST_F(EkfInitializationTest, gpsFusion) @@ -135,7 +132,7 @@ TEST_F(EkfInitializationTest, gpsFusion) // WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec _sensor_simulator.startGps(); - _sensor_simulator.run_seconds(11); + _sensor_simulator.runSeconds(11); // THEN: EKF should fuse GPS, but no other position sensor filter_control_status_u control_status; @@ -171,29 +168,23 @@ TEST_F(EkfInitializationTest, accleBiasEstimation) { // GIVEN: initialized EKF with default IMU, baro and mag input for 2s // WHEN: Added more sensor measurements with accel bias and gps measurements - Vector3f accel_bias = {0.0f,0.0f,0.1f}; + Vector3f accel_bias_sim = {0.0f,0.0f,0.1f}; _sensor_simulator.startGps(); - _sensor_simulator.setImuBias(accel_bias, Vector3f{0.0f,0.0f,0.0f}); - _sensor_simulator.run_seconds(10); + _sensor_simulator.setImuBias(accel_bias_sim, Vector3f{0.0f,0.0f,0.0f}); + _sensor_simulator.runSeconds(10); - float converged_pos[3]; - float converged_vel[3]; - float converged_accel_bias[3]; - float converged_gyro_bias[3]; - _ekf->get_position(converged_pos); - _ekf->get_velocity(converged_vel); - _ekf->get_accel_bias(converged_accel_bias); - _ekf->get_gyro_bias(converged_gyro_bias); + Vector3f pos = _ekf_wrapper.getPosition(); + Vector3f vel = _ekf_wrapper.getVelocity(); + Vector3f accel_bias = _ekf_wrapper.getAccelBias(); + Vector3f gyro_bias = _ekf_wrapper.getGyroBias(); + Vector3f zero{0.0f, 0.0f, 0.0f}; - // THEN: EKF should estimate bias correctelly - for(int i=0; i<3; ++i) - { - EXPECT_NEAR(0.0f,converged_pos[i],0.001f) << "i: " << i; - EXPECT_NEAR(0.0f,converged_vel[i],0.001f) << "i: " << i; - EXPECT_NEAR(accel_bias(i),converged_accel_bias[i],0.001f) << "i: " << i; - EXPECT_NEAR(0.0f,converged_gyro_bias[i],0.001f) << "i: " << i; - } + // THEN: EKF should stay or converge to zero + EXPECT_TRUE(matrix::isEqual(pos, zero, 0.001f)); + EXPECT_TRUE(matrix::isEqual(vel, zero, 0.001f)); + EXPECT_TRUE(matrix::isEqual(accel_bias, accel_bias_sim, 0.001f)); + EXPECT_TRUE(matrix::isEqual(gyro_bias, zero, 0.001f)); } // TODO: Add sampling tests diff --git a/test/test_EKF_fusionLogic.cpp b/test/test_EKF_fusionLogic.cpp new file mode 100644 index 0000000000..567277e27b --- /dev/null +++ b/test/test_EKF_fusionLogic.cpp @@ -0,0 +1,185 @@ +/**************************************************************************** + * + * Copyright (c) 2019 ECL Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Feeds Ekf with Imu data + * @author Kamil Ritz + */ + +#include +#include "EKF/ekf.h" +#include "sensor_simulator/sensor_simulator.h" +#include "sensor_simulator/ekf_wrapper.h" + + +class EkfFusionLogicTest : public ::testing::Test { + public: + + EkfFusionLogicTest(): ::testing::Test(), + _ekf{std::make_shared()}, + _sensor_simulator(_ekf), + _ekf_wrapper(_ekf) {}; + + std::shared_ptr _ekf; + SensorSimulator _sensor_simulator; + EkfWrapper _ekf_wrapper; + + // Setup the Ekf with synthetic measurements + void SetUp() override + { + _ekf->init(0); + _sensor_simulator.runSeconds(5); + } + + // Use this method to clean up any memory, network etc. after each test + void TearDown() override + { + } +}; + + +TEST_F(EkfFusionLogicTest, doGpsFusion) +{ + // GIVEN: a tilt and heading aligned filter + // WHEN: we enable GPS fusion and we send good quality gps data for 11s + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(15); + + // THEN: EKF should intend to fuse GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + // THEN: Local and global position should be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + + // WHEN: GPS data is not send for 11s + _sensor_simulator.stopGps(); + _sensor_simulator.runSeconds(11); + + // THEN: EKF should stop to intend to fuse GPS + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: GPS data is send again for 11s + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(11); + + // THEN: EKF should to intend to fuse GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_TRUE(_ekf->global_position_is_valid()); + + // // WHEN: clients decides to stop GPS fusion + // _ekf_wrapper.disableGpsFusion(); + // // THEN: EKF should stop to intend to fuse GPS immediately + // _sensor_simulator.runMicroseconds(1000); + // EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); + // THIS is not happening at the moment +} + +TEST_F(EkfFusionLogicTest, rejectGpsSignalJump) +{ + // GIVEN: a tilt and heading aligned filter + // WHEN: we enable GPS fusion and we send good quality gps data for 11s + _ekf_wrapper.enableGpsFusion(); + _sensor_simulator.startGps(); + _sensor_simulator.runSeconds(15); + + // THEN: EKF should intend to fuse GPS + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + // WHEN: Having a big horizontal position Gps jump coming from the Gps Receiver + Vector3f pos_old = _ekf_wrapper.getPosition(); + Vector3f vel_old = _ekf_wrapper.getVelocity(); + Vector3f accel_bias_old = _ekf_wrapper.getAccelBias(); + _sensor_simulator._gps.stepHorizontalPositionByMeters(Vector2f{10.0f, 0.0f}); + _sensor_simulator.runSeconds(2); + + // THEN: The estimate should not change much in the short run + // and GPS fusion should be stopped after a while. + Vector3f pos_new = _ekf_wrapper.getPosition(); + Vector3f vel_new = _ekf_wrapper.getVelocity(); + Vector3f accel_bias_new = _ekf_wrapper.getAccelBias(); + EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f)); + EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f)); + EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f)); + + _sensor_simulator.runSeconds(10); + pos_new = _ekf_wrapper.getPosition(); + vel_new = _ekf_wrapper.getVelocity(); + accel_bias_new = _ekf_wrapper.getAccelBias(); + EXPECT_TRUE(matrix::isEqual(pos_new, pos_old, 0.01f)); + EXPECT_TRUE(matrix::isEqual(vel_new, vel_old, 0.01f)); + EXPECT_TRUE(matrix::isEqual(accel_bias_new, accel_bias_old, 0.01f)); + // EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); // What do we expect here? +} + +TEST_F(EkfFusionLogicTest, doFlowFusion) +{ + // GIVEN: a tilt and heading aligned filter + // WHEN: sending flow data without having the flow fusion enabled + // flow measurement fusion should not be intended. + _sensor_simulator.startFlow(); + _sensor_simulator.runSeconds(3); + + // THEN: EKF should intend to fuse flow measurements + EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); + // THEN: Local and global position should not be valid + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: Flow data is not send and we enable flow fusion + _sensor_simulator.stopFlow(); + _sensor_simulator.runSeconds(3); // TODO: without this line tests fail + // probably there are still values in the buffer. + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.runSeconds(3); + + // THEN: EKF should not intend to fuse flow + EXPECT_FALSE(_ekf_wrapper.isIntendingFlowFusion()); + // THEN: Local and global position should not be valid + EXPECT_FALSE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); + + // WHEN: Flow data is send and we enable flow fusion + _sensor_simulator.startFlow(); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.runSeconds(10); + + // THEN: EKF should intend to fuse flow + EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); + // THEN: Local and global position should be valid + EXPECT_TRUE(_ekf->local_position_is_valid()); + EXPECT_FALSE(_ekf->global_position_is_valid()); +}