From 7ed6a437c75da9b2157549866c2536fb0cf53bb1 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 9 Jan 2020 15:46:34 +0100 Subject: [PATCH] Add airspeed sensor to sensor_simulator --- .gitignore | 2 + test/sensor_simulator/.gitignore | 1 + test/sensor_simulator/CMakeLists.txt | 1 + test/sensor_simulator/airspeed.cpp | 32 +++++++++++ test/sensor_simulator/airspeed.h | 64 ++++++++++++++++++++++ test/sensor_simulator/baro.cpp | 2 +- test/sensor_simulator/baro.h | 2 +- test/sensor_simulator/flow.cpp | 3 +- test/sensor_simulator/flow.h | 2 +- test/sensor_simulator/gps.cpp | 24 +++++++- test/sensor_simulator/gps.h | 6 +- test/sensor_simulator/imu.cpp | 2 +- test/sensor_simulator/imu.h | 2 +- test/sensor_simulator/mag.cpp | 2 +- test/sensor_simulator/mag.h | 2 +- test/sensor_simulator/range_finder.cpp | 2 +- test/sensor_simulator/range_finder.h | 2 +- test/sensor_simulator/sensor.cpp | 6 +- test/sensor_simulator/sensor.h | 10 ++-- test/sensor_simulator/sensor_simulator.cpp | 29 ++++++---- test/sensor_simulator/sensor_simulator.h | 10 +++- test/sensor_simulator/vio.cpp | 2 +- test/sensor_simulator/vio.h | 2 +- 23 files changed, 177 insertions(+), 33 deletions(-) create mode 100644 test/sensor_simulator/.gitignore create mode 100644 test/sensor_simulator/airspeed.cpp create mode 100644 test/sensor_simulator/airspeed.h diff --git a/.gitignore b/.gitignore index 871efc8e9e..9fd936329f 100644 --- a/.gitignore +++ b/.gitignore @@ -1,6 +1,8 @@ *.DS_Store *.gcov *~ +*.orig + .cache/ build/ diff --git a/test/sensor_simulator/.gitignore b/test/sensor_simulator/.gitignore new file mode 100644 index 0000000000..c18dd8d83c --- /dev/null +++ b/test/sensor_simulator/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/test/sensor_simulator/CMakeLists.txt b/test/sensor_simulator/CMakeLists.txt index 88730c87cf..6dd4fa5313 100644 --- a/test/sensor_simulator/CMakeLists.txt +++ b/test/sensor_simulator/CMakeLists.txt @@ -43,6 +43,7 @@ set(SRCS flow.cpp range_finder.cpp vio.cpp + airspeed.cpp ) add_library(ecl_sensor_sim ${SRCS}) diff --git a/test/sensor_simulator/airspeed.cpp b/test/sensor_simulator/airspeed.cpp new file mode 100644 index 0000000000..6c6c174f20 --- /dev/null +++ b/test/sensor_simulator/airspeed.cpp @@ -0,0 +1,32 @@ +#include "airspeed.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +Airspeed::Airspeed(std::shared_ptr ekf):Sensor(ekf) +{ +} + +Airspeed::~Airspeed() +{ +} + +void Airspeed::send(uint64_t time) +{ + if(_true_airspeed_data > FLT_EPSILON && _indicated_airspeed_data > FLT_EPSILON) + { + float eas2tas = _true_airspeed_data / _indicated_airspeed_data; + _ekf->setAirspeedData(time, _true_airspeed_data, eas2tas); + } +} + +void Airspeed::setData(float true_airspeed, float indicated_airspeed) +{ + _true_airspeed_data = true_airspeed; + _indicated_airspeed_data = indicated_airspeed; +} + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/airspeed.h b/test/sensor_simulator/airspeed.h new file mode 100644 index 0000000000..546d77440b --- /dev/null +++ b/test/sensor_simulator/airspeed.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 Mag data + * @author Kamil Ritz + */ +#pragma once + +#include "sensor.h" + +namespace sensor_simulator +{ +namespace sensor +{ + +class Airspeed: public Sensor +{ +public: + Airspeed(std::shared_ptr ekf); + ~Airspeed(); + + void setData(float true_airspeed, float eas2tas); + +private: + float _true_airspeed_data; + float _indicated_airspeed_data; + + void send(uint64_t time) override; + +}; + +} // namespace sensor +} // namespace sensor_simulator diff --git a/test/sensor_simulator/baro.cpp b/test/sensor_simulator/baro.cpp index 0a0d7a9efb..c7f9b75750 100644 --- a/test/sensor_simulator/baro.cpp +++ b/test/sensor_simulator/baro.cpp @@ -13,7 +13,7 @@ Baro::~Baro() { } -void Baro::send(uint32_t time) +void Baro::send(uint64_t time) { _ekf->setBaroData(time,_baro_data); } diff --git a/test/sensor_simulator/baro.h b/test/sensor_simulator/baro.h index 6e55181a99..f6f3fe92a8 100644 --- a/test/sensor_simulator/baro.h +++ b/test/sensor_simulator/baro.h @@ -55,7 +55,7 @@ public: private: float _baro_data; - void send(uint32_t time) override; + void send(uint64_t time) override; }; diff --git a/test/sensor_simulator/flow.cpp b/test/sensor_simulator/flow.cpp index a6689c1d66..8c2328cbcc 100644 --- a/test/sensor_simulator/flow.cpp +++ b/test/sensor_simulator/flow.cpp @@ -13,8 +13,9 @@ Flow::~Flow() { } -void Flow::send(uint32_t time) +void Flow::send(uint64_t time) { + _flow_data.dt = time - _time_last_data_sent; _ekf->setOpticalFlowData(time, &_flow_data); } diff --git a/test/sensor_simulator/flow.h b/test/sensor_simulator/flow.h index b212efd2ba..947c9c815d 100644 --- a/test/sensor_simulator/flow.h +++ b/test/sensor_simulator/flow.h @@ -56,7 +56,7 @@ public: private: flow_message _flow_data; - void send(uint32_t time) override; + void send(uint64_t time) override; }; diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index 0c36e2a3c3..f6931a54e4 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -13,7 +13,7 @@ Gps::~Gps() { } -void Gps::send(uint32_t time) +void Gps::send(uint64_t time) { _gps_data.time_usec = time; _ekf->setGpsData(time, _gps_data); @@ -24,6 +24,28 @@ void Gps::setData(const gps_message& gps) _gps_data = gps; } +void Gps::setAltitude(int32_t alt) +{ + _gps_data.alt = alt; +} + +void Gps::setLatitude(int32_t lat) +{ + _gps_data.lat = lat; +} + +void Gps::setLongitude(int32_t lon) +{ + _gps_data.lon = lon; +} + +void Gps::setVelocity(const Vector3f& vel) +{ + _gps_data.vel_ned[0] = vel(0); + _gps_data.vel_ned[1] = vel(1); + _gps_data.vel_ned[2] = vel(2); +} + void Gps::stepHeightByMeters(float hgt_change) { _gps_data.alt += hgt_change * 1e3f; diff --git a/test/sensor_simulator/gps.h b/test/sensor_simulator/gps.h index 6686c53c1c..931846e555 100644 --- a/test/sensor_simulator/gps.h +++ b/test/sensor_simulator/gps.h @@ -53,13 +53,17 @@ public: void setData(const gps_message& gps); void stepHeightByMeters(float hgt_change); void stepHorizontalPositionByMeters(Vector2f hpos_change); + void setAltitude(int32_t alt); + void setLatitude(int32_t lat); + void setLongitude(int32_t lon); + void setVelocity(const Vector3f& vel); gps_message getDefaultGpsData(); private: gps_message _gps_data; - void send(uint32_t time) override; + void send(uint64_t time) override; }; diff --git a/test/sensor_simulator/imu.cpp b/test/sensor_simulator/imu.cpp index f80cc7ee5d..a966176ef6 100644 --- a/test/sensor_simulator/imu.cpp +++ b/test/sensor_simulator/imu.cpp @@ -13,7 +13,7 @@ Imu::~Imu() { } -void Imu::send(uint32_t time) +void Imu::send(uint64_t time) { imuSample imu_sample; imu_sample.time_us = time; diff --git a/test/sensor_simulator/imu.h b/test/sensor_simulator/imu.h index f088c000c9..bdd2632b92 100644 --- a/test/sensor_simulator/imu.h +++ b/test/sensor_simulator/imu.h @@ -58,7 +58,7 @@ private: Vector3f _accel_data; Vector3f _gyro_data; - void send(uint32_t time) override; + void send(uint64_t time) override; }; diff --git a/test/sensor_simulator/mag.cpp b/test/sensor_simulator/mag.cpp index 3e34e654bb..d40493ce44 100644 --- a/test/sensor_simulator/mag.cpp +++ b/test/sensor_simulator/mag.cpp @@ -13,7 +13,7 @@ Mag::~Mag() { } -void Mag::send(uint32_t time) +void Mag::send(uint64_t time) { float mag[3]; _mag_data.copyTo(mag); diff --git a/test/sensor_simulator/mag.h b/test/sensor_simulator/mag.h index 6d4fb1dc92..8b659ff672 100644 --- a/test/sensor_simulator/mag.h +++ b/test/sensor_simulator/mag.h @@ -55,7 +55,7 @@ public: private: Vector3f _mag_data; - void send(uint32_t time) override; + void send(uint64_t time) override; }; diff --git a/test/sensor_simulator/range_finder.cpp b/test/sensor_simulator/range_finder.cpp index 4796b8bcf3..130f43c6ca 100644 --- a/test/sensor_simulator/range_finder.cpp +++ b/test/sensor_simulator/range_finder.cpp @@ -13,7 +13,7 @@ RangeFinder::~RangeFinder() { } -void RangeFinder::send(uint32_t time) +void RangeFinder::send(uint64_t time) { _ekf->setRangeData(time, _range_data, _range_quality); } diff --git a/test/sensor_simulator/range_finder.h b/test/sensor_simulator/range_finder.h index 6afc87236c..42904d9e04 100644 --- a/test/sensor_simulator/range_finder.h +++ b/test/sensor_simulator/range_finder.h @@ -57,7 +57,7 @@ private: float _range_data; int8_t _range_quality; - void send(uint32_t time) override; + void send(uint64_t time) override; }; diff --git a/test/sensor_simulator/sensor.cpp b/test/sensor_simulator/sensor.cpp index c0506340f1..dea0974b1b 100644 --- a/test/sensor_simulator/sensor.cpp +++ b/test/sensor_simulator/sensor.cpp @@ -11,7 +11,7 @@ Sensor::~Sensor() { } -void Sensor::update(uint32_t time) +void Sensor::update(uint64_t time) { if(should_send(time)) { @@ -20,12 +20,12 @@ void Sensor::update(uint32_t time) } } -bool Sensor::should_send(uint32_t time) const +bool Sensor::should_send(uint64_t time) const { return _is_running && is_time_to_send(time); } -bool Sensor::is_time_to_send(uint32_t time) const +bool Sensor::is_time_to_send(uint64_t time) const { return (time >= _time_last_data_sent) && ((time - _time_last_data_sent) >= _update_period); } diff --git a/test/sensor_simulator/sensor.h b/test/sensor_simulator/sensor.h index 55440f889a..a7d6222a3b 100644 --- a/test/sensor_simulator/sensor.h +++ b/test/sensor_simulator/sensor.h @@ -52,7 +52,7 @@ public: Sensor(std::shared_ptr ekf); virtual ~Sensor(); - void update(uint32_t time); + void update(uint64_t time); void setRateHz(uint32_t rate){ _update_period = uint32_t(1000000)/rate; } @@ -67,17 +67,17 @@ protected: std::shared_ptr _ekf; // time in microseconds uint32_t _update_period; - uint32_t _time_last_data_sent{0}; + uint64_t _time_last_data_sent{0}; bool _is_running{false}; - bool should_send(uint32_t time) const; + bool should_send(uint64_t time) const; // Checks that the right amount time passed since last send data to fulfill rate - bool is_time_to_send(uint32_t time) const; + bool is_time_to_send(uint64_t time) const; // call set*Data function of Ekf - virtual void send(uint32_t time) = 0; + virtual void send(uint64_t time) = 0; }; diff --git a/test/sensor_simulator/sensor_simulator.cpp b/test/sensor_simulator/sensor_simulator.cpp index 6b5ff1b906..b779f8bfee 100644 --- a/test/sensor_simulator/sensor_simulator.cpp +++ b/test/sensor_simulator/sensor_simulator.cpp @@ -9,7 +9,8 @@ _baro(ekf), _gps(ekf), _flow(ekf), _rng(ekf), -_vio(ekf) +_vio(ekf), +_airspeed(ekf) { setSensorDataToDefault(); setSensorRateToDefault(); @@ -30,6 +31,7 @@ void SensorSimulator::setSensorDataToDefault() _flow.setRateHz(50); _rng.setRateHz(30); _vio.setRateHz(30); + _airspeed.setRateHz(30); // TODO: check this rate } void SensorSimulator::setSensorRateToDefault() { @@ -41,6 +43,7 @@ void SensorSimulator::setSensorRateToDefault() _flow.setData(_flow.dataAtRest()); _rng.setData(0.2f, 100); _vio.setData(_vio.dataAtRest()); + _airspeed.setData(0.0f, 0.0f); } void SensorSimulator::startBasicSensor() { @@ -57,22 +60,28 @@ void SensorSimulator::runSeconds(float duration_seconds) void SensorSimulator::runMicroseconds(uint32_t duration) { // simulate in 1000us steps - const uint32_t start_time = _time; + const uint64_t start_time = _time; - for(;_time < start_time + duration; _time+=1000) + for(;_time < start_time + (uint64_t)duration; _time+=1000) { - _imu.update(_time); - _mag.update(_time); - _baro.update(_time); - _gps.update(_time); - _flow.update(_time); - _rng.update(_time); - _vio.update(_time); + updateSensors(); _ekf->update(); } } +void SensorSimulator::updateSensors() +{ + _imu.update(_time); + _mag.update(_time); + _baro.update(_time); + _gps.update(_time); + _flow.update(_time); + _rng.update(_time); + _vio.update(_time); + _airspeed.update(_time); +} + void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias) { _imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias, diff --git a/test/sensor_simulator/sensor_simulator.h b/test/sensor_simulator/sensor_simulator.h index 51169eb6c9..4cb0c3d191 100644 --- a/test/sensor_simulator/sensor_simulator.h +++ b/test/sensor_simulator/sensor_simulator.h @@ -51,6 +51,7 @@ #include "flow.h" #include "range_finder.h" #include "vio.h" +#include "airspeed.h" #include "EKF/ekf.h" using namespace sensor_simulator::sensor; @@ -61,16 +62,19 @@ class SensorSimulator private: std::shared_ptr _ekf; - uint32_t _time {0}; // in microseconds + uint64_t _time {0}; // in microseconds void setSensorDataToDefault(); void setSensorRateToDefault(); void startBasicSensor(); + void updateSensors(); public: SensorSimulator(std::shared_ptr ekf); ~SensorSimulator(); + uint64_t getTime() const{ return _time; }; + void runSeconds(float duration_seconds); void runMicroseconds(uint32_t duration); @@ -86,6 +90,9 @@ public: void startExternalVision(){ _vio.start(); } void stopExternalVision(){ _vio.stop(); } + void startAirspeedSensor(){ _airspeed.start(); } + void stopAirspeedSensor(){ _airspeed.stop(); } + void setImuBias(Vector3f accel_bias, Vector3f gyro_bias); void simulateOrientation(Quatf orientation); @@ -96,4 +103,5 @@ public: Flow _flow; RangeFinder _rng; Vio _vio; + Airspeed _airspeed; }; diff --git a/test/sensor_simulator/vio.cpp b/test/sensor_simulator/vio.cpp index 856047e207..8051c6b844 100644 --- a/test/sensor_simulator/vio.cpp +++ b/test/sensor_simulator/vio.cpp @@ -13,7 +13,7 @@ Vio::~Vio() { } -void Vio::send(uint32_t time) +void Vio::send(uint64_t time) { _ekf->setExtVisionData(time, &_vio_data); } diff --git a/test/sensor_simulator/vio.h b/test/sensor_simulator/vio.h index a53f0238bb..731dda9ffd 100644 --- a/test/sensor_simulator/vio.h +++ b/test/sensor_simulator/vio.h @@ -63,7 +63,7 @@ public: private: ext_vision_message _vio_data; - void send(uint32_t time) override; + void send(uint64_t time) override; };