From 2d2edd90e367dbee22a57b84bac0ae0b3a70647e Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 12 Dec 2019 18:39:20 +0100 Subject: [PATCH] ECL Unit Test Clean Up --- test/CMakeLists.txt | 4 +- test/SensorSimulator.cpp | 89 ------------------- test/SensorSimulator/Baro.cpp | 20 +++++ .../Baro.h} | 42 ++++----- test/SensorSimulator/CMakeLists.txt | 45 ++++++++++ test/SensorSimulator/Gps.cpp | 20 +++++ test/SensorSimulator/Gps.h | 56 ++++++++++++ test/SensorSimulator/Imu.cpp | 39 ++++++++ test/SensorSimulator/Imu.h | 59 ++++++++++++ test/SensorSimulator/Mag.cpp | 22 +++++ test/SensorSimulator/Mag.h | 56 ++++++++++++ test/SensorSimulator/Sensor.cpp | 28 ++++++ test/SensorSimulator/Sensor.h | 78 ++++++++++++++++ test/SensorSimulator/SensorSimulator.cpp | 81 +++++++++++++++++ test/{ => SensorSimulator}/SensorSimulator.h | 72 +++++---------- test/test_EKF_basics.cpp | 16 ++-- 16 files changed, 548 insertions(+), 179 deletions(-) delete mode 100644 test/SensorSimulator.cpp create mode 100644 test/SensorSimulator/Baro.cpp rename test/{test_SensorSimulator.cpp => SensorSimulator/Baro.h} (74%) create mode 100644 test/SensorSimulator/CMakeLists.txt create mode 100644 test/SensorSimulator/Gps.cpp create mode 100644 test/SensorSimulator/Gps.h create mode 100644 test/SensorSimulator/Imu.cpp create mode 100644 test/SensorSimulator/Imu.h create mode 100644 test/SensorSimulator/Mag.cpp create mode 100644 test/SensorSimulator/Mag.h create mode 100644 test/SensorSimulator/Sensor.cpp create mode 100644 test/SensorSimulator/Sensor.h create mode 100644 test/SensorSimulator/SensorSimulator.cpp rename test/{ => SensorSimulator}/SensorSimulator.h (58%) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0a0ad2bbd5..741a2a2dc1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -33,8 +33,7 @@ include(gtest.cmake) -add_library(SENSOR_SIMULATOR SensorSimulator.cpp) -target_link_libraries(SENSOR_SIMULATOR ecl_EKF) +add_subdirectory(SensorSimulator) set(SRCS main.cpp @@ -42,7 +41,6 @@ set(SRCS test_EKF_ringbuffer.cpp test_EKF_imuSampling.cpp test_AlphaFilter.cpp - test_SensorSimulator.cpp ) add_executable(ECL_GTESTS ${SRCS}) diff --git a/test/SensorSimulator.cpp b/test/SensorSimulator.cpp deleted file mode 100644 index 5783a1a62c..0000000000 --- a/test/SensorSimulator.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include "SensorSimulator.h" - -namespace simulator -{ - -SensorSimulator::SensorSimulator(Ekf* ekf) -{ - _ekf = ekf; - setGpsMessageToDefaul(); -} - -SensorSimulator::~SensorSimulator() -{ - -} - -void SensorSimulator::setGpsMessageToDefaul() -{ - // setup gps message to reasonable default values - _gps_message.time_usec = 0; - _gps_message.lat = 473566094; - _gps_message.lon = 85190237; - _gps_message.alt = 422056; - _gps_message.yaw = 0.0f; - _gps_message.yaw_offset = 0.0f; - _gps_message.fix_type = 3; - _gps_message.eph = 0.5f; - _gps_message.epv = 0.8f; - _gps_message.sacc = 0.2f; - _gps_message.vel_m_s = 0.0; - _gps_message.vel_ned[0] = 0.0f; - _gps_message.vel_ned[1] = 0.0f; - _gps_message.vel_ned[2] = 0.0f; - _gps_message.vel_ned_valid = 1; - _gps_message.nsats = 16; - _gps_message.pdop = 0.0f; -} - -void SensorSimulator::update_with_const_sensors(uint32_t duration_us, - Vector3f ang_vel, Vector3f accel, - Vector3f mag_data, float baro_data) -{ - // store start time - uint32_t start_time_us = _t_us; - - // compute update time step such that we can update the basic sensor at different rates - _update_dt_us = gcd(_imu_dt_us, gcd(_mag_dt_us, gcd(_baro_dt_us,_gps_dt_us))); - - // update EKF with synthetic sensor measurements - for( ; _t_us < start_time_us+duration_us; _t_us += _update_dt_us) - { - // Check which sensors update we should do - if(_fuse_imu && !(_t_us %_imu_dt_us)) - { - // push imu data into estimator - imuSample imu_sample_new; - imu_sample_new.time_us = _t_us; - imu_sample_new.delta_ang_dt = _imu_dt_us * 1.e-6f; - imu_sample_new.delta_ang = ang_vel * imu_sample_new.delta_ang_dt; - imu_sample_new.delta_vel_dt = _imu_dt_us * 1.e-6f; - imu_sample_new.delta_vel = accel * imu_sample_new.delta_vel_dt; - - _ekf->setIMUData(imu_sample_new); - _counter_imu++; - } - if(_fuse_baro && !(_t_us % _baro_dt_us)) - { - _ekf->setBaroData(_t_us,baro_data); - _counter_baro++; - } - if(_fuse_mag && !(_t_us % _mag_dt_us)) - { - float mag[3]; - mag_data.copyTo(mag); - _ekf->setMagData(_t_us,mag); - _counter_mag++; - } - if(_fuse_gps && !(_t_us % _gps_dt_us)) - { - _gps_message.time_usec = _t_us; - _ekf->setGpsData(_t_us,_gps_message); - _counter_mag++; - } - - _ekf->update(); - } -} - -} // end of namespace simulator diff --git a/test/SensorSimulator/Baro.cpp b/test/SensorSimulator/Baro.cpp new file mode 100644 index 0000000000..b490cb63f5 --- /dev/null +++ b/test/SensorSimulator/Baro.cpp @@ -0,0 +1,20 @@ +#include "Baro.h" + +Baro::Baro(Ekf* ekf):Sensor(ekf) +{ +} + +Baro::~Baro() +{ +} + +void Baro::send(uint32_t time) +{ + _ekf->setBaroData(time,_baro_data); + _time_last_data_sent = time; +} + +void Baro::setData(float baro) +{ + _baro_data = baro; +} diff --git a/test/test_SensorSimulator.cpp b/test/SensorSimulator/Baro.h similarity index 74% rename from test/test_SensorSimulator.cpp rename to test/SensorSimulator/Baro.h index 94c5bd4925..becbcf2cba 100644 --- a/test/test_SensorSimulator.cpp +++ b/test/SensorSimulator/Baro.h @@ -31,38 +31,26 @@ * ****************************************************************************/ -#include -#include -#include "SensorSimulator.h" -#include "EKF/ekf.h" +/** + * Feeds Ekf with Mag data + * @author Kamil Ritz + */ +#pragma once -using namespace simulator; +#include "Sensor.h" +class Baro: public Sensor +{ +public: + Baro(Ekf* ekf); + ~Baro(); -class SensorSimulatorTest : public ::testing::Test { - public: + void setData(float baro); - // Ekf* _ekf; - // SensorSimulator* _sensor_simulator; +private: + float _baro_data; - void SetUp() override - { - // _ekf = new Ekf(); - // _ekf->init(0); - } + void send(uint32_t time); - void TearDown() override - { - // delete _sensor_simulator; - // delete _ekf; - } }; -TEST_F(SensorSimulatorTest, greatestCommonDivider) -{ - // Test gcd function with some example - EXPECT_EQ(gcd(uint32_t(3), uint32_t(402)), uint32_t(3)); - EXPECT_EQ(gcd(uint32_t(3), uint32_t(401)), uint32_t(1)); - - -} diff --git a/test/SensorSimulator/CMakeLists.txt b/test/SensorSimulator/CMakeLists.txt new file mode 100644 index 0000000000..644c5dc3b2 --- /dev/null +++ b/test/SensorSimulator/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# 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 ECL 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. +# +############################################################################ + + +set(SRCS + SensorSimulator.cpp + Sensor.cpp + Imu.cpp + Mag.cpp + Baro.cpp + Gps.cpp + ) + +add_library(SENSOR_SIMULATOR ${SRCS}) +target_link_libraries(SENSOR_SIMULATOR ecl_EKF) diff --git a/test/SensorSimulator/Gps.cpp b/test/SensorSimulator/Gps.cpp new file mode 100644 index 0000000000..dcb007adc3 --- /dev/null +++ b/test/SensorSimulator/Gps.cpp @@ -0,0 +1,20 @@ +#include "Gps.h" + +Gps::Gps(Ekf* ekf):Sensor(ekf) +{ +} + +Gps::~Gps() +{ +} + +void Gps::send(uint32_t time) +{ + _gps_data.time_usec = time; + _ekf->setGpsData(time, _gps_data); +} + +void Gps::setData(gps_message gps) +{ + _gps_data = gps; +} diff --git a/test/SensorSimulator/Gps.h b/test/SensorSimulator/Gps.h new file mode 100644 index 0000000000..65623fdcbb --- /dev/null +++ b/test/SensorSimulator/Gps.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 Gps data + * @author Kamil Ritz + */ +#pragma once + +#include "Sensor.h" + +class Gps: public Sensor +{ +public: + Gps(Ekf* ekf); + ~Gps(); + + void setData(gps_message gps); + +private: + gps_message _gps_data; + + void send(uint32_t time); + +}; + diff --git a/test/SensorSimulator/Imu.cpp b/test/SensorSimulator/Imu.cpp new file mode 100644 index 0000000000..45ea968298 --- /dev/null +++ b/test/SensorSimulator/Imu.cpp @@ -0,0 +1,39 @@ +#include "Imu.h" + +Imu::Imu(Ekf* ekf):Sensor(ekf) +{ +} + +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; + imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt; + imu_sample.delta_vel_dt = (time - _time_last_data_sent) * 1.e-6f; + imu_sample.delta_vel = _accel_data * imu_sample.delta_vel_dt; + + _ekf->setIMUData(imu_sample); + _time_last_data_sent = time; +} + +void Imu::setData(Vector3f accel, Vector3f gyro) +{ + setAccelData(accel); + setGyroData(gyro); +} + +void Imu::setAccelData(Vector3f accel) +{ + _accel_data = accel; +} + +void Imu::setGyroData(Vector3f gyro) +{ + _gyro_data = gyro; +} diff --git a/test/SensorSimulator/Imu.h b/test/SensorSimulator/Imu.h new file mode 100644 index 0000000000..d6e2a9e08e --- /dev/null +++ b/test/SensorSimulator/Imu.h @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * 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 + */ +#pragma once + +#include "Sensor.h" + +class Imu: public Sensor +{ +public: + Imu(Ekf* ekf); + ~Imu(); + + void setData(Vector3f accel, Vector3f gyro); + void setAccelData(Vector3f accel); + void setGyroData(Vector3f gyro); + +private: + Vector3f _accel_data; + Vector3f _gyro_data; + + void send(uint32_t time); + +}; + diff --git a/test/SensorSimulator/Mag.cpp b/test/SensorSimulator/Mag.cpp new file mode 100644 index 0000000000..1ed0557796 --- /dev/null +++ b/test/SensorSimulator/Mag.cpp @@ -0,0 +1,22 @@ +#include "Mag.h" + +Mag::Mag(Ekf* ekf):Sensor(ekf) +{ +} + +Mag::~Mag() +{ +} + +void Mag::send(uint32_t time) +{ + float mag[3]; + _mag_data.copyTo(mag); + _ekf->setMagData(time,mag); + _time_last_data_sent = time; +} + +void Mag::setData(Vector3f mag) +{ + _mag_data = mag; +} diff --git a/test/SensorSimulator/Mag.h b/test/SensorSimulator/Mag.h new file mode 100644 index 0000000000..412cda8fe0 --- /dev/null +++ b/test/SensorSimulator/Mag.h @@ -0,0 +1,56 @@ +/**************************************************************************** + * + * 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 Mag data + * @author Kamil Ritz + */ +#pragma once + +#include "Sensor.h" + +class Mag: public Sensor +{ +public: + Mag(Ekf* ekf); + ~Mag(); + + void setData(Vector3f mag); + +private: + Vector3f _mag_data; + + void send(uint32_t time); + +}; + diff --git a/test/SensorSimulator/Sensor.cpp b/test/SensorSimulator/Sensor.cpp new file mode 100644 index 0000000000..adfa8fed3a --- /dev/null +++ b/test/SensorSimulator/Sensor.cpp @@ -0,0 +1,28 @@ +#include "Sensor.h" + +Sensor::Sensor(Ekf* ekf) +{ + _ekf = ekf; +} + +Sensor::~Sensor() +{ +} + +void Sensor::update(uint32_t time) +{ + if(should_send(time)) + { + send(time); + } +} + +bool Sensor::should_send(uint32_t time) +{ + return _is_running && is_time_to_send(time); +} + +bool Sensor::is_time_to_send(uint32_t time) +{ + return (time >= _time_last_data_sent) && ((time - _time_last_data_sent) >= _update_period); +} diff --git a/test/SensorSimulator/Sensor.h b/test/SensorSimulator/Sensor.h new file mode 100644 index 0000000000..af259201eb --- /dev/null +++ b/test/SensorSimulator/Sensor.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * Base class for defining the interface for simulaton of a sensor + * @author Kamil Ritz + */ + +#pragma once + +#include "EKF/ekf.h" +#include + +class Sensor +{ +public: + + Sensor(Ekf* ekf); + virtual ~Sensor(); + + void update(uint32_t time); + + void setRate(uint32_t rate){ _update_period = uint32_t(1000000)/rate; } + + bool isRunning(){ return _is_running; } + + void start(){ _is_running = true; } + + void stop(){ _is_running = false; } + +protected: + + Ekf* _ekf; + // time in microseconds + uint32_t _update_period; + uint32_t _time_last_data_sent{0}; + + bool _is_running{false}; + + bool should_send(uint32_t time); + + // Checks that the right amount time passed since last send data to fulfill rate + bool is_time_to_send(uint32_t time); + + // call set*Data function of Ekf + virtual void send(uint32_t time) = 0; + +}; diff --git a/test/SensorSimulator/SensorSimulator.cpp b/test/SensorSimulator/SensorSimulator.cpp new file mode 100644 index 0000000000..c643485ad8 --- /dev/null +++ b/test/SensorSimulator/SensorSimulator.cpp @@ -0,0 +1,81 @@ +#include "SensorSimulator.h" + + +SensorSimulator::SensorSimulator(Ekf* ekf): +_imu{ekf}, +_mag{ekf}, +_baro{ekf}, +_gps{ekf} +{ + _ekf = 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(); +} + +SensorSimulator::~SensorSimulator() +{ + +} + +gps_message SensorSimulator::getDefaultGpsData() +{ + // 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; +} + +void SensorSimulator::run(uint32_t duration) +{ + // simulate in 1000us steps + uint32_t start_time = _time; + + for(;_time < start_time + duration; _time+=1000) + { + _imu.update(_time); + _mag.update(_time); + _baro.update(_time); + _gps.update(_time); + + _ekf->update(); + } +} + +void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias) +{ + _imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias, + Vector3f{0.0f,0.0f,0.0f} + gyro_bias); +} diff --git a/test/SensorSimulator.h b/test/SensorSimulator/SensorSimulator.h similarity index 58% rename from test/SensorSimulator.h rename to test/SensorSimulator/SensorSimulator.h index 268e8f0331..8e24b60720 100644 --- a/test/SensorSimulator.h +++ b/test/SensorSimulator/SensorSimulator.h @@ -42,73 +42,41 @@ #pragma once +#include "Imu.h" +#include "Mag.h" +#include "Baro.h" +#include "Gps.h" #include "EKF/ekf.h" -#include -namespace simulator -{ - class SensorSimulator { public: SensorSimulator(Ekf* ekf); ~SensorSimulator(); - 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); + 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 setGpsFusionTrue(){ _fuse_gps = true; } - void setGpsFusionFalse(){ _fuse_gps = false; } + void run(uint32_t duration); + + void startGps(){ _gps.start(); } + void stopGps(){ _gps.stop(); } + + void setImuBias(Vector3f accel_bias, Vector3f gyro_bias); private: Ekf* _ekf; - // current time - uint32_t _t_us{0}; - - // Basics sensors - const uint32_t _imu_dt_us{4000}; // 250 Hz Period between IMU updates - const uint32_t _baro_dt_us{12500}; // 80 Hz Period between barometer updates - const uint32_t _mag_dt_us{12500}; // 80 Hz Period between magnetometer updates - const uint32_t _gps_dt_us{200000}; // 5 Hz Period between GPS updates - // const uint32_t _flow_dt_us{20000}; // 50 Hz Period between Flow updates - // const uint32_t _ev_dt_us{40000}; // 25 Hz Period between external vision updates - - uint32_t _update_dt_us{}; // greatest common divider of all basic sensor periods - - - // Flags that control if a sensor is fused - bool _fuse_imu{true}; - bool _fuse_baro{true}; - bool _fuse_mag{true}; - // Not expected to be fused from beginning - bool _fuse_gps{false}; - // bool _fuse_flow{false}; - // bool _fuse_ev{false}; - - gps_message _gps_message{}; - - // used for debugging until now, replace with tests - // counter of how many sensor measurement are put into Ekf - uint32_t _counter_imu{0}; - uint32_t _counter_baro{0}; - uint32_t _counter_mag{0}; - - - void setGpsMessageToDefaul(); + Imu _imu; + Mag _mag; + Baro _baro; + Gps _gps; + uint32_t _time {0}; // in microseconds + gps_message getDefaultGpsData(); }; - -// Compute greatest common divider -inline uint32_t gcd(uint32_t a, uint32_t b) -{ - return b == 0 ? a : gcd(b, a % b); -} - -} // end of namespace diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index 4fd27ae832..603b60abb5 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -34,9 +34,8 @@ #include #include #include "EKF/ekf.h" -#include "SensorSimulator.h" +#include "SensorSimulator/SensorSimulator.h" -using namespace simulator; class EkfInitializationTest : public ::testing::Test { public: @@ -54,7 +53,7 @@ class EkfInitializationTest : public ::testing::Test { _ekf->init(0); _sensor_simulator = new SensorSimulator(_ekf); - _sensor_simulator->update_with_const_sensors(_init_duration_us); + _sensor_simulator->run(_init_duration_us); } // Use this method to clean up any memory, network etc. after each test @@ -111,7 +110,7 @@ TEST_F(EkfInitializationTest, convergesToZero) { // GIVEN: initialized EKF with default IMU, baro and mag input for 2s // WHEN: Added more defautl sensor measurements - _sensor_simulator->update_with_const_sensors(4000000); // for further 4s + _sensor_simulator->run(4000000); // for further 4s float converged_pos[3]; float converged_vel[3]; @@ -137,8 +136,8 @@ TEST_F(EkfInitializationTest, gpsFusion) // GIVEN: initialized EKF with default IMU, baro and mag input for 2s // WHEN: setting GPS measurements for 11s, minimum GPS health time is set to 10 sec - _sensor_simulator->setGpsFusionTrue(); - _sensor_simulator->update_with_const_sensors(11000000,Vector3f{0.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}); // for further 3s + _sensor_simulator->startGps(); + _sensor_simulator->run(11000000); // for further 3s // THEN: EKF should fuse GPS, but no other position sensor filter_control_status_u control_status; @@ -176,8 +175,9 @@ TEST_F(EkfInitializationTest, accleBiasEstimation) // WHEN: Added more sensor measurements with accel bias and gps measurements Vector3f accel_bias = {0.0f,0.0f,0.1f}; - _sensor_simulator->setGpsFusionTrue(); - _sensor_simulator->update_with_const_sensors(10000000,Vector3f{0.0f,0.0f,0.0f},Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}+accel_bias); // for further 10s + _sensor_simulator->startGps(); + _sensor_simulator->setImuBias(accel_bias, Vector3f{0.0f,0.0f,0.0f}); + _sensor_simulator->run(10000000); float converged_pos[3]; float converged_vel[3];