kamilritz
5 years ago
committed by
Mathieu Bresciani
5 changed files with 305 additions and 124 deletions
@ -0,0 +1,89 @@
@@ -0,0 +1,89 @@
|
||||
#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
|
||||
@ -0,0 +1,114 @@
@@ -0,0 +1,114 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
/**
|
||||
* This class is providing methods to feed the ECL EKF with measurement. |
||||
* It takes a pointer to the Ekf object and will manipulate the object |
||||
* by call set*Data functions. |
||||
* It simulates the time to allow for sensor data being set at certain rate |
||||
* and also calls the update method of the EKF |
||||
* @author Kamil Ritz <ka.ritz@hotmail.com> |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "EKF/ekf.h" |
||||
#include <math.h> |
||||
|
||||
|
||||
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 setGpsFusionTrue(){ _fuse_gps = true; } |
||||
void setGpsFusionFalse(){ _fuse_gps = false; } |
||||
|
||||
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(); |
||||
|
||||
|
||||
|
||||
}; |
||||
|
||||
// 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
|
||||
@ -0,0 +1,68 @@
@@ -0,0 +1,68 @@
|
||||
/****************************************************************************
|
||||
* |
||||
* 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. |
||||
* |
||||
****************************************************************************/ |
||||
|
||||
#include <gtest/gtest.h> |
||||
#include <math.h> |
||||
#include "SensorSimulator.h" |
||||
#include "EKF/ekf.h" |
||||
|
||||
using namespace simulator; |
||||
|
||||
|
||||
class SensorSimulatorTest : public ::testing::Test { |
||||
public: |
||||
|
||||
// Ekf* _ekf;
|
||||
// SensorSimulator* _sensor_simulator;
|
||||
|
||||
void SetUp() override |
||||
{ |
||||
// _ekf = new Ekf();
|
||||
// _ekf->init(0);
|
||||
} |
||||
|
||||
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)); |
||||
|
||||
|
||||
} |
||||
Loading…
Reference in new issue