From 980886e99bcb76851dbca9f543d464416075423a Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Thu, 30 Aug 2018 23:34:31 +0200 Subject: [PATCH 01/51] DUNE/Hardware/PWM: class for interfacing with HW PWM --- src/DUNE/Hardware.hpp | 1 + src/DUNE/Hardware/PWM.cpp | 183 ++++++++++++++++++++++++++++++++++++++ src/DUNE/Hardware/PWM.hpp | 80 +++++++++++++++++ 3 files changed, 264 insertions(+) create mode 100644 src/DUNE/Hardware/PWM.cpp create mode 100644 src/DUNE/Hardware/PWM.hpp diff --git a/src/DUNE/Hardware.hpp b/src/DUNE/Hardware.hpp index e690999c62..e83f677d16 100644 --- a/src/DUNE/Hardware.hpp +++ b/src/DUNE/Hardware.hpp @@ -55,5 +55,6 @@ namespace DUNE #include #include #include +#include #endif diff --git a/src/DUNE/Hardware/PWM.cpp b/src/DUNE/Hardware/PWM.cpp new file mode 100644 index 0000000000..e04b6a1278 --- /dev/null +++ b/src/DUNE/Hardware/PWM.cpp @@ -0,0 +1,183 @@ +//*************************************************************************** +// Copyright 2007-2017 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Kristoffer Gryte * +//*************************************************************************** + +#include +#include + +#include +#include +#include + +#include + +namespace DUNE +{ + namespace Hardware + { + using DUNE::System::Error; + using DUNE::Utils::String; + + PWM::PWM() + { + PWM(0); + } + + PWM::PWM(const unsigned pwm_number) + { + PWM(pwm_number, "/sys/class/pwm/pwmchip0/"); + } + + PWM::PWM(const unsigned pwm_number, const std::string& chip_path): + m_pwm_number(pwm_number), + m_chip_path(chip_path) + { +# if defined(DUNE_OS_LINUX) + writeToFile(m_chip_path + "export", m_pwm_number); + + const std::string prefix = m_chip_path + "pwm" + String::str(m_pwm_number); + m_file_duty_cycle_path = prefix + "/duty_cycle"; + m_file_period_path = prefix + "/period"; + m_file_enable_path = prefix + "/enable"; + + enable(); +# else + throw Error("unimplemented feature", "DUNE::Hardware::PWM"); +# endif + } + + PWM::~PWM() + { +# if defined(DUNE_OS_LINUX) + try + { + writeToFile(m_chip_path + "unexport", m_pwm_number); + } + catch (std::exception& e) + { + DUNE_ERR("DUNE::Hardware::PWM", e.what()); + } +# endif + } + + void + PWM::setFrequency(const float frequency_hertz) + { + const float period_seconds = 1 / frequency_hertz; + setPeriod(period_seconds); + } + + void + PWM::setPeriod(const float period_seconds) + { + const unsigned period_nanoseconds = period_seconds * 1e9; + setPeriod(period_nanoseconds); + } + + void + PWM::setPeriod(const unsigned period_nanoseconds) + { +# if defined(DUNE_OS_LINUX) + writeToFile(m_file_period_path, period_nanoseconds); +# endif + m_period_nanoseconds = period_nanoseconds; + } + + void + PWM::setDutyCyclePercentage(const float duty_cycle_percentage) + { + const float duty_cycle_normalized = duty_cycle_percentage * 0.01; + setDutyCycleNormalized(duty_cycle_normalized); + } + + void + PWM::setDutyCycleNormalized(const float duty_cycle_normalized) + { + const unsigned pulse_width_nanoseconds = m_period_nanoseconds * duty_cycle_normalized; + setPulseWidth(pulse_width_nanoseconds); + } + + void + PWM::setPulseWidth(const float pulse_width_seconds) + { + const unsigned pulse_width_nanoseconds = pulse_width_seconds * 1e9; + setPulseWidth(pulse_width_nanoseconds); + } + + void + PWM::setPulseWidth(unsigned pulse_width_nanoseconds) + { + if (pulse_width_nanoseconds > m_period_nanoseconds) + pulse_width_nanoseconds = m_period_nanoseconds; + +# if defined(DUNE_OS_LINUX) + writeToFile(m_file_duty_cycle_path, pulse_width_nanoseconds); +# else + throw Error("unimplemented feature", "DUNE::Hardware::PWM"); +# endif + } + + void + PWM::enable() + { +# if defined(DUNE_OS_LINUX) + writeToFile(m_file_enable_path, 1); +# else + throw Error("unimplemented feature", "DUNE::Hardware::PWM"); +# endif + } + + void + PWM::disable() + { +# if defined(DUNE_OS_LINUX) + writeToFile(m_file_enable_path, 0); +# else + throw Error("unimplemented feature", "DUNE::Hardware::PWM"); +# endif + } + +# if defined(DUNE_OS_LINUX) + void + PWM::writeToFile(const std::string& file, unsigned value) + { + writeToFile(file, String::str(value)); + } + + void + PWM::writeToFile(const std::string& file, const std::string& value) + { + std::FILE* fd = std::fopen(file.c_str(), "w"); + if (fd == 0) + throw Error(errno, "unable to export PWM " + file, value); + std::fputs(value.c_str(), fd); + std::fclose(fd); + } +# endif + } +} diff --git a/src/DUNE/Hardware/PWM.hpp b/src/DUNE/Hardware/PWM.hpp new file mode 100644 index 0000000000..19ea563651 --- /dev/null +++ b/src/DUNE/Hardware/PWM.hpp @@ -0,0 +1,80 @@ +//*************************************************************************** +// Copyright 2007-2017 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Kristoffer Gryte * +//*************************************************************************** + +#ifndef USER2_HARDWARE_PWM_HPP_INCLUDED_ +#define USER2_HARDWARE_PWM_HPP_INCLUDED_ + +#include + +namespace DUNE +{ + namespace Hardware + { + class DUNE_DLL_SYM PWM; + + class PWM + { + public: + PWM(); + PWM(unsigned pwm_number); + PWM(unsigned pwm_number, const std::string& chip_path); + ~PWM(); + + // You'll only need one of these + void setFrequency(float frequency_hertz); + void setPeriod(float period_seconds); + + void setDutyCyclePercentage(float duty_cycle_percentage); + void setDutyCycleNormalized(float duty_cycle_normalized); + void setPulseWidth(float pulse_width_seconds); + + void enable(); + void disable(); + + private: + unsigned m_pwm_number; + std::string m_chip_path; + unsigned m_period_nanoseconds; + + void setPeriod(unsigned period_nanoseconds); + void setPulseWidth(unsigned active_time_nanoseconds); + +# if defined(DUNE_OS_LINUX) + std::string m_file_duty_cycle_path; + std::string m_file_period_path; + std::string m_file_enable_path; + + static void writeToFile(const std::string& file, unsigned value); + static void writeToFile(const std::string& file, const std::string& value); +# endif + }; + } +} + +#endif From ae60233a243c5706c013e4070f22b4587bd600e4 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Thu, 30 Aug 2018 23:53:38 +0200 Subject: [PATCH 02/51] DUNE/Coordinates/WGS84: more accurate displace function --- src/DUNE/Coordinates/WGS84.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/DUNE/Coordinates/WGS84.hpp b/src/DUNE/Coordinates/WGS84.hpp index 8e83dc6ab2..ccc4dfb23e 100644 --- a/src/DUNE/Coordinates/WGS84.hpp +++ b/src/DUNE/Coordinates/WGS84.hpp @@ -182,7 +182,9 @@ namespace DUNE toECEF(*lat, *lon, *hae, &x, &y, &z); // Compute Geocentric latitude - double phi = std::atan2(z, std::sqrt(x * x + y * y)); + double N = computeRn(*lat); + double p = std::sqrt(x * x + y * y); + double phi = std::atan2(z,p*(1 - c_wgs84_e2 * N / (N + *hae))); // Compute all needed sine and cosine terms for conversion. double slon = std::sin(*lon); From 0ead2aab8f6fab8e05828b11b52329fe3748c530 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Thu, 30 Aug 2018 23:54:02 +0200 Subject: [PATCH 03/51] DUNE/Coordinates/WGS84: make to/fromECEF public --- src/DUNE/Coordinates/WGS84.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/Coordinates/WGS84.hpp b/src/DUNE/Coordinates/WGS84.hpp index ccc4dfb23e..7c9d8228a7 100644 --- a/src/DUNE/Coordinates/WGS84.hpp +++ b/src/DUNE/Coordinates/WGS84.hpp @@ -287,7 +287,6 @@ namespace DUNE getNEBearingAndRange(lat1, lon1, lat2, lon2, azimuth, &tmp); } - private: //! Convert WGS-84 coordinates to ECEF (Earch Center Earth Fixed) coordinates. //! //! @param[in] lat WGS-84 latitude (rad). @@ -349,6 +348,7 @@ namespace DUNE } } + private: //! Compute the radius of curvature in the prime vertical (Rn). //! //! @param[in] lat WGS-84 latitude (rad). From a66003df0b051474f4ebd080a6727469a2d06fbc Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Mon, 13 Aug 2018 15:26:19 +0200 Subject: [PATCH 04/51] DUNE/Math/Matrix: Make identity constructor explicit --- src/DUNE/Math/Matrix.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/Math/Matrix.hpp b/src/DUNE/Math/Matrix.hpp index 36a1c01139..890865a18c 100644 --- a/src/DUNE/Math/Matrix.hpp +++ b/src/DUNE/Math/Matrix.hpp @@ -99,7 +99,7 @@ namespace DUNE //! Constructor. //! Construct a square identity matrix of size n //! param[in] n size of new matrix (n * n) - Matrix(size_t n); + explicit Matrix(size_t n); //! Constructor. //! Construct a diagonal matrix using the values in data From 84cdf428286e47e65e94547718661a34460155ac Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Mon, 10 Sep 2018 20:16:18 +0200 Subject: [PATCH 05/51] DUNE/Math/Matrix: Make diagonal constructor take const pointer --- src/DUNE/Math/Matrix.cpp | 2 +- src/DUNE/Math/Matrix.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/DUNE/Math/Matrix.cpp b/src/DUNE/Math/Matrix.cpp index 5efa52412e..3120330819 100644 --- a/src/DUNE/Math/Matrix.cpp +++ b/src/DUNE/Math/Matrix.cpp @@ -151,7 +151,7 @@ namespace DUNE identity(); } - Matrix::Matrix(double* diag, size_t n) + Matrix::Matrix(const double* diag, size_t n) { m_nrows = n; m_ncols = n; diff --git a/src/DUNE/Math/Matrix.hpp b/src/DUNE/Math/Matrix.hpp index 890865a18c..a43b7a1b25 100644 --- a/src/DUNE/Math/Matrix.hpp +++ b/src/DUNE/Math/Matrix.hpp @@ -105,7 +105,7 @@ namespace DUNE //! Construct a diagonal matrix using the values in data //! param[in] diag pointer to data to be copied to diagonal matrix //! param[in] n size of new matrix (n * n) - Matrix(double* diag, size_t n); + Matrix(const double* diag, size_t n); //! Destructor. //! Decrement the number of copies of a Matrix and frees the From 74daa5ef8da8e6e497cdafcaec2a25e373caa194 Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Fri, 14 Sep 2018 13:21:07 +0200 Subject: [PATCH 06/51] DUNE/Math/Matrix: Turn skew(double[]) into non-class function The way skew(double data[3]) was defined, it was impossible to call because it was a friend function that did not take it's friendly class as an argument, so ADL cannot find it. Since skew(double[]) does not need access to private members of class Matrix anyway, it can just be made into a regular free function outside of the class, there is no need for friend. --- src/DUNE/Math/Matrix.cpp | 2 +- src/DUNE/Math/Matrix.hpp | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/DUNE/Math/Matrix.cpp b/src/DUNE/Math/Matrix.cpp index 3120330819..2f41ad4b37 100644 --- a/src/DUNE/Math/Matrix.cpp +++ b/src/DUNE/Math/Matrix.cpp @@ -1938,7 +1938,7 @@ namespace DUNE } Matrix - skew(double data[3]) + skew(const double data[3]) { Matrix m(3, 3, 0.0); m(0, 1) = -data[2]; diff --git a/src/DUNE/Math/Matrix.hpp b/src/DUNE/Math/Matrix.hpp index a43b7a1b25..7a2a5fb56b 100644 --- a/src/DUNE/Math/Matrix.hpp +++ b/src/DUNE/Math/Matrix.hpp @@ -720,13 +720,6 @@ namespace DUNE friend DUNE_DLL_SYM Matrix inverse(const Matrix& a, const Matrix& b); - //! This function returns a 3x3 skew symmetrical - //! matrix using an array with 3 elements. - //! @param[in] data array with 3 elements - //! @return skewed matrix - friend Matrix - skew(double data[3]); - //! This function returns a 3x3 skew symmetrical //! matrix using a matrix (3x1 or 1x3) //! @param[in] a row or column vector with 3 elements @@ -851,6 +844,13 @@ namespace DUNE void split(void); }; + + //! This function returns a 3x3 skew symmetrical + //! matrix using an array with 3 elements. + //! @param[in] data array with 3 elements + //! @return skewed matrix + Matrix + skew(const double data[3]); } } From 380e72b9df55a79033e380c67312fc91f75bb114 Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Wed, 19 Sep 2018 15:56:39 +0200 Subject: [PATCH 07/51] DUNE/Math/Matrix: Add begin/end/cbegin/cend methods --- src/DUNE/Math/Matrix.cpp | 36 ++++++++++++++++++++++++++++++++++++ src/DUNE/Math/Matrix.hpp | 24 ++++++++++++++++++++++++ 2 files changed, 60 insertions(+) diff --git a/src/DUNE/Math/Matrix.cpp b/src/DUNE/Math/Matrix.cpp index 2f41ad4b37..3fa785585f 100644 --- a/src/DUNE/Math/Matrix.cpp +++ b/src/DUNE/Math/Matrix.cpp @@ -206,6 +206,42 @@ namespace DUNE *m_counter = 1; } + double* + Matrix::begin(void) + { + return m_data; + } + + double* + Matrix::end(void) + { + return m_data + m_size; + } + + const double* + Matrix::begin(void) const + { + return m_data; + } + + const double* + Matrix::end(void) const + { + return m_data + m_size; + } + + const double* + Matrix::cbegin(void) const + { + return m_data; + } + + const double* + Matrix::cend(void) const + { + return m_data + m_size; + } + int Matrix::rows(void) const { diff --git a/src/DUNE/Math/Matrix.hpp b/src/DUNE/Math/Matrix.hpp index 7a2a5fb56b..9f45ba2789 100644 --- a/src/DUNE/Math/Matrix.hpp +++ b/src/DUNE/Math/Matrix.hpp @@ -112,6 +112,30 @@ namespace DUNE //! allocated memory if this number reaches zero. ~Matrix(void); + //! Pointer to first element. + double* + begin(void); + + //! Pointer to element after last element. + double* + end(void); + + //! Const pointer to first element. + const double* + begin(void) const; + + //! Const pointer to element after last element. + const double* + end(void) const; + + //! Const pointer to first element. + const double* + cbegin(void) const; + + //! Const pointer to element after last element. + const double* + cend(void) const; + //! Retrieve the number of rows of the matrix. //! @return number of rows of the matrix. int From aa16a6a39882f5817cc43c9678ef268546c49832 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Thu, 11 Oct 2018 20:50:57 +0200 Subject: [PATCH 08/51] Control/UAV/Ardupilot: fixed scaling of raw IMU data --- src/Control/UAV/Ardupilot/Task.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/src/Control/UAV/Ardupilot/Task.cpp b/src/Control/UAV/Ardupilot/Task.cpp index 489da90016..d610634cf6 100644 --- a/src/Control/UAV/Ardupilot/Task.cpp +++ b/src/Control/UAV/Ardupilot/Task.cpp @@ -1768,23 +1768,28 @@ namespace Control double tstamp = Clock::getSinceEpoch(); IMC::Acceleration acce; - acce.x = raw.xacc; - acce.y = raw.yacc; - acce.z = raw.zacc; + // raw_imu acc unit is in milli gs + // g used in AP is 9.80665 (see libraries/AP_Math/definitions.h) + acce.x = raw.xacc*0.001*9.80665; + acce.y = raw.yacc*0.001*9.80665; + acce.z = raw.zacc*0.001*9.80665; acce.setTimeStamp(tstamp); dispatch(acce); + // raw_imu ars unit is milli rad/s IMC::AngularVelocity avel; - avel.x = raw.xgyro; - avel.y = raw.ygyro; - avel.z = raw.zgyro; + avel.x = raw.xgyro*0.001; + avel.y = raw.ygyro*0.001; + avel.z = raw.zgyro*0.001; avel.setTimeStamp(tstamp); dispatch(avel); + // raw_imu mag unit is milli Tesla + // IMC mag unit is Gauss = 10^-4 Tesla IMC::MagneticField magn; - magn.x = raw.xmag; - magn.y = raw.ymag; - magn.z = raw.zmag; + magn.x = raw.xmag*0.1; + magn.y = raw.ymag*0.1; + magn.z = raw.zmag*0.1; magn.setTimeStamp(tstamp); dispatch(magn); } From 5fc40d1d6c3819e654113c27360257197f3aa028 Mon Sep 17 00:00:00 2001 From: Morten Fyhn Amundsen Date: Wed, 27 Mar 2019 09:19:43 +0100 Subject: [PATCH 09/51] DUNE/IMC/MessageList: Add empty() method --- src/DUNE/IMC/MessageList.hpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/DUNE/IMC/MessageList.hpp b/src/DUNE/IMC/MessageList.hpp index bd6e587986..ae19ee3588 100644 --- a/src/DUNE/IMC/MessageList.hpp +++ b/src/DUNE/IMC/MessageList.hpp @@ -126,6 +126,14 @@ namespace DUNE return m_list.end(); } + //! Check if the list is empty. + //! @return true if the list is empty. + bool + empty(void) const + { + return m_list.empty(); + } + //! Add a new element at the end of the list, after its current //! last element. The content of this new element is initialized //! to a copy of 'msg'. From f07e41e3d2b0f156a5370a7421946e6bda62969a Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Wed, 8 May 2019 18:01:06 +0100 Subject: [PATCH 10/51] DUNE/Utils/String: Added replaceAll() method. --- src/DUNE/Utils/String.cpp | 14 ++++++++++++++ src/DUNE/Utils/String.hpp | 8 ++++++++ 2 files changed, 22 insertions(+) diff --git a/src/DUNE/Utils/String.cpp b/src/DUNE/Utils/String.cpp index 7807bcb46e..eaf48380f8 100644 --- a/src/DUNE/Utils/String.cpp +++ b/src/DUNE/Utils/String.cpp @@ -179,6 +179,20 @@ namespace DUNE return rv; } + std::string + String::replaceAll(const std::string& inStr, const std::string& searchStr, const std::string& replaceStr) + { + std::string result = inStr; + size_t pos = result.find(searchStr); + while (pos != std::string::npos) + { + result.replace(pos, searchStr.size(), replaceStr); + pos = result.find(searchStr, pos + searchStr.size()); + } + + return result; + } + void String::toLowerCase(std::string& str) { diff --git a/src/DUNE/Utils/String.hpp b/src/DUNE/Utils/String.hpp index 4117babc55..0f7e1dec6f 100644 --- a/src/DUNE/Utils/String.hpp +++ b/src/DUNE/Utils/String.hpp @@ -244,6 +244,14 @@ namespace DUNE static std::string replace(const std::string& str, char rep, const std::string& pat); + //! Replace all occurrences of a substring in a string + //! @param[in] inStr string. + //! @param[in] searchStr substring to search for. + //! @param[in] replaceStr substring replacement. + //! @return resulting string with all substring occurrences replaced. + static std::string + replaceAll(const std::string& inStr, const std::string& searchStr, const std::string& replaceStr); + static std::string escape(const std::string& input); From b021f2d07d15b6d9518c95febc1168f922c8f194 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Thu, 9 May 2019 15:54:52 +0200 Subject: [PATCH 11/51] Control/UAV/Ardupilot: fixed bug in down velocity --- src/Control/UAV/Ardupilot/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Control/UAV/Ardupilot/Task.cpp b/src/Control/UAV/Ardupilot/Task.cpp index 5d2d339d62..e5bad52720 100644 --- a/src/Control/UAV/Ardupilot/Task.cpp +++ b/src/Control/UAV/Ardupilot/Task.cpp @@ -1828,7 +1828,7 @@ namespace Control m_estate.vx = 1e-02 * gp.vx; m_estate.vy = 1e-02 * gp.vy; - m_estate.vz = -1e-02 * gp.vz; + m_estate.vz = 1e-02 * gp.vz; // Note: the following will yield body-fixed *ground* velocity // Maybe this can be fixed w/IAS readings (anyway not too important) From 2d3759d954041785ab4301d981a32273962f1a5b Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Thu, 16 May 2019 14:02:26 +0100 Subject: [PATCH 12/51] Config: development/xp1-trex: Added configuration for T-REX simulation. --- etc/development/xp1-trex.ini | 49 ++++++++++++++++++++++++++++++++++++ 1 file changed, 49 insertions(+) create mode 100644 etc/development/xp1-trex.ini diff --git a/etc/development/xp1-trex.ini b/etc/development/xp1-trex.ini new file mode 100644 index 0000000000..871baea98e --- /dev/null +++ b/etc/development/xp1-trex.ini @@ -0,0 +1,49 @@ +############################################################################ +# Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia # +# Laboratório de Sistemas e Tecnologia Subaquática (LSTS) # +############################################################################ +# This file is part of DUNE: Unified Navigation Environment. # +# # +# Commercial Licence Usage # +# Licencees holding valid commercial DUNE licences may use this file in # +# accordance with the commercial licence agreement provided with the # +# Software or, alternatively, in accordance with the terms contained in a # +# written agreement between you and Faculdade de Engenharia da # +# Universidade do Porto. For licensing terms, conditions, and further # +# information contact lsts@fe.up.pt. # +# # +# Modified European Union Public Licence - EUPL v.1.1 Usage # +# Alternatively, this file may be used under the terms of the Modified # +# EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md # +# included in the packaging of this file. You may not use this work # +# except in compliance with the Licence. Unless required by applicable # +# law or agreed to in writing, software distributed under the Licence is # +# distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF # +# ANY KIND, either express or implied. See the Licence for the specific # +# language governing permissions and limitations at # +# https://github.com/LSTS/dune/blob/master/LICENCE.md and # +# http://ec.europa.eu/idabc/eupl.html. # +############################################################################ +# Author: Jose Pinto # +############################################################################ +# LAUV Xplore 1 simulation with T-REX support # +############################################################################ + +[Require ../lauv-xplore-1.ini] + +[Require ../common/trex.ini] + +[Simulators.VSIM] +Stream Speed North = -0.3 +Stream Speed East = 0.1 + +[Simulators.Iridium] +Enabled = Simulation +Entity Label = Iridium Simulator +Server Address = ripples-web.appspot.com +Server Port = 80 +Debug Level = Debug + +[Transports.UDP] +Underwater Communications = false +Always Transmitted Messages = SimulatedState From fe021736159c0f643c74e95815dc63abca77feaa Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Thu, 16 May 2019 14:02:26 +0100 Subject: [PATCH 13/51] Config: development/xp1-trex: Added configuration for T-REX simulation. --- etc/development/xp1-trex.ini | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/etc/development/xp1-trex.ini b/etc/development/xp1-trex.ini index 871baea98e..4d68bd57b7 100644 --- a/etc/development/xp1-trex.ini +++ b/etc/development/xp1-trex.ini @@ -47,3 +47,7 @@ Debug Level = Debug [Transports.UDP] Underwater Communications = false Always Transmitted Messages = SimulatedState + +[Autonomy.TREX] +TREX ID = 65001 +Debug Level = Debug From 4685ae88e5a52c282e633e2eaa455f8620c637ab Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Thu, 16 May 2019 16:16:09 +0100 Subject: [PATCH 14/51] Config: development/xp2-trex: Added XP2 configuration for TREX simulations. --- etc/development/xp2-trex.ini | 53 ++++++++++++++++++++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100644 etc/development/xp2-trex.ini diff --git a/etc/development/xp2-trex.ini b/etc/development/xp2-trex.ini new file mode 100644 index 0000000000..10220c0e7c --- /dev/null +++ b/etc/development/xp2-trex.ini @@ -0,0 +1,53 @@ +############################################################################ +# Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia # +# Laboratório de Sistemas e Tecnologia Subaquática (LSTS) # +############################################################################ +# This file is part of DUNE: Unified Navigation Environment. # +# # +# Commercial Licence Usage # +# Licencees holding valid commercial DUNE licences may use this file in # +# accordance with the commercial licence agreement provided with the # +# Software or, alternatively, in accordance with the terms contained in a # +# written agreement between you and Faculdade de Engenharia da # +# Universidade do Porto. For licensing terms, conditions, and further # +# information contact lsts@fe.up.pt. # +# # +# Modified European Union Public Licence - EUPL v.1.1 Usage # +# Alternatively, this file may be used under the terms of the Modified # +# EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md # +# included in the packaging of this file. You may not use this work # +# except in compliance with the Licence. Unless required by applicable # +# law or agreed to in writing, software distributed under the Licence is # +# distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF # +# ANY KIND, either express or implied. See the Licence for the specific # +# language governing permissions and limitations at # +# https://github.com/LSTS/dune/blob/master/LICENCE.md and # +# http://ec.europa.eu/idabc/eupl.html. # +############################################################################ +# Author: Jose Pinto # +############################################################################ +# LAUV Xplore 2 simulation with T-REX support # +############################################################################ + +[Require ../lauv-xplore-2.ini] + +[Require ../common/trex.ini] + +[Simulators.VSIM] +Stream Speed North = -0.3 +Stream Speed East = 0.1 + +[Simulators.Iridium] +Enabled = Simulation +Entity Label = Iridium Simulator +Server Address = ripples-web.appspot.com +Server Port = 80 +Debug Level = Debug + +[Transports.UDP] +Underwater Communications = false +Always Transmitted Messages = SimulatedState + +[Autonomy.TREX] +TREX ID = 65002 +Debug Level = Debug \ No newline at end of file From ab3f10c11a45c6b361be560a208310a6960bb732 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Thu, 6 Jun 2019 19:00:26 +0200 Subject: [PATCH 15/51] Control/UAV/Ardupilot: Fix acc/angvel/mag timestamp by adding DF_KEEP_TIME --- src/Control/UAV/Ardupilot/Task.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Control/UAV/Ardupilot/Task.cpp b/src/Control/UAV/Ardupilot/Task.cpp index 5d2d339d62..7886c0a43a 100644 --- a/src/Control/UAV/Ardupilot/Task.cpp +++ b/src/Control/UAV/Ardupilot/Task.cpp @@ -1772,21 +1772,21 @@ namespace Control acce.y = raw.yacc; acce.z = raw.zacc; acce.setTimeStamp(tstamp); - dispatch(acce); + dispatch(acce, DF_KEEP_TIME); IMC::AngularVelocity avel; avel.x = raw.xgyro; avel.y = raw.ygyro; avel.z = raw.zgyro; avel.setTimeStamp(tstamp); - dispatch(avel); + dispatch(avel, DF_KEEP_TIME); IMC::MagneticField magn; magn.x = raw.xmag; magn.y = raw.ymag; magn.z = raw.zmag; magn.setTimeStamp(tstamp); - dispatch(magn); + dispatch(magn, DF_KEEP_TIME); } void From 8d106ce1e073b9bb5c76122f02ed6f14d73550ca Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Fri, 21 Jun 2019 12:46:11 +0200 Subject: [PATCH 16/51] Control/UAV/Ardupilot: Made "Seconds before Waypoint" float To enable more fine tuning --- src/Control/UAV/Ardupilot/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Control/UAV/Ardupilot/Task.cpp b/src/Control/UAV/Ardupilot/Task.cpp index e5bad52720..e2809c975c 100644 --- a/src/Control/UAV/Ardupilot/Task.cpp +++ b/src/Control/UAV/Ardupilot/Task.cpp @@ -137,7 +137,7 @@ namespace Control //! Has Power Module bool pwrm; //! WP seconds before reach - int secs; + float secs; //! WP Copter: Minimum wp switch radius float cp_wp_radius; //! RC setup From 4a563421f718570c2b6813956e483b29a2cf6116 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Pinto?= Date: Tue, 25 Jun 2019 15:50:17 +0100 Subject: [PATCH 17/51] Config: lauv-xplore-2: Changes to use BATMAN power board. --- etc/lauv-xplore-2.ini | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/etc/lauv-xplore-2.ini b/etc/lauv-xplore-2.ini index ea0666e698..529434ca81 100644 --- a/etc/lauv-xplore-2.ini +++ b/etc/lauv-xplore-2.ini @@ -1,5 +1,5 @@ ############################################################################ -# Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia # +# Copyright 2007-2017 Universidade do Porto - Faculdade de Engenharia # # Laboratório de Sistemas e Tecnologia Subaquática (LSTS) # ############################################################################ # This file is part of DUNE: Unified Navigation Environment. # @@ -32,6 +32,7 @@ [Require auv/basic.ini] [Require hardware/lctr-a6xx/basic.ini] [Require hardware/lctr-a6xx/broom-36v.ini] +[Require hardware/lctr-a6xx/batman.ini] [Require hardware/lctr-a6xx/gps-lc2m.ini] [Require hardware/lctr-a6xx/gsm-lc2m.ini] [Require hardware/lctr-a6xx/iridiumsbd-lc2m.ini] @@ -114,8 +115,8 @@ Default Monitoring -- Hardware = AHRS, [Monitors.FuelLevel] Capacity Decay Factor = 6.0 -Voltage Error Level = 24.5 -Voltage Threshold Error Level = 0.5 +Warning Level = 20 +Error Level = 15 # to be corrected [Navigation.AUV.Navigation] @@ -132,14 +133,23 @@ ADC0 - Entity Label = Chlorophyll Probe Voltage [Sensors.Microstrain3DMGX3] #APDL #Hard-Iron Calibration = 0.010155, 0.024699, 0.0 -#Sesimbra +#Sesimbra Hard-Iron Calibration = 0.005450, 0.013942, 0.0 [Simulators.Environment] Simulate - Bottom Distance = false Simulate - Forward Distance = false +[Power.BATMANv2] +Serial Port - Device = /dev/uart/9 +Scale Factor A/Ah = 2.5 +Warning Level = 20 +Error Level = 15 + [Power.PCTLv2] +Leak 0 - Entity Label = Medium Sensor +Leak 0 - Medium Sensor = true +Leak 1 - Entity Label = Leak Sensor ADC Reference Voltage = 1.083 Power Channel 3 - Name = SADC Power Channel 3 - State = 1 @@ -151,8 +161,8 @@ Power Channel 11 - Name = Auxiliary CPU Power Channel 11 - State = 1 Power Channel 12 - Name = Wet Sensor Probe Power Channel 12 - State = 0 -Power Channel 15 - Name = N/C (+12V_2) -Power Channel 15 - State = 0 +Power Channel 15 - Name = BATMAN +Power Channel 15 - State = 1 Power Channel 17 - Name = N/C (DVL) Power Channel 17 - State = 0 @@ -161,8 +171,7 @@ Enabled = Hardware Entity Label = Logger (Digest) Sample Interval = 1 Flush Interval = 5 -Transports = EntityInfo, - EstimatedState, +Transports = EstimatedState, Temperature, Salinity, Conductivity, From c35d67d1db3c33f5861f4571cb9c680638904c95 Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Mon, 23 May 2016 14:21:29 +0100 Subject: [PATCH 18/51] DUNE/Time: Using static variable for keeping simulation time multiplier across all Time utilities. --- src/DUNE/Time/Clock.cpp | 32 +++++++++++++++++++++++++++++--- src/DUNE/Time/Clock.hpp | 14 ++++++++++++++ src/DUNE/Time/Constants.hpp | 2 ++ src/DUNE/Time/Delay.cpp | 5 +++++ src/DUNE/Time/PeriodicDelay.hpp | 4 ++++ 5 files changed, 54 insertions(+), 3 deletions(-) diff --git a/src/DUNE/Time/Clock.cpp b/src/DUNE/Time/Clock.cpp index d22bc445a2..8b45515fa1 100644 --- a/src/DUNE/Time/Clock.cpp +++ b/src/DUNE/Time/Clock.cpp @@ -55,9 +55,16 @@ namespace DUNE { namespace Time { + + uint64_t Clock::s_starttime = getSinceEpochNsec(); + uint64_t Clock::getNsec(void) { + + if (s_time_multiplier != 1.0) + return getSinceEpochNsec() - s_starttime; + // POSIX RT. #if defined(DUNE_SYS_HAS_CLOCK_GETTIME) timespec ts; @@ -82,17 +89,20 @@ namespace DUNE uint64_t Clock::getSinceEpochNsec(void) { + + uint64_t time = 0; + // POSIX RT. #if defined(DUNE_SYS_HAS_CLOCK_GETTIME) timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - return (uint64_t)ts.tv_sec * c_nsec_per_sec + (uint64_t)ts.tv_nsec; + time = (uint64_t)ts.tv_sec * c_nsec_per_sec + (uint64_t)ts.tv_nsec; // POSIX. #elif defined(DUNE_SYS_HAS_GETTIMEOFDAY) timeval tv; gettimeofday(&tv, 0); - return (uint64_t)tv.tv_sec * c_nsec_per_sec + (uint64_t)tv.tv_usec * 1000; + time = (uint64_t)tv.tv_sec * c_nsec_per_sec + (uint64_t)tv.tv_usec * 1000; // Microsoft Windows. #elif defined(DUNE_SYS_HAS_GET_SYSTEM_TIME_AS_FILE_TIME) @@ -105,18 +115,26 @@ namespace DUNE // epoch (Jan. 1, 1601) and the Unix epoch (Jan. 1, 1970). tm -= 116444736000000000ULL; - return tm * 100; + time = tm * 100; // Unsupported system. #else # error Clock::getSinceEpochNsec() is not yet implemented in this system. #endif + //if (s_time_multiplier != 1.0) + // time = (time - s_starttime) * s_time_multiplier + s_starttime; + + return time; } void Clock::set(double value) { + + if (s_time_multiplier != 1.0) + return; + #if defined(DUNE_SYS_HAS_SETTIMEOFDAY) timeval tv; tv.tv_sec = static_cast(value); @@ -127,5 +145,13 @@ namespace DUNE (void)value; #endif } + + void + Clock::setTimeMultiplier(double mul) + { + s_time_multiplier = 1.0; + s_starttime = getSinceEpochNsec(); + s_time_multiplier = mul; + } } } diff --git a/src/DUNE/Time/Clock.hpp b/src/DUNE/Time/Clock.hpp index ecd347bcb4..405f2920f9 100644 --- a/src/DUNE/Time/Clock.hpp +++ b/src/DUNE/Time/Clock.hpp @@ -41,6 +41,7 @@ namespace DUNE // Export DLL Symbol. class DUNE_DLL_SYM Clock; + //! %System clock routines. class Clock { @@ -120,6 +121,19 @@ namespace DUNE //! @param value time in seconds. static void set(double value); + + //! Time multiplier for non-realtime simulations + //! @param mul multiplier (e.g. 4.0 for 4x speed) + static void + setTimeMultiplier(double mul); + + //! Return configured time multipler + //! @return simulation time multiplier (1.0 for real-time) + static double + getTimeMultiplier(); + + private: + static uint64_t s_starttime; }; } } diff --git a/src/DUNE/Time/Constants.hpp b/src/DUNE/Time/Constants.hpp index 7147f097c3..32be7253bf 100644 --- a/src/DUNE/Time/Constants.hpp +++ b/src/DUNE/Time/Constants.hpp @@ -57,6 +57,8 @@ namespace DUNE static const unsigned c_nsec_per_msec = 1000000u; //! Number of nanoseconds in a second (floating point). static const double c_nsec_per_sec_fp = 1000000000.0; + //! Time multiplier to use in simulation + static double s_time_multiplier = 1.0; } } diff --git a/src/DUNE/Time/Delay.cpp b/src/DUNE/Time/Delay.cpp index 474a6e9f0f..477e568dd6 100644 --- a/src/DUNE/Time/Delay.cpp +++ b/src/DUNE/Time/Delay.cpp @@ -31,6 +31,7 @@ #include #include #include +#include // Platform headers. #if defined(DUNE_SYS_HAS_TIME_H) @@ -48,6 +49,10 @@ namespace DUNE void Delay::waitNsec(uint64_t nsec) { + + if (s_time_multiplier != 1.0) + nsec = (uint64_t) (nsec / s_time_multiplier); + // Microsoft Windows. #if defined(DUNE_SYS_HAS_CREATE_WAITABLE_TIMER) HANDLE t = CreateWaitableTimer(0, TRUE, 0); diff --git a/src/DUNE/Time/PeriodicDelay.hpp b/src/DUNE/Time/PeriodicDelay.hpp index 94d21925b9..4cff827b12 100644 --- a/src/DUNE/Time/PeriodicDelay.hpp +++ b/src/DUNE/Time/PeriodicDelay.hpp @@ -40,6 +40,7 @@ // DUNE headers. #include +#include namespace DUNE { @@ -58,6 +59,9 @@ namespace DUNE void set(uint32_t delay_usec) { + + delay_usec = (uint32_t) (delay_usec / s_time_multiplier); + // Microsoft Windows. #if defined(DUNE_SYS_HAS_GET_SYSTEM_TIME_AS_FILE_TIME) m_delay = delay_usec * 10; From a9060b7b4f75f97ddf46b149533f287c99b1ed6d Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Mon, 23 May 2016 14:22:04 +0100 Subject: [PATCH 19/51] Simulators/VSIM: Add simulation time multiplier parameter. --- src/Simulators/VSIM/Task.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/Simulators/VSIM/Task.cpp b/src/Simulators/VSIM/Task.cpp index 7b344edf35..456250ac9c 100644 --- a/src/Simulators/VSIM/Task.cpp +++ b/src/Simulators/VSIM/Task.cpp @@ -64,6 +64,8 @@ namespace Simulators double wx; //! Stream speed East parameter (m/s). double wy; + //! Simulation time multiplier + double time_multiplier; }; //! Simulator task. @@ -100,10 +102,16 @@ namespace Simulators .defaultValue("0.0") .description("Water current speed along the East in the NED frame"); + param("Time Multiplier", m_args.time_multiplier) + .defaultValue("1.0") + .description("Simulation time multiplier"); + // Register handler routines. bind(this); bind(this); bind(this); + + Time::Clock::setTimeMultiplier(m_args.time_multiplier); } //! Release allocated resources. From 71348c419aac12b139aaf93d7e39f709e9d5db61 Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Tue, 24 May 2016 14:17:43 +0100 Subject: [PATCH 20/51] Core: DUNE/Time: Several changes to allow for faster than real-time sim. --- src/DUNE/Time/Clock.cpp | 89 +++++++++++++++++++++------------ src/DUNE/Time/Clock.hpp | 19 +++++++ src/DUNE/Time/Constants.hpp | 1 - src/DUNE/Time/Delay.cpp | 3 -- src/DUNE/Time/Delay.hpp | 3 +- src/DUNE/Time/PeriodicDelay.hpp | 2 +- 6 files changed, 78 insertions(+), 39 deletions(-) diff --git a/src/DUNE/Time/Clock.cpp b/src/DUNE/Time/Clock.cpp index 8b45515fa1..a047406541 100644 --- a/src/DUNE/Time/Clock.cpp +++ b/src/DUNE/Time/Clock.cpp @@ -55,14 +55,13 @@ namespace DUNE { namespace Time { - uint64_t Clock::s_starttime = getSinceEpochNsec(); + double Clock::s_time_multiplier = 1.0; uint64_t Clock::getNsec(void) { - - if (s_time_multiplier != 1.0) + if (Clock::s_time_multiplier != 1.0) return getSinceEpochNsec() - s_starttime; // POSIX RT. @@ -89,20 +88,64 @@ namespace DUNE uint64_t Clock::getSinceEpochNsec(void) { + uint64_t time = getSinceEpochNsecRT(); + if (Clock::s_time_multiplier != 1.0) { + double ellapsed_time = (time - s_starttime); + time = ellapsed_time * Clock::s_time_multiplier + s_starttime; + } + return time; + } + + void + Clock::set(double value) + { + if (Clock::s_time_multiplier != 1.0) { + s_starttime = value * c_nsec_per_sec; + setTimeMultiplier(Clock::s_time_multiplier); + return; + } + +#if defined(DUNE_SYS_HAS_SETTIMEOFDAY) + timeval tv; + tv.tv_sec = static_cast(value); + tv.tv_usec = 0; + if (settimeofday(&tv, 0) != 0) + throw System::Error(errno, DTR("failed to set time")); +#else + (void)value; +#endif + } - uint64_t time = 0; + void + Clock::setTimeMultiplier(double mul) + { + Clock::s_time_multiplier = 1.0; + s_starttime = getSinceEpochNsec(); + Clock::s_time_multiplier = mul; + } + double + Clock::getTimeMultiplier(void) + { + return Clock::s_time_multiplier; + } + + + + double + Clock::getSinceEpochNsecRT(void) + { // POSIX RT. #if defined(DUNE_SYS_HAS_CLOCK_GETTIME) timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - time = (uint64_t)ts.tv_sec * c_nsec_per_sec + (uint64_t)ts.tv_nsec; + return (uint64_t)ts.tv_sec * c_nsec_per_sec + (uint64_t)ts.tv_nsec; // POSIX. #elif defined(DUNE_SYS_HAS_GETTIMEOFDAY) timeval tv; gettimeofday(&tv, 0); - time = (uint64_t)tv.tv_sec * c_nsec_per_sec + (uint64_t)tv.tv_usec * 1000; + return (uint64_t)tv.tv_sec * c_nsec_per_sec + (uint64_t)tv.tv_usec * 1000; // Microsoft Windows. #elif defined(DUNE_SYS_HAS_GET_SYSTEM_TIME_AS_FILE_TIME) @@ -115,43 +158,23 @@ namespace DUNE // epoch (Jan. 1, 1601) and the Unix epoch (Jan. 1, 1970). tm -= 116444736000000000ULL; - time = tm * 100; + return tm * 100; // Unsupported system. #else # error Clock::getSinceEpochNsec() is not yet implemented in this system. #endif - //if (s_time_multiplier != 1.0) - // time = (time - s_starttime) * s_time_multiplier + s_starttime; - - return time; } - void - Clock::set(double value) + double + Clock::toSimTime(double timestamp) { + double starttime = s_starttime / c_nsec_per_sec_fp; + if (timestamp < starttime) + return timestamp; - if (s_time_multiplier != 1.0) - return; - -#if defined(DUNE_SYS_HAS_SETTIMEOFDAY) - timeval tv; - tv.tv_sec = static_cast(value); - tv.tv_usec = 0; - if (settimeofday(&tv, 0) != 0) - throw System::Error(errno, DTR("failed to set time")); -#else - (void)value; -#endif - } - - void - Clock::setTimeMultiplier(double mul) - { - s_time_multiplier = 1.0; - s_starttime = getSinceEpochNsec(); - s_time_multiplier = mul; + return ((timestamp - starttime) * Time::Clock::s_time_multiplier) + starttime; } } } diff --git a/src/DUNE/Time/Clock.hpp b/src/DUNE/Time/Clock.hpp index 405f2920f9..d9226ca0f6 100644 --- a/src/DUNE/Time/Clock.hpp +++ b/src/DUNE/Time/Clock.hpp @@ -132,8 +132,27 @@ namespace DUNE static double getTimeMultiplier(); + static double + getStartTime() + { + return s_starttime; + } + + static double + getSinceEpochNsecRT(void); + + static double + getSinceEpochRT(void) + { + return getSinceEpochNsecRT() / c_nsec_per_sec_fp; + } + + static double + toSimTime(double timestamp); + private: static uint64_t s_starttime; + static double s_time_multiplier; }; } } diff --git a/src/DUNE/Time/Constants.hpp b/src/DUNE/Time/Constants.hpp index 32be7253bf..bc3f991234 100644 --- a/src/DUNE/Time/Constants.hpp +++ b/src/DUNE/Time/Constants.hpp @@ -58,7 +58,6 @@ namespace DUNE //! Number of nanoseconds in a second (floating point). static const double c_nsec_per_sec_fp = 1000000000.0; //! Time multiplier to use in simulation - static double s_time_multiplier = 1.0; } } diff --git a/src/DUNE/Time/Delay.cpp b/src/DUNE/Time/Delay.cpp index 477e568dd6..c75704de3d 100644 --- a/src/DUNE/Time/Delay.cpp +++ b/src/DUNE/Time/Delay.cpp @@ -50,9 +50,6 @@ namespace DUNE Delay::waitNsec(uint64_t nsec) { - if (s_time_multiplier != 1.0) - nsec = (uint64_t) (nsec / s_time_multiplier); - // Microsoft Windows. #if defined(DUNE_SYS_HAS_CREATE_WAITABLE_TIMER) HANDLE t = CreateWaitableTimer(0, TRUE, 0); diff --git a/src/DUNE/Time/Delay.hpp b/src/DUNE/Time/Delay.hpp index 3c87ab8db9..fed61325b4 100644 --- a/src/DUNE/Time/Delay.hpp +++ b/src/DUNE/Time/Delay.hpp @@ -33,7 +33,7 @@ // DUNE headers. #include #include - +#include namespace DUNE { namespace Time @@ -78,6 +78,7 @@ namespace DUNE uint64_t secs = (uint64_t)s; uint64_t nsecs = secs * c_nsec_per_sec + (uint64_t)((s - secs) * c_nsec_per_sec_fp); + nsecs /= Time::Clock::getTimeMultiplier(); waitNsec(nsecs); } }; diff --git a/src/DUNE/Time/PeriodicDelay.hpp b/src/DUNE/Time/PeriodicDelay.hpp index 4cff827b12..ce3cb06db4 100644 --- a/src/DUNE/Time/PeriodicDelay.hpp +++ b/src/DUNE/Time/PeriodicDelay.hpp @@ -60,7 +60,7 @@ namespace DUNE set(uint32_t delay_usec) { - delay_usec = (uint32_t) (delay_usec / s_time_multiplier); + delay_usec = (uint32_t) (delay_usec / Clock::getTimeMultiplier()); // Microsoft Windows. #if defined(DUNE_SYS_HAS_GET_SYSTEM_TIME_AS_FILE_TIME) From eae279808a4d31ff86755bdea3c05f5db5461592 Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Tue, 24 May 2016 14:18:20 +0100 Subject: [PATCH 21/51] Core: DUNE/IMC/Message: setTimestamp() uses real-time time. --- src/DUNE/Concurrency/Condition.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/DUNE/Concurrency/Condition.cpp b/src/DUNE/Concurrency/Condition.cpp index 5be3012d6a..7bdaca8850 100644 --- a/src/DUNE/Concurrency/Condition.cpp +++ b/src/DUNE/Concurrency/Condition.cpp @@ -96,7 +96,13 @@ namespace DUNE if (t > 0) { - t += m_clock_monotonic ? Time::Clock::get() : Time::Clock::getSinceEpoch(); + if (Time::Clock::getTimeMultiplier() != 1.0) + { + t /= Time::Clock::getTimeMultiplier(); + t += Time::Clock::getSinceEpochNsecRT() / Time::c_nsec_per_sec_fp; + } + else + t += m_clock_monotonic ? Time::Clock::get() : Time::Clock::getSinceEpoch(); timespec ts = DUNE_TIMESPEC_INIT_SEC_FP(t); rv = pthread_cond_timedwait(&m_cond, &m_mutex, &ts); From c1da0cd6a8a4c05ab16bf2d140fc02544a36598c Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Tue, 24 May 2016 14:19:05 +0100 Subject: [PATCH 22/51] Core: DUNE/IMC/Message: setTimestmap() uses real-time timestamps. --- src/DUNE/IMC/Message.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/IMC/Message.hpp b/src/DUNE/IMC/Message.hpp index 567ed46017..3f774e3d22 100644 --- a/src/DUNE/IMC/Message.hpp +++ b/src/DUNE/IMC/Message.hpp @@ -107,7 +107,7 @@ namespace DUNE double setTimeStamp(void) { - return setTimeStamp(Time::Clock::getSinceEpoch()); + return setTimeStamp(Time::Clock::getSinceEpochRT()); } //! Retrieve the message's time stamp. From 84d249954bfbc3d01f81ea13ac01e447cc03bed5 Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Tue, 24 May 2016 14:19:52 +0100 Subject: [PATCH 23/51] Config: development/np2-fast: Faster-than-real-time AUV configuration. --- etc/development/np2-fast.ini | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) create mode 100644 etc/development/np2-fast.ini diff --git a/etc/development/np2-fast.ini b/etc/development/np2-fast.ini new file mode 100644 index 0000000000..28164eed09 --- /dev/null +++ b/etc/development/np2-fast.ini @@ -0,0 +1,36 @@ +############################################################################ +# Copyright 2007-2016 Universidade do Porto - Faculdade de Engenharia # +# Laboratório de Sistemas e Tecnologia Subaquática (LSTS) # +############################################################################ +# This file is part of DUNE: Unified Navigation Environment. # +# # +# Commercial Licence Usage # +# Licencees holding valid commercial DUNE licences may use this file in # +# accordance with the commercial licence agreement provided with the # +# Software or, alternatively, in accordance with the terms contained in a # +# written agreement between you and Universidade do Porto. For licensing # +# terms, conditions, and further information contact lsts@fe.up.pt. # +# # +# European Union Public Licence - EUPL v.1.1 Usage # +# Alternatively, this file may be used under the terms of the EUPL, # +# Version 1.1 only (the "Licence"), appearing in the file LICENCE.md # +# included in the packaging of this file. You may not use this work # +# except in compliance with the Licence. Unless required by applicable # +# law or agreed to in writing, software distributed under the Licence is # +# distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF # +# ANY KIND, either express or implied. See the Licence for the specific # +# language governing permissions and limitations at # +# http://ec.europa.eu/idabc/eupl.html. # +############################################################################ +# Author: Jose Pinto # +############################################################################ +# LAUV Noptilus-2 25x faster than real time. # +############################################################################ + +[Require ../lauv-noptilus-2.ini] + +[Simulators.VSIM] +Time Multiplier = 25 + +[Monitors.Entities] +Default Monitoring = \ No newline at end of file From eb75d9c182fc4d053069d962a6ae2897bab81964 Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Fri, 5 Jul 2019 14:17:55 +0100 Subject: [PATCH 24/51] Transports/Iridium: Use 'iridium' source for received text messages. --- src/Transports/Iridium/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Transports/Iridium/Task.cpp b/src/Transports/Iridium/Task.cpp index ef3c9fcb75..fa0d1c0799 100644 --- a/src/Transports/Iridium/Task.cpp +++ b/src/Transports/Iridium/Task.cpp @@ -227,7 +227,7 @@ namespace Transports std::string text(msg->data.begin(), msg->data.end()); IMC::TextMessage tm; tm.text = text; - tm.origin = "Iridium (text)"; + tm.origin = "iridium"; std::stringstream ss; tm.toText(ss); spew("sending this message to bus: %s", ss.str().c_str()); From 63f2222c8582499ef164e4b075f1586e17cddb8a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jos=C3=A9=20Pinto?= Date: Fri, 5 Jul 2019 15:00:53 +0100 Subject: [PATCH 25/51] Transports/CommManager: Reply to iridium texts as plain text. --- src/Transports/CommManager/Task.cpp | 28 +++++++++++++++++++++------- 1 file changed, 21 insertions(+), 7 deletions(-) diff --git a/src/Transports/CommManager/Task.cpp b/src/Transports/CommManager/Task.cpp index 7100008112..be5dd5f9f8 100644 --- a/src/Transports/CommManager/Task.cpp +++ b/src/Transports/CommManager/Task.cpp @@ -45,6 +45,8 @@ namespace Transports { //! Period, in seconds, between state report transmissions over iridium int iridium_period; + //! Send Iridium text messages as plain text + bool iridium_plain_texts; }; //! Config section from where to fetch emergency sms number @@ -82,6 +84,10 @@ namespace Transports .description("Period, in seconds, between transmission of states via Iridium. Value of 0 disables transmission.") .defaultValue("300"); + param("Send Iridium plain texts", m_args.iridium_plain_texts) + .description("Send Iridium text messages as plain text (and not IMC)") + .defaultValue("1"); + bind(this); bind(this); bind(this); @@ -181,13 +187,21 @@ namespace Transports } else // text mode { - IMC::IridiumCommand m; - m.destination = 0xFFFF; - m.source = getSystemId(); - m.command = msg->txt_data; - uint8_t buffer[65535]; - int len = m.serialize(buffer); - tx.data.assign(buffer, buffer + len); + if (m_args.iridium_plain_texts) + { + const char* txt = msg->txt_data.c_str(); + tx.data.assign(txt, txt + msg->txt_data.length()); + } + else + { + IMC::IridiumCommand m; + m.destination = 0xFFFF; + m.source = getSystemId(); + m.command = msg->txt_data; + uint8_t buffer[65535]; + int len = m.serialize(buffer); + tx.data.assign(buffer, buffer + len); + } } m_transmission_requests[tx.req_id] = msg->clone(); dispatch(tx); From 3bd4ca9d0bb6d7812098c1243e05f8c080e64bde Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Mon, 8 Jul 2019 19:18:48 +0100 Subject: [PATCH 26/51] DUNE/Time: Changes to allow the use of monotonic clock --- src/DUNE/Time/Clock.cpp | 30 +++++++++++++-------- src/DUNE/Time/Clock.hpp | 60 ++++++++++++++++++++++++++++++++--------- 2 files changed, 66 insertions(+), 24 deletions(-) diff --git a/src/DUNE/Time/Clock.cpp b/src/DUNE/Time/Clock.cpp index a047406541..d715382097 100644 --- a/src/DUNE/Time/Clock.cpp +++ b/src/DUNE/Time/Clock.cpp @@ -55,15 +55,24 @@ namespace DUNE { namespace Time { - uint64_t Clock::s_starttime = getSinceEpochNsec(); + uint64_t Clock::s_starttime_epoch = getSinceEpochNsecRT(); + uint64_t Clock::s_starttime_mono = getNsecRT(); double Clock::s_time_multiplier = 1.0; uint64_t Clock::getNsec(void) { - if (Clock::s_time_multiplier != 1.0) - return getSinceEpochNsec() - s_starttime; + uint64_t time = getNsecRT(); + if (Clock::s_time_multiplier != 1.0) { + double ellapsed_time = (time - s_starttime_mono); + time = ellapsed_time * Clock::s_time_multiplier + s_starttime_mono; + } + return time; + } + uint64_t + Clock::getNsecRT(void) + { // POSIX RT. #if defined(DUNE_SYS_HAS_CLOCK_GETTIME) timespec ts; @@ -90,8 +99,8 @@ namespace DUNE { uint64_t time = getSinceEpochNsecRT(); if (Clock::s_time_multiplier != 1.0) { - double ellapsed_time = (time - s_starttime); - time = ellapsed_time * Clock::s_time_multiplier + s_starttime; + double ellapsed_time = (time - s_starttime_epoch); + time = ellapsed_time * Clock::s_time_multiplier + s_starttime_epoch; } return time; } @@ -100,7 +109,7 @@ namespace DUNE Clock::set(double value) { if (Clock::s_time_multiplier != 1.0) { - s_starttime = value * c_nsec_per_sec; + s_starttime_epoch = value * c_nsec_per_sec; setTimeMultiplier(Clock::s_time_multiplier); return; } @@ -120,7 +129,8 @@ namespace DUNE Clock::setTimeMultiplier(double mul) { Clock::s_time_multiplier = 1.0; - s_starttime = getSinceEpochNsec(); + s_starttime_epoch = getSinceEpochNsecRT(); + s_starttime_mono = getNsecRT(); Clock::s_time_multiplier = mul; } @@ -130,9 +140,7 @@ namespace DUNE return Clock::s_time_multiplier; } - - - double + uint64_t Clock::getSinceEpochNsecRT(void) { // POSIX RT. @@ -170,7 +178,7 @@ namespace DUNE double Clock::toSimTime(double timestamp) { - double starttime = s_starttime / c_nsec_per_sec_fp; + double starttime = s_starttime_epoch / c_nsec_per_sec_fp; if (timestamp < starttime) return timestamp; diff --git a/src/DUNE/Time/Clock.hpp b/src/DUNE/Time/Clock.hpp index d9226ca0f6..2f9c7ac005 100644 --- a/src/DUNE/Time/Clock.hpp +++ b/src/DUNE/Time/Clock.hpp @@ -46,14 +46,14 @@ namespace DUNE class Clock { public: - //! Get the amount of time (in nanoseconds) since an unspecified + //! Get the amount of non-realtime (in nanoseconds) since an unspecified //! point in the past. If the system permits, this point does //! not change after system start-up time. //! @return time in nanoseconds. static uint64_t getNsec(void); - //! Get the amount of time (in microseconds) since an unspecified + //! Get the amount of non-realtime (in microseconds) since an unspecified //! point in the past. If the system permits, this point does //! not change after system start-up time. //! @return time in microseconds. @@ -63,7 +63,7 @@ namespace DUNE return getNsec() / c_nsec_per_usec; } - //! Get the amount of time (in milliseconds) since an unspecified + //! Get the amount of non-realtime (in milliseconds) since an unspecified //! point in the past. If the system permits, this point does //! not change after system start-up time. //! @return time in milliseconds. @@ -73,7 +73,7 @@ namespace DUNE return getNsec() / c_nsec_per_msec; } - //! Get the amount of time (in seconds) since an unspecified + //! Get the amount of non-realtime (in seconds) since an unspecified //! point in the past. If the system permits, this point does //! not change after system start-up time. //! @return time in seconds. @@ -83,13 +83,13 @@ namespace DUNE return getNsec() / c_nsec_per_sec_fp; } - //! Get the amount of time (in nanoseconds) elapsed since the + //! Get the amount of non-realtime (in nanoseconds) elapsed since the //! UNIX Epoch (Midnight UTC of January 1, 1970). //! @return time in nanoseconds. static uint64_t getSinceEpochNsec(void); - //! Get the amount of time (in microseconds) elapsed since the + //! Get the amount of non-realtime (in microseconds) elapsed since the //! UNIX Epoch (Midnight UTC of January 1, 1970). //! @return time in microseconds. static uint64_t @@ -98,7 +98,7 @@ namespace DUNE return getSinceEpochNsec() / c_nsec_per_usec; } - //! Get the amount of time (in milliseconds) elapsed since the + //! Get the amount of non-realtime (in milliseconds) elapsed since the //! UNIX Epoch (Midnight UTC of January 1, 1970). //! @return time in milliseconds. static uint64_t @@ -107,7 +107,7 @@ namespace DUNE return getSinceEpochNsec() / c_nsec_per_msec; } - //! Get the amount of time (in seconds) elapsed since the + //! Get the amount of non-realtime (in seconds) elapsed since the //! UNIX Epoch (Midnight UTC of January 1, 1970). //! @return time in seconds. static double @@ -132,29 +132,63 @@ namespace DUNE static double getTimeMultiplier(); + //! Return reference time used for epoch clock acceleration + //! @return s_starttime_epoch epoch time reference static double - getStartTime() + getStartTimeEpoch() { - return s_starttime; + return s_starttime_epoch; } - + + //! Return reference time used for monotonic clock acceleration + //! @return s_starttime_epoch epoch time reference static double + getStartTimeMono() + { + return s_starttime_mono; + } + + //! Get the amount of realtime (in nanoseconds) elapsed since the + //! UNIX Epoch (Midnight UTC of January 1, 1970). + //! @return time in nanoseconds. + static uint64_t getSinceEpochNsecRT(void); + //! Get the amount of realtime (in nanoseconds) since an unspecified + //! point in the past. If the system permits, this point does + //! not change after system start-up time. + //! @return time in nanoseconds. + static uint64_t + getNsecRT(void); + + //! Get the amount of realtime (in seconds) elapsed since the + //! UNIX Epoch (Midnight UTC of January 1, 1970). + //! @return time in seconds. static double getSinceEpochRT(void) { return getSinceEpochNsecRT() / c_nsec_per_sec_fp; } + //! Get the amount of realtime (in seconds) since an unspecified + //! point in the past. If the system permits, this point does + //! not change after system start-up time. + //! @return time in seconds. + static double + getRT(void) + { + return getNsecRT() / c_nsec_per_sec_fp; + } + static double toSimTime(double timestamp); private: - static uint64_t s_starttime; + static uint64_t s_starttime_epoch; + static uint64_t s_starttime_mono; static double s_time_multiplier; }; } } -#endif +#endif \ No newline at end of file From cf762887431b856e27cdef85d13c895a8e808af3 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Mon, 8 Jul 2019 19:21:51 +0100 Subject: [PATCH 27/51] DUNE/Concurrency: Allow use of monotonic clock in non-real time --- src/DUNE/Concurrency/Condition.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/Concurrency/Condition.cpp b/src/DUNE/Concurrency/Condition.cpp index 7bdaca8850..6fb3318d89 100644 --- a/src/DUNE/Concurrency/Condition.cpp +++ b/src/DUNE/Concurrency/Condition.cpp @@ -99,7 +99,7 @@ namespace DUNE if (Time::Clock::getTimeMultiplier() != 1.0) { t /= Time::Clock::getTimeMultiplier(); - t += Time::Clock::getSinceEpochNsecRT() / Time::c_nsec_per_sec_fp; + t += m_clock_monotonic ? Time::Clock::getRT() : Time::Clock::getSinceEpochRT(); } else t += m_clock_monotonic ? Time::Clock::get() : Time::Clock::getSinceEpoch(); From d580480e1f5ec418b0343dd1fb9145d53d9fb4a0 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Mon, 8 Jul 2019 19:23:31 +0100 Subject: [PATCH 28/51] Simulators/VSIM: Added warning when in non-real time --- src/Simulators/VSIM/Task.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/Simulators/VSIM/Task.cpp b/src/Simulators/VSIM/Task.cpp index 456250ac9c..93d1be007a 100644 --- a/src/Simulators/VSIM/Task.cpp +++ b/src/Simulators/VSIM/Task.cpp @@ -110,8 +110,16 @@ namespace Simulators bind(this); bind(this); bind(this); + } - Time::Clock::setTimeMultiplier(m_args.time_multiplier); + void + onUpdateParameters(void) + { + if (m_args.time_multiplier != 1.0) + { + Time::Clock::setTimeMultiplier(m_args.time_multiplier); + war("Using time multiplier: x%.2f", Time::Clock::getTimeMultiplier()); + } } //! Release allocated resources. From 943c3b3fcbb8a4762a4d9e993807042abc53bdb9 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Mon, 8 Jul 2019 19:24:34 +0100 Subject: [PATCH 29/51] Transports/Replay: Added option for faster than real-time replay. --- src/Transports/Replay/Task.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/Transports/Replay/Task.cpp b/src/Transports/Replay/Task.cpp index 3f93836001..562c469c58 100644 --- a/src/Transports/Replay/Task.cpp +++ b/src/Transports/Replay/Task.cpp @@ -47,6 +47,7 @@ namespace Transports std::string startup_file; std::vector msgs; std::vector ents; + double time_multiplier; }; static const int c_stats_period = 10; @@ -110,6 +111,10 @@ namespace Transports .defaultValue("") .description("Entities for which state should be reported"); + param("Time Multiplier", m_args.time_multiplier) + .defaultValue("1.0") + .description("Time multiplier for fast replay."); + bind(this); } @@ -123,6 +128,12 @@ namespace Transports bind(this); reset(); + + if (m_args.time_multiplier != 1.0) + { + Time::Clock::setTimeMultiplier(m_args.time_multiplier); + war("Using time multiplier: x%.2f", Time::Clock::getTimeMultiplier()); + } } ~Task(void) @@ -329,6 +340,7 @@ namespace Transports void onMain(void) { + if (!m_args.startup_file.empty()) startReplay(m_args.startup_file); From faabd4eee952b7a41f45ebb830d7a1fe33f5ad80 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Thu, 11 Jul 2019 11:01:51 +0100 Subject: [PATCH 30/51] auv/simulator: Added time multiplier option --- etc/auv/simulator.ini | 1 + 1 file changed, 1 insertion(+) diff --git a/etc/auv/simulator.ini b/etc/auv/simulator.ini index f8537d17e7..37d602d6bf 100644 --- a/etc/auv/simulator.ini +++ b/etc/auv/simulator.ini @@ -39,6 +39,7 @@ Execution Frequency = 100 Entity Label = Simulation Engine Stream Speed North = 0.0 Stream Speed East = 0.0 +Time Multiplier = 1.0 [Simulators.GPS] Enabled = Simulation From 37049d479052615ce6fd7a97a960501e2d2fc2e2 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Thu, 11 Jul 2019 11:16:31 +0100 Subject: [PATCH 31/51] testing/replays: Added time multiplier option to all replay configuration files --- etc/testing/replays/assist.ini | 1 + etc/testing/replays/btrack-replay.ini | 1 + etc/testing/replays/control-replay.ini | 1 + etc/testing/replays/fuel-replay.ini | 1 + etc/testing/replays/monitors-entities-replay.ini | 1 + etc/testing/replays/servomonitor-replay.ini | 1 + etc/testing/replays/sgnav-replay.ini | 1 + 7 files changed, 7 insertions(+) diff --git a/etc/testing/replays/assist.ini b/etc/testing/replays/assist.ini index f0e9e68a1d..5856af1c13 100644 --- a/etc/testing/replays/assist.ini +++ b/etc/testing/replays/assist.ini @@ -42,6 +42,7 @@ Vehicle = lauv-noptilus-1 [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider/report on if necessary Entities = Batteries, Power Supply diff --git a/etc/testing/replays/btrack-replay.ini b/etc/testing/replays/btrack-replay.ini index 1205f3285f..bee9e1e6a5 100644 --- a/etc/testing/replays/btrack-replay.ini +++ b/etc/testing/replays/btrack-replay.ini @@ -87,6 +87,7 @@ Log PID Parcels = true [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider/report on if necessary Entities = Echo Sounder diff --git a/etc/testing/replays/control-replay.ini b/etc/testing/replays/control-replay.ini index 268b2992f0..b50ed4ea78 100644 --- a/etc/testing/replays/control-replay.ini +++ b/etc/testing/replays/control-replay.ini @@ -92,6 +92,7 @@ Maximum Increase = 50 [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider/report on if necessary Entities = SCRT diff --git a/etc/testing/replays/fuel-replay.ini b/etc/testing/replays/fuel-replay.ini index eb6b94f8b5..e654dfb31b 100644 --- a/etc/testing/replays/fuel-replay.ini +++ b/etc/testing/replays/fuel-replay.ini @@ -42,6 +42,7 @@ Vehicle = lauv-seacon-2 [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider/report on if necessary Entities = Batteries, Power Supply diff --git a/etc/testing/replays/monitors-entities-replay.ini b/etc/testing/replays/monitors-entities-replay.ini index 5bf9c83b70..b8bf564ff3 100644 --- a/etc/testing/replays/monitors-entities-replay.ini +++ b/etc/testing/replays/monitors-entities-replay.ini @@ -41,6 +41,7 @@ Vehicle = lauv-seacon-1 [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider/report on if necessary Entities = DVL diff --git a/etc/testing/replays/servomonitor-replay.ini b/etc/testing/replays/servomonitor-replay.ini index e98678ef76..4ff1ab1a2a 100644 --- a/etc/testing/replays/servomonitor-replay.ini +++ b/etc/testing/replays/servomonitor-replay.ini @@ -67,6 +67,7 @@ Entity Label - Current 3 = Servo Controller 3 [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider/report on if necessary Entities = Allocator, diff --git a/etc/testing/replays/sgnav-replay.ini b/etc/testing/replays/sgnav-replay.ini index 829f05d708..5f614463c4 100644 --- a/etc/testing/replays/sgnav-replay.ini +++ b/etc/testing/replays/sgnav-replay.ini @@ -47,6 +47,7 @@ Distance Between GPS and CG = 0.25 [Transports.Replay] Enabled = Always Entity Label = Replay +Time Multiplier = 1.0 # NOTE: Add entity names that replay should consider Entities = Depth Sensor, AHRS, From e4effc6d6129d3b6599509a37cae513fa768b401 Mon Sep 17 00:00:00 2001 From: Jose Pinto Date: Thu, 11 Jul 2019 11:25:16 +0100 Subject: [PATCH 32/51] Config: development/xp*-soi: Use iridium simulator running on localhost. --- etc/development/xp1-soi.ini | 4 ++-- etc/development/xp2-soi.ini | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/etc/development/xp1-soi.ini b/etc/development/xp1-soi.ini index 4a47f3b374..67b2ec5b14 100644 --- a/etc/development/xp1-soi.ini +++ b/etc/development/xp1-soi.ini @@ -40,8 +40,8 @@ Stream Speed East = 0.1 [Simulators.Iridium] Enabled = Simulation Entity Label = Iridium Simulator -Server Address = ripples-web.appspot.com -Server Port = 80 +Server Address = localhost +Server Port = 9090 Debug Level = Debug [Transports.UDP] diff --git a/etc/development/xp2-soi.ini b/etc/development/xp2-soi.ini index 1ddfed6801..5590438acb 100644 --- a/etc/development/xp2-soi.ini +++ b/etc/development/xp2-soi.ini @@ -40,8 +40,8 @@ Stream Speed East = 0.1 [Simulators.Iridium] Enabled = Simulation Entity Label = Iridium Simulator -Server Address = ripples-web.appspot.com -Server Port = 80 +Server Address = localhost +Server Port = 9090 Debug Level = Debug [Transports.TCP.Server/BackSeat] From b5d5121ccddc4f8891cf419d44f6afc15d596ab9 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Wed, 17 Jul 2019 17:06:46 +0100 Subject: [PATCH 33/51] CMake: Added option to build using elliptical coordinates in the (WGS84) displace function --- CMakeLists.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 52da192b3a..390344d6f2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -90,6 +90,7 @@ dune_option(BLUEVIEW "Enable support for the BlueView SDK") dune_option(OPENCV "Enable support for OpenCV") dune_option(NO_RTTI "Disable support for RTTI") dune_option(UEYE "Enable support for IDS uEye cameras") +dune_option(ELLIPSOIDAL "Enable ellipsoidal coordinates in (WGS84) displace") # Internationalization. include(${PROJECT_SOURCE_DIR}/cmake/I18N.cmake) @@ -231,6 +232,11 @@ endif(DUNE_SHARED) add_dependencies(dune-core dune-version) +# WGS84 Coordinate Type +if(ELLIPSOIDAL) + add_definitions(-DDUNE_ELLIPSOIDAL_DISPLACE) +endif(ELLIPSOIDAL) + ########################################################################## # Tasks # ########################################################################## From a32d06fbd0e6c689222d7b019bb34afe4c273d70 Mon Sep 17 00:00:00 2001 From: Luis Venancio Date: Wed, 17 Jul 2019 17:07:18 +0100 Subject: [PATCH 34/51] DUNE/Coordinates/WGS84: Added optional elliptical coordinates to displace function --- src/DUNE/Coordinates/WGS84.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/DUNE/Coordinates/WGS84.hpp b/src/DUNE/Coordinates/WGS84.hpp index 7c9d8228a7..2f9edfb6f7 100644 --- a/src/DUNE/Coordinates/WGS84.hpp +++ b/src/DUNE/Coordinates/WGS84.hpp @@ -182,9 +182,15 @@ namespace DUNE toECEF(*lat, *lon, *hae, &x, &y, &z); // Compute Geocentric latitude - double N = computeRn(*lat); double p = std::sqrt(x * x + y * y); +#if defined(DUNE_ELLIPSOIDAL_DISPLACE) + // Use elliptical coordinates + double N = computeRn(*lat); double phi = std::atan2(z,p*(1 - c_wgs84_e2 * N / (N + *hae))); +#else + // Use spherical coordinates + double phi = std::atan2(z,p); +#endif // Compute all needed sine and cosine terms for conversion. double slon = std::sin(*lon); From ac0ede924c0ec0fbc2110aa62f0d9cd5ca0c8b79 Mon Sep 17 00:00:00 2001 From: mcpca Date: Thu, 18 Jul 2019 14:04:08 +0100 Subject: [PATCH 35/51] DUNE/Parsers: Moved HDF5Reader to core. --- src/DUNE/Parsers.hpp | 1 + src/DUNE/Parsers/HDF5Reader.cpp | 211 +++++++++++++++++ .../Parsers}/HDF5Reader.hpp | 100 ++++---- src/Simulators/StreamSpeed/HDF5Reader.cpp | 218 ------------------ .../StreamSpeed/ModelDataStreamGenerator.hpp | 5 +- 5 files changed, 263 insertions(+), 272 deletions(-) create mode 100644 src/DUNE/Parsers/HDF5Reader.cpp rename src/{Simulators/StreamSpeed => DUNE/Parsers}/HDF5Reader.hpp (54%) delete mode 100644 src/Simulators/StreamSpeed/HDF5Reader.cpp diff --git a/src/DUNE/Parsers.hpp b/src/DUNE/Parsers.hpp index 218451a7c6..bc4e10d164 100644 --- a/src/DUNE/Parsers.hpp +++ b/src/DUNE/Parsers.hpp @@ -46,5 +46,6 @@ namespace DUNE #include #include #include +#include #endif diff --git a/src/DUNE/Parsers/HDF5Reader.cpp b/src/DUNE/Parsers/HDF5Reader.cpp new file mode 100644 index 0000000000..019a35586b --- /dev/null +++ b/src/DUNE/Parsers/HDF5Reader.cpp @@ -0,0 +1,211 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Miguel Aguiar * +//*************************************************************************** + +#include +#include + +#include "DUNE/I18N.hpp" + +// This component depends on the h5cpp library +// (https://github.com/ess-dmsc/h5cpp.com). +// To enable support for h5cpp, set the H5CPP flag to 1 when calling CMake. +// If support for h5cpp is not enabled, the compilation will not fail, but +// the constructor of HDF5Reader will throw. +#if DUNE_H5CPP_ENABLED +# include "h5cpp/hdf5.hpp" +#else +// Define hdf5::file::File to avoid compilation errors due to incomplete type +namespace hdf5 +{ + namespace file + { + class File + {}; + } // namespace file +} // namespace hdf5 +#endif + +#include "HDF5Reader.hpp" + +namespace DUNE +{ + namespace Parsers + { + HDF5Reader::HDF5Reader(std::string const& filename) + : m_file(std::make_unique()) + { +#ifndef DUNE_H5CPP_ENABLED + throw std::runtime_error( + DTR("HDF5Reader::HDF5Reader(): support for HDF5 i/o is not " + "enabled.")); +#else + try + { + *m_file = hdf5::file::open(filename, hdf5::file::AccessFlags::READONLY); + + if (m_file == nullptr) + throw; + } + catch (...) + { + throw std::runtime_error( + DTR("HDF5Reader::HDF5Reader(): unable to open file.")); + } +#endif + } + + HDF5Reader::~HDF5Reader() = default; + +#ifdef DUNE_H5CPP_ENABLED + inline bool + dsetExists(File const* f, std::string const& path) + { + return f->root().has_dataset(path); + return false; + } +#endif + + bool + HDF5Reader::datasetExists(std::string const& path) const + { +#ifdef DUNE_H5CPP_ENABLED + return dsetExists(m_file.get(), path); +#else + return false; +#endif + } + + template + HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const& path) const + { +#ifdef DUNE_H5CPP_ENABLED + if (!dsetExists(m_file.get(), path)) + throw std::runtime_error( + DTR("HDF5Reader::getDataset(): The requested dataset does not " + "exist.")); + + auto dset = m_file->root().get_dataset(path); + auto dimensions = + hdf5::dataspace::Simple(dset.dataspace()).current_dimensions(); + + // Total number of gridpoints is the product of the sizes of all + // dimensions. + size_t size = std::accumulate( + std::begin(dimensions), std::end(dimensions), 1, std::multiplies<>()); + + std::vector data(size); + dset.read(data); + + // Convert elements of dimensions to size_t + std::vector dimensions_(dimensions.size()); + + std::copy(std::begin(dimensions), + std::end(dimensions), + std::begin(dimensions_)); + + return {dimensions_, data}; +#else + return {}; +#endif + } + + template + std::vector + HDF5Reader::getAttribute(std::string const& path, + std::string const& attribute) const + { +#ifdef DUNE_H5CPP_ENABLED + auto node = hdf5::node::get_node(m_file->root(), path); + auto attributes = node.attributes; + + if (!attributes.exists(attribute)) + throw std::runtime_error( + DTR("HDF5Reader::getAttribute(): requested attribute does not " + "exist.")); + + auto attr_handle = attributes[attribute]; + + std::vector data(attr_handle.dataspace().size()); + + attr_handle.read(data); + + return data; +#else + return {}; +#endif + } + + // Template specialization declarations for getDataset + template HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const&) const; + + template HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const&) const; + + template HDF5Reader::HDF5Dataset + HDF5Reader::getDataset(std::string const&) const; + + // Template specialization declarations for getAttribute + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + + template std::vector + HDF5Reader::getAttribute(std::string const&, + std::string const&) const; + } // namespace Parsers +} // namespace DUNE diff --git a/src/Simulators/StreamSpeed/HDF5Reader.hpp b/src/DUNE/Parsers/HDF5Reader.hpp similarity index 54% rename from src/Simulators/StreamSpeed/HDF5Reader.hpp rename to src/DUNE/Parsers/HDF5Reader.hpp index 17cdd9fc72..fd58da7ace 100644 --- a/src/Simulators/StreamSpeed/HDF5Reader.hpp +++ b/src/DUNE/Parsers/HDF5Reader.hpp @@ -27,8 +27,8 @@ // Author: Miguel Aguiar * //*************************************************************************** -#ifndef SIMULATORS_STREAM_SPEED_HDF5_READER_HPP_INCLUDED_ -#define SIMULATORS_STREAM_SPEED_HDF5_READER_HPP_INCLUDED_ +#ifndef DUNE_PARSERS_HDF5_READER_HPP_INCLUDED_ +#define DUNE_PARSERS_HDF5_READER_HPP_INCLUDED_ #include #include @@ -43,64 +43,60 @@ namespace hdf5 } } // namespace hdf5 -namespace Simulators +namespace DUNE { - namespace StreamSpeed + namespace Parsers { - namespace StreamGenerator - { - using hdf5::file::File; + using hdf5::file::File; - //! Simplifies reading data and attributes from HDF5 format files. - class HDF5Reader - { - public: - //! Constructor. - //! @param[in] filename path to an hdf5 file. - HDF5Reader(std::string const& filename); + //! Simplifies reading data and attributes from HDF5 format files. + class HDF5Reader + { + public: + //! Constructor. + //! @param[in] filename path to an hdf5 file. + HDF5Reader(std::string const& filename); - //! Destructor. - ~HDF5Reader(); + //! Destructor. + ~HDF5Reader(); - //! Structure holding an arbitrary multidimensional HDF5 dataset. - template - struct HDF5Dataset - { - //! Number of points in each dimension. - std::vector dimensions; - //! The data values in row-major (C-style) order. - std::vector data; - }; + //! Structure holding an arbitrary multidimensional HDF5 dataset. + template + struct HDF5Dataset + { + //! Number of points in each dimension. + std::vector dimensions; + //! The data values in row-major (C-style) order. + std::vector data; + }; - //! Check if a dataset exists in the given file. - //! @param[in] path path to the dataset in the file. - //! @return whether the dataset exists in the file. - bool - datasetExists(std::string const& path) const; + //! Check if a dataset exists in the given file. + //! @param[in] path path to the dataset in the file. + //! @return whether the dataset exists in the file. + bool + datasetExists(std::string const& path) const; - //! Get a dataset and its dimensions. - //! @param[in] path path to the dataset in the file. - //! @return structure containing the data and the dataset dimensions. - template - HDF5Dataset - getDataset(std::string const& path) const; + //! Get a dataset and its dimensions. + //! @param[in] path path to the dataset in the file. + //! @return structure containing the data and the dataset dimensions. + template + HDF5Dataset + getDataset(std::string const& path) const; - //! Get an attribute. - //! @param[in] path path to the node in the file where the attribute is - //! stored. - //! @param[in] attribute name of the attribute to get. - //! @return the attribute's data. - template - std::vector - getAttribute(std::string const& path, - std::string const& attribute) const; + //! Get an attribute. + //! @param[in] path path to the node in the file where the attribute is + //! stored. + //! @param[in] attribute name of the attribute to get. + //! @return the attribute's data. + template + std::vector + getAttribute(std::string const& path, std::string const& attribute) const; - private: - //! Handle to an HDF5 file. - std::unique_ptr m_file; - }; - } // namespace StreamGenerator - } // namespace StreamSpeed -} // namespace Simulators + private: + //! Handle to an HDF5 file. + std::unique_ptr m_file; + }; + } // namespace Parsers +} // namespace DUNE #endif diff --git a/src/Simulators/StreamSpeed/HDF5Reader.cpp b/src/Simulators/StreamSpeed/HDF5Reader.cpp deleted file mode 100644 index c600e77983..0000000000 --- a/src/Simulators/StreamSpeed/HDF5Reader.cpp +++ /dev/null @@ -1,218 +0,0 @@ -//*************************************************************************** -// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * -// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * -//*************************************************************************** -// This file is part of DUNE: Unified Navigation Environment. * -// * -// Commercial Licence Usage * -// Licencees holding valid commercial DUNE licences may use this file in * -// accordance with the commercial licence agreement provided with the * -// Software or, alternatively, in accordance with the terms contained in a * -// written agreement between you and Faculdade de Engenharia da * -// Universidade do Porto. For licensing terms, conditions, and further * -// information contact lsts@fe.up.pt. * -// * -// Modified European Union Public Licence - EUPL v.1.1 Usage * -// Alternatively, this file may be used under the terms of the Modified * -// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * -// included in the packaging of this file. You may not use this work * -// except in compliance with the Licence. Unless required by applicable * -// law or agreed to in writing, software distributed under the Licence is * -// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * -// ANY KIND, either express or implied. See the Licence for the specific * -// language governing permissions and limitations at * -// https://github.com/LSTS/dune/blob/master/LICENCE.md and * -// http://ec.europa.eu/idabc/eupl.html. * -//*************************************************************************** -// Author: Miguel Aguiar * -//*************************************************************************** - -#include -#include - -#include "DUNE/I18N.hpp" - -// This component depends on the h5cpp library -// (https://github.com/ess-dmsc/h5cpp.com). -// To enable support for h5cpp, set the H5CPP flag to 1 when calling CMake. -// If support for h5cpp is not enabled, the compilation will not fail, but -// the constructor of HDF5Reader will throw. -#if DUNE_H5CPP_ENABLED -# include "h5cpp/hdf5.hpp" -#else -// Define hdf5::file::File to avoid compilation errors due to incomplete type -namespace hdf5 -{ - namespace file - { - class File - {}; - } // namespace file -} // namespace hdf5 -#endif - -#include "HDF5Reader.hpp" - -namespace Simulators -{ - namespace StreamSpeed - { - namespace StreamGenerator - { - HDF5Reader::HDF5Reader(std::string const& filename) - : m_file(std::make_unique()) - { -#ifndef DUNE_H5CPP_ENABLED - throw std::runtime_error( - DTR("HDF5Reader::HDF5Reader(): support for HDF5 i/o is not " - "enabled.")); -#else - try - { - *m_file = - hdf5::file::open(filename, hdf5::file::AccessFlags::READONLY); - - if (m_file == nullptr) - throw; - } - catch (...) - { - throw std::runtime_error( - DTR("HDF5Reader::HDF5Reader(): unable to open file.")); - } -#endif - } - - HDF5Reader::~HDF5Reader() = default; - -#ifdef DUNE_H5CPP_ENABLED - inline bool - dsetExists(File const* f, std::string const& path) - { - return f->root().has_dataset(path); - return false; - } -#endif - - bool - HDF5Reader::datasetExists(std::string const& path) const - { -#ifdef DUNE_H5CPP_ENABLED - return dsetExists(m_file.get(), path); -#else - return false; -#endif - } - - template - HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const& path) const - { -#ifdef DUNE_H5CPP_ENABLED - if (!dsetExists(m_file.get(), path)) - throw std::runtime_error( - DTR("HDF5Reader::getDataset(): The requested dataset does not " - "exist.")); - - auto dset = m_file->root().get_dataset(path); - auto dimensions = - hdf5::dataspace::Simple(dset.dataspace()).current_dimensions(); - - // Total number of gridpoints is the product of the sizes of all - // dimensions. - size_t size = std::accumulate(std::begin(dimensions), - std::end(dimensions), - 1, - std::multiplies<>()); - - std::vector data(size); - dset.read(data); - - // Convert elements of dimensions to size_t - std::vector dimensions_(dimensions.size()); - - std::copy(std::begin(dimensions), - std::end(dimensions), - std::begin(dimensions_)); - - return {dimensions_, data}; -#else - return {}; -#endif - } - - template - std::vector - HDF5Reader::getAttribute(std::string const& path, - std::string const& attribute) const - { -#ifdef DUNE_H5CPP_ENABLED - auto node = hdf5::node::get_node(m_file->root(), path); - auto attributes = node.attributes; - - if (!attributes.exists(attribute)) - throw std::runtime_error( - DTR("HDF5Reader::getAttribute(): requested attribute does not " - "exist.")); - - auto attr_handle = attributes[attribute]; - - std::vector data(attr_handle.dataspace().size()); - - attr_handle.read(data); - - return data; -#else - return {}; -#endif - } - - // Template specialization declarations for getDataset - template HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const&) const; - - template HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const&) const; - - template HDF5Reader::HDF5Dataset - HDF5Reader::getDataset(std::string const&) const; - - // Template specialization declarations for getAttribute - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - - template std::vector - HDF5Reader::getAttribute(std::string const&, - std::string const&) const; - } // namespace StreamGenerator - } // namespace StreamSpeed -} // namespace Simulators diff --git a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp index 31df4c100f..708821a8ae 100644 --- a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp +++ b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp @@ -33,8 +33,9 @@ #include #include +#include "DUNE/Parsers/HDF5Reader.hpp" + #include "Grid.hpp" -#include "HDF5Reader.hpp" #include "StreamGenerator.hpp" namespace Simulators @@ -91,7 +92,7 @@ namespace Simulators double time = 0.0) const override; private: - HDF5Reader m_file; + DUNE::Parsers::HDF5Reader m_file; //! Velocity in the East direction. std::vector m_u; //! Velocity in the North direction. From 58e7e0f516868f6398f64c78c628c5ece845f4bb Mon Sep 17 00:00:00 2001 From: mcpca Date: Thu, 18 Jul 2019 14:13:56 +0100 Subject: [PATCH 36/51] DUNE/Math: Moved Grid to core --- src/DUNE/Math.hpp | 1 + src/DUNE/Math/Grid.hpp | 268 +++++++++++++++++ src/Simulators/StreamSpeed/Grid.hpp | 271 ------------------ .../StreamSpeed/ModelDataStreamGenerator.hpp | 4 +- 4 files changed, 271 insertions(+), 273 deletions(-) create mode 100644 src/DUNE/Math/Grid.hpp delete mode 100644 src/Simulators/StreamSpeed/Grid.hpp diff --git a/src/DUNE/Math.hpp b/src/DUNE/Math.hpp index 5a88490784..c9a3361c3b 100644 --- a/src/DUNE/Math.hpp +++ b/src/DUNE/Math.hpp @@ -49,5 +49,6 @@ namespace DUNE #include #include #include +#include #endif diff --git a/src/DUNE/Math/Grid.hpp b/src/DUNE/Math/Grid.hpp new file mode 100644 index 0000000000..2d5a9d9c7e --- /dev/null +++ b/src/DUNE/Math/Grid.hpp @@ -0,0 +1,268 @@ +//*************************************************************************** +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * +// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * +//*************************************************************************** +// This file is part of DUNE: Unified Navigation Environment. * +// * +// Commercial Licence Usage * +// Licencees holding valid commercial DUNE licences may use this file in * +// accordance with the commercial licence agreement provided with the * +// Software or, alternatively, in accordance with the terms contained in a * +// written agreement between you and Faculdade de Engenharia da * +// Universidade do Porto. For licensing terms, conditions, and further * +// information contact lsts@fe.up.pt. * +// * +// Modified European Union Public Licence - EUPL v.1.1 Usage * +// Alternatively, this file may be used under the terms of the Modified * +// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * +// included in the packaging of this file. You may not use this work * +// except in compliance with the Licence. Unless required by applicable * +// law or agreed to in writing, software distributed under the Licence is * +// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * +// ANY KIND, either express or implied. See the Licence for the specific * +// language governing permissions and limitations at * +// https://github.com/LSTS/dune/blob/master/LICENCE.md and * +// http://ec.europa.eu/idabc/eupl.html. * +//*************************************************************************** +// Author: Miguel Aguiar * +//*************************************************************************** + +#ifndef DUNE_MATH_GRID_HPP_INCLUDED_ +#define DUNE_MATH_GRID_HPP_INCLUDED_ + +#include +#include +#include +#include +#include + +namespace DUNE +{ + namespace Math + { + //! Converts between grid indices, coordinate points and row-major + //! ordering. + //! @tparam dim number of dimensions. + template + class Grid + { + public: + //! Constructor. + //! @param[in] min the lower limits of the grid in each dimension. + //! @param[in] max the upper limits of the grid in each dimension. + //! @param[in] dimensions the number of points in each dimension. + Grid(std::vector const& min, + std::vector const& max, + std::vector const& dimensions); + + //! Convert an array of grid indices to a row-major ordered offset. + //! @param[in] indices the indices of the grid point. + //! @return the row-major offset of the grid point. + size_t + getOffset(std::array const& indices) const; + + //! Convert a row-major ordered offset to an array of grid indices. + //! @param[in] offset the row-major offset of the gridpoint. + //! @return indices of the gridpoint. + std::array + getIndices(size_t offset) const; + + //! Get the indices of the "lower corner" of the grid cell where a + //! point lies. + //! @param[in] coordinates coordinates of a point in dim-dimensional + //! space. + //! @return grid indices of the "lower" vertex of the grid cell where + //! the point lies. + std::array + getCorner(std::array const& coordinates) const; + + //! Get the coordinates of a gridpoint given by an array of indices. + //! @param[in] indices indices of a gridpoint. + //! @return the coordinates of the corresponding point in + //! dim-dimensional space. + std::array + getCoordinates(std::array const& indices) const; + + //! Get the coordinates of a gridpoint given by a row-major offset. + //! @param[in] offset the row-major offset of the gridpoint. + //! @return the coordinates of the corresponding point in + //! dim-dimensional space. + std::array + getCoordinates(size_t offset) const; + + //! Get the number of points in the grid along a dimension. + //! @param[in] dimension index of the dimension to query. + //! @return number of points along the given dimension. + size_t + getDimensions(size_t dimension) const + { + if (dimension >= dim) + throw std::runtime_error("Grid::getDimensions(): invalid index"); + + return m_npts[dimension]; + } + + //! @return the total number of gridpoints. + size_t + size() const + { + return m_size; + } + + //! Get the upper limit of the grid along a dimension. + //! @param[in] dimension index of the dimension to query. + //! @return upper limit along the given dimension. + double + getUpper(size_t dimension) const + { + if (dimension >= dim) + throw std::runtime_error("Grid::getUpper(): invalid index"); + + return m_max[dimension]; + } + + //! Get the loer limit of the grid along a dimension. + //! @param[in] dimension index of the dimension to query. + //! @return lower limit along the given dimension. + double + getLower(size_t dimension) const + { + if (dimension >= dim) + throw std::runtime_error("Grid::getLower(): invalid index"); + + return m_min[dimension]; + } + + //! Get the grid spacing along a dimension. + //! @param[in] dimension index of the dimension to query. + //! @return spacing along the given dimension. + double + getSpacing(size_t dimension) const + { + if (dimension >= dim) + throw std::runtime_error("Grid::getSpacing(): invalid index"); + + return m_h[dimension]; + } + + private: + //! The upper limits of the grid in each dimension. + std::array m_max; + //! The lower limits of the grid in each dimension. + std::array m_min; + //! The grid spacing in each dimension. + std::array m_h; + //! The number of gridpoints in each dimension. + std::array m_npts; + //! The total number of gridpoints. + size_t m_size; + }; + + template + Grid::Grid(std::vector const& min, + std::vector const& max, + std::vector const& dimensions) + + { + if (min.size() < dim || max.size() < dim || dimensions.size() < dim) + throw std::runtime_error( + "Grid::Grid(): insufficient number of elements to create the " + "grid."); + + std::copy_n(std::begin(min), dim, std::begin(m_min)); + std::copy_n(std::begin(max), dim, std::begin(m_max)); + std::copy_n(std::begin(dimensions), dim, std::begin(m_npts)); + + for (auto i = 0; i < dim; ++i) + { + if (m_npts[i] < 2) + throw std::runtime_error("Grid::Grid(): invalid grid dimensions."); + + if (m_max[i] <= m_min[i]) + throw std::runtime_error("Grid::Grid(): invalid grid limits."); + + m_h[i] = (m_max[i] - m_min[i]) / (m_npts[i] - 1); + } + + m_size = std::accumulate( + std::begin(m_npts), std::end(m_npts), 1.0, std::multiplies<>()); + } + + template + size_t + Grid::getOffset(std::array const& indices) const + { + if (indices[0] >= m_npts[0]) + throw std::runtime_error("Grid::getOffset(): out of bounds."); + + size_t offset = indices[0]; + + for (auto i = 1; i < dim; ++i) + { + if (indices[i] >= m_npts[i]) + throw std::runtime_error("Grid::getOffset(): out of bounds."); + + offset = m_npts[i] * offset + indices[i]; + } + + return offset; + } + + template + std::array + Grid::getIndices(size_t offset) const + { + if (offset >= m_size) + throw std::runtime_error("Grid::getIndices(): out of bounds."); + + std::array point; + + for (auto i = dim; i > 0; --i) + { + point[i - 1] = offset % m_npts[i - 1]; + offset /= m_npts[i - 1]; + } + + return point; + } + + template + std::array + Grid::getCorner(std::array const& coordinates) const + { + std::array corner; + + for (auto i = 0; i < dim; ++i) + { + if (coordinates[i] < m_min[i] || coordinates[i] > m_max[i]) + corner[i] = m_npts[i]; + else + corner[i] = std::floor((coordinates[i] - m_min[i]) / m_h[i]); + } + + return corner; + } + + template + std::array + Grid::getCoordinates(std::array const& indices) const + { + std::array coordinates; + + for (auto i = 0; i < dim; ++i) + coordinates[i] = m_min[i] + m_h[i] * indices[i]; + + return coordinates; + } + + template + std::array + Grid::getCoordinates(size_t offset) const + { + return getCoordinates(getIndices(offset)); + } + + } // namespace Math +} // namespace DUNE + +#endif diff --git a/src/Simulators/StreamSpeed/Grid.hpp b/src/Simulators/StreamSpeed/Grid.hpp deleted file mode 100644 index 09d3b94a26..0000000000 --- a/src/Simulators/StreamSpeed/Grid.hpp +++ /dev/null @@ -1,271 +0,0 @@ -//*************************************************************************** -// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * -// Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * -//*************************************************************************** -// This file is part of DUNE: Unified Navigation Environment. * -// * -// Commercial Licence Usage * -// Licencees holding valid commercial DUNE licences may use this file in * -// accordance with the commercial licence agreement provided with the * -// Software or, alternatively, in accordance with the terms contained in a * -// written agreement between you and Faculdade de Engenharia da * -// Universidade do Porto. For licensing terms, conditions, and further * -// information contact lsts@fe.up.pt. * -// * -// Modified European Union Public Licence - EUPL v.1.1 Usage * -// Alternatively, this file may be used under the terms of the Modified * -// EUPL, Version 1.1 only (the "Licence"), appearing in the file LICENCE.md * -// included in the packaging of this file. You may not use this work * -// except in compliance with the Licence. Unless required by applicable * -// law or agreed to in writing, software distributed under the Licence is * -// distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF * -// ANY KIND, either express or implied. See the Licence for the specific * -// language governing permissions and limitations at * -// https://github.com/LSTS/dune/blob/master/LICENCE.md and * -// http://ec.europa.eu/idabc/eupl.html. * -//*************************************************************************** -// Author: Miguel Aguiar * -//*************************************************************************** - -#ifndef SIMULATORS_STREAM_SPEED_GRID_HPP_INCLUDED_ -#define SIMULATORS_STREAM_SPEED_GRID_HPP_INCLUDED_ - -#include -#include -#include -#include -#include - -namespace Simulators -{ - namespace StreamSpeed - { - namespace StreamGenerator - { - //! Converts between grid indices, coordinate points and row-major - //! ordering. - //! @tparam dim number of dimensions. - template - class Grid - { - public: - //! Constructor. - //! @param[in] min the lower limits of the grid in each dimension. - //! @param[in] max the upper limits of the grid in each dimension. - //! @param[in] dimensions the number of points in each dimension. - Grid(std::vector const& min, - std::vector const& max, - std::vector const& dimensions); - - //! Convert an array of grid indices to a row-major ordered offset. - //! @param[in] indices the indices of the grid point. - //! @return the row-major offset of the grid point. - size_t - getOffset(std::array const& indices) const; - - //! Convert a row-major ordered offset to an array of grid indices. - //! @param[in] offset the row-major offset of the gridpoint. - //! @return indices of the gridpoint. - std::array - getIndices(size_t offset) const; - - //! Get the indices of the "lower corner" of the grid cell where a - //! point lies. - //! @param[in] coordinates coordinates of a point in dim-dimensional - //! space. - //! @return grid indices of the "lower" vertex of the grid cell where - //! the point lies. - std::array - getCorner(std::array const& coordinates) const; - - //! Get the coordinates of a gridpoint given by an array of indices. - //! @param[in] indices indices of a gridpoint. - //! @return the coordinates of the corresponding point in - //! dim-dimensional space. - std::array - getCoordinates(std::array const& indices) const; - - //! Get the coordinates of a gridpoint given by a row-major offset. - //! @param[in] offset the row-major offset of the gridpoint. - //! @return the coordinates of the corresponding point in - //! dim-dimensional space. - std::array - getCoordinates(size_t offset) const; - - //! Get the number of points in the grid along a dimension. - //! @param[in] dimension index of the dimension to query. - //! @return number of points along the given dimension. - size_t - getDimensions(size_t dimension) const - { - if (dimension >= dim) - throw std::runtime_error("Grid::getDimensions(): invalid index"); - - return m_npts[dimension]; - } - - //! @return the total number of gridpoints. - size_t - size() const - { - return m_size; - } - - //! Get the upper limit of the grid along a dimension. - //! @param[in] dimension index of the dimension to query. - //! @return upper limit along the given dimension. - double - getUpper(size_t dimension) const - { - if (dimension >= dim) - throw std::runtime_error("Grid::getUpper(): invalid index"); - - return m_max[dimension]; - } - - //! Get the loer limit of the grid along a dimension. - //! @param[in] dimension index of the dimension to query. - //! @return lower limit along the given dimension. - double - getLower(size_t dimension) const - { - if (dimension >= dim) - throw std::runtime_error("Grid::getLower(): invalid index"); - - return m_min[dimension]; - } - - //! Get the grid spacing along a dimension. - //! @param[in] dimension index of the dimension to query. - //! @return spacing along the given dimension. - double - getSpacing(size_t dimension) const - { - if(dimension >= dim) - throw std::runtime_error("Grid::getSpacing(): invalid index"); - - return m_h[dimension]; - } - - private: - //! The upper limits of the grid in each dimension. - std::array m_max; - //! The lower limits of the grid in each dimension. - std::array m_min; - //! The grid spacing in each dimension. - std::array m_h; - //! The number of gridpoints in each dimension. - std::array m_npts; - //! The total number of gridpoints. - size_t m_size; - }; - - template - Grid::Grid(std::vector const& min, - std::vector const& max, - std::vector const& dimensions) - - { - if (min.size() < dim || max.size() < dim || dimensions.size() < dim) - throw std::runtime_error( - "Grid::Grid(): insufficient number of elements to create the " - "grid."); - - std::copy_n(std::begin(min), dim, std::begin(m_min)); - std::copy_n(std::begin(max), dim, std::begin(m_max)); - std::copy_n(std::begin(dimensions), dim, std::begin(m_npts)); - - for (auto i = 0; i < dim; ++i) - { - if (m_npts[i] < 2) - throw std::runtime_error("Grid::Grid(): invalid grid dimensions."); - - if (m_max[i] <= m_min[i]) - throw std::runtime_error("Grid::Grid(): invalid grid limits."); - - m_h[i] = (m_max[i] - m_min[i]) / (m_npts[i] - 1); - } - - m_size = std::accumulate( - std::begin(m_npts), std::end(m_npts), 1.0, std::multiplies<>()); - } - - template - size_t - Grid::getOffset(std::array const& indices) const - { - if (indices[0] >= m_npts[0]) - throw std::runtime_error("Grid::getOffset(): out of bounds."); - - size_t offset = indices[0]; - - for (auto i = 1; i < dim; ++i) - { - if (indices[i] >= m_npts[i]) - throw std::runtime_error("Grid::getOffset(): out of bounds."); - - offset = m_npts[i] * offset + indices[i]; - } - - return offset; - } - - template - std::array - Grid::getIndices(size_t offset) const - { - if (offset >= m_size) - throw std::runtime_error("Grid::getIndices(): out of bounds."); - - std::array point; - - for (auto i = dim; i > 0; --i) - { - point[i - 1] = offset % m_npts[i - 1]; - offset /= m_npts[i - 1]; - } - - return point; - } - - template - std::array - Grid::getCorner(std::array const& coordinates) const - { - std::array corner; - - for (auto i = 0; i < dim; ++i) - { - if (coordinates[i] < m_min[i] || coordinates[i] > m_max[i]) - corner[i] = m_npts[i]; - else - corner[i] = std::floor((coordinates[i] - m_min[i]) / m_h[i]); - } - - return corner; - } - - template - std::array - Grid::getCoordinates(std::array const& indices) const - { - std::array coordinates; - - for (auto i = 0; i < dim; ++i) - coordinates[i] = m_min[i] + m_h[i] * indices[i]; - - return coordinates; - } - - template - std::array - Grid::getCoordinates(size_t offset) const - { - return getCoordinates(getIndices(offset)); - } - - } // namespace StreamGenerator - } // namespace StreamSpeed -} // namespace Simulators - -#endif diff --git a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp index 708821a8ae..7f9afee1d5 100644 --- a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp +++ b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp @@ -34,8 +34,8 @@ #include #include "DUNE/Parsers/HDF5Reader.hpp" +#include "DUNE/Math/Grid.hpp" -#include "Grid.hpp" #include "StreamGenerator.hpp" namespace Simulators @@ -98,7 +98,7 @@ namespace Simulators //! Velocity in the North direction. std::vector m_v; //! Converts between grid indices and datapoint indices. - Grid<3> m_grid; + DUNE::Math::Grid<3> m_grid; }; } // namespace StreamGenerator } // namespace StreamSpeed From 2346f52a036aca370489fb183af88e437a780e42 Mon Sep 17 00:00:00 2001 From: mcpca Date: Thu, 18 Jul 2019 14:43:59 +0100 Subject: [PATCH 37/51] DUNE/Math: Grid.hpp: Fixed signedness-related warnings. --- src/DUNE/Math/Grid.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/DUNE/Math/Grid.hpp b/src/DUNE/Math/Grid.hpp index 2d5a9d9c7e..3e89edd08a 100644 --- a/src/DUNE/Math/Grid.hpp +++ b/src/DUNE/Math/Grid.hpp @@ -173,7 +173,7 @@ namespace DUNE std::copy_n(std::begin(max), dim, std::begin(m_max)); std::copy_n(std::begin(dimensions), dim, std::begin(m_npts)); - for (auto i = 0; i < dim; ++i) + for (size_t i = 0; i < dim; ++i) { if (m_npts[i] < 2) throw std::runtime_error("Grid::Grid(): invalid grid dimensions."); @@ -197,7 +197,7 @@ namespace DUNE size_t offset = indices[0]; - for (auto i = 1; i < dim; ++i) + for (size_t i = 1; i < dim; ++i) { if (indices[i] >= m_npts[i]) throw std::runtime_error("Grid::getOffset(): out of bounds."); @@ -217,7 +217,7 @@ namespace DUNE std::array point; - for (auto i = dim; i > 0; --i) + for (size_t i = dim; i > 0; --i) { point[i - 1] = offset % m_npts[i - 1]; offset /= m_npts[i - 1]; @@ -232,7 +232,7 @@ namespace DUNE { std::array corner; - for (auto i = 0; i < dim; ++i) + for (size_t i = 0; i < dim; ++i) { if (coordinates[i] < m_min[i] || coordinates[i] > m_max[i]) corner[i] = m_npts[i]; @@ -249,7 +249,7 @@ namespace DUNE { std::array coordinates; - for (auto i = 0; i < dim; ++i) + for (size_t i = 0; i < dim; ++i) coordinates[i] = m_min[i] + m_h[i] * indices[i]; return coordinates; From 63fb83916cd9459325efb514b54920dbbea65309 Mon Sep 17 00:00:00 2001 From: mcpca Date: Thu, 18 Jul 2019 14:50:56 +0100 Subject: [PATCH 38/51] Simulators/StreamSpeed: Fixed warnings due to unused parameters. --- src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp | 2 +- src/Simulators/StreamSpeed/StreamGenerator.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp index 9cb15ea2ba..7fcd4ef19b 100644 --- a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp +++ b/src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp @@ -90,7 +90,7 @@ namespace Simulators std::array Gridded2DModelDataStreamGenerator::getSpeed(double lat, double lon, - double depth, + double, double time) const { // Find closest grid cell diff --git a/src/Simulators/StreamSpeed/StreamGenerator.cpp b/src/Simulators/StreamSpeed/StreamGenerator.cpp index a38f42d2d9..740a954c38 100644 --- a/src/Simulators/StreamSpeed/StreamGenerator.cpp +++ b/src/Simulators/StreamSpeed/StreamGenerator.cpp @@ -40,10 +40,10 @@ namespace Simulators {} std::array - StreamGenerator::getSpeed(double lat, - double lon, - double depth, - double time) const + StreamGenerator::getSpeed(double, + double, + double, + double) const { return getDefaultSpeed(); } From 8930b1fe81b8ac1a5e8ac8aefdebad7786dc1cec Mon Sep 17 00:00:00 2001 From: mcpca Date: Thu, 18 Jul 2019 15:28:53 +0100 Subject: [PATCH 39/51] DUNE/Parsers: HDF5Reader: Fixed warnings due to unused parameters when DUNE is compiled without h5cpp support. --- src/DUNE/Parsers/HDF5Reader.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/DUNE/Parsers/HDF5Reader.cpp b/src/DUNE/Parsers/HDF5Reader.cpp index 019a35586b..c6b411938b 100644 --- a/src/DUNE/Parsers/HDF5Reader.cpp +++ b/src/DUNE/Parsers/HDF5Reader.cpp @@ -61,6 +61,7 @@ namespace DUNE : m_file(std::make_unique()) { #ifndef DUNE_H5CPP_ENABLED + (void)filename; throw std::runtime_error( DTR("HDF5Reader::HDF5Reader(): support for HDF5 i/o is not " "enabled.")); @@ -97,6 +98,7 @@ namespace DUNE #ifdef DUNE_H5CPP_ENABLED return dsetExists(m_file.get(), path); #else + (void)path; return false; #endif } @@ -132,6 +134,7 @@ namespace DUNE return {dimensions_, data}; #else + (void)path; return {}; #endif } @@ -158,6 +161,8 @@ namespace DUNE return data; #else + (void)path; + (void)attribute; return {}; #endif } From 95dc3cb5bff556e39774722d890f1d3f661a8942 Mon Sep 17 00:00:00 2001 From: mcpca Date: Fri, 19 Jul 2019 17:42:13 +0100 Subject: [PATCH 40/51] Simulators/StreamSpeed: Check for NaN values before dispatching. --- src/Simulators/StreamSpeed/Task.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/Simulators/StreamSpeed/Task.cpp b/src/Simulators/StreamSpeed/Task.cpp index 9fe9769d9e..53e48195d8 100644 --- a/src/Simulators/StreamSpeed/Task.cpp +++ b/src/Simulators/StreamSpeed/Task.cpp @@ -30,6 +30,7 @@ // Standard library headers. #include #include +#include // DUNE headers. #include @@ -201,14 +202,14 @@ namespace Simulators DUNE::Time::Clock::get() + m_time0); //! Fill EstimatedStreamVelocity. - m_esv.x = p[0]; - m_esv.y = p[1]; - m_esv.z = p[2]; + m_esv.x = std::isnan(p[0]) ? m_args.default_wx : p[0]; + m_esv.y = std::isnan(p[1]) ? m_args.default_wy : p[1]; + m_esv.z = std::isnan(p[2]) ? m_args.default_wz : p[2]; debug(DTR("Stream speed is %f m/s N : %f m/s E : %f m/s D"), - p[0], - p[1], - p[2]); + m_esv.x, + m_esv.y, + m_esv.z); } void From bba574d135bcf15c8df019aea04c3c7dc57418e5 Mon Sep 17 00:00:00 2001 From: mcpca Date: Fri, 19 Jul 2019 17:47:34 +0100 Subject: [PATCH 41/51] Simulators/StreamSpeed: Check SimulatedState source. --- src/Simulators/StreamSpeed/Task.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Simulators/StreamSpeed/Task.cpp b/src/Simulators/StreamSpeed/Task.cpp index 53e48195d8..e1e7b345a6 100644 --- a/src/Simulators/StreamSpeed/Task.cpp +++ b/src/Simulators/StreamSpeed/Task.cpp @@ -215,7 +215,8 @@ namespace Simulators void consume(IMC::SimulatedState const* msg) { - m_state = *msg; + if (msg->getSource() == getSystemId()) + m_state = *msg; } void From bac1d09f2fce00b5f308460569cf7db70897b66d Mon Sep 17 00:00:00 2001 From: mcpca Date: Fri, 19 Jul 2019 18:28:44 +0100 Subject: [PATCH 42/51] Simulators/StreamSpeed: Pass correct height value to the stream speed generator. --- src/Simulators/StreamSpeed/Task.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Simulators/StreamSpeed/Task.cpp b/src/Simulators/StreamSpeed/Task.cpp index e1e7b345a6..3ab88ed756 100644 --- a/src/Simulators/StreamSpeed/Task.cpp +++ b/src/Simulators/StreamSpeed/Task.cpp @@ -198,7 +198,7 @@ namespace Simulators auto p = m_ssg->getSpeed(Angles::degrees(lat), Angles::degrees(lon), - -m_state.height, + -height, DUNE::Time::Clock::get() + m_time0); //! Fill EstimatedStreamVelocity. From ab987fafd5c3a3cca29a818904301319e59054fd Mon Sep 17 00:00:00 2001 From: mcpca Date: Fri, 19 Jul 2019 18:38:44 +0100 Subject: [PATCH 43/51] Simulators/StreamSpeed: Renamed task to StreamVelocity --- .../ModelDataStreamGenerator.cpp | 4 ++-- .../ModelDataStreamGenerator.hpp | 4 ++-- .../{StreamSpeed => StreamVelocity}/StreamGenerator.cpp | 4 ++-- .../{StreamSpeed => StreamVelocity}/StreamGenerator.hpp | 4 ++-- .../StreamGeneratorFactory.hpp | 4 ++-- src/Simulators/{StreamSpeed => StreamVelocity}/Task.cmake | 0 src/Simulators/{StreamSpeed => StreamVelocity}/Task.cpp | 4 ++-- 7 files changed, 12 insertions(+), 12 deletions(-) rename src/Simulators/{StreamSpeed => StreamVelocity}/ModelDataStreamGenerator.cpp (99%) rename src/Simulators/{StreamSpeed => StreamVelocity}/ModelDataStreamGenerator.hpp (98%) rename src/Simulators/{StreamSpeed => StreamVelocity}/StreamGenerator.cpp (97%) rename src/Simulators/{StreamSpeed => StreamVelocity}/StreamGenerator.hpp (98%) rename src/Simulators/{StreamSpeed => StreamVelocity}/StreamGeneratorFactory.hpp (98%) rename src/Simulators/{StreamSpeed => StreamVelocity}/Task.cmake (100%) rename src/Simulators/{StreamSpeed => StreamVelocity}/Task.cpp (99%) diff --git a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp similarity index 99% rename from src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp rename to src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp index 7fcd4ef19b..b4b2220a59 100644 --- a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.cpp +++ b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp @@ -34,7 +34,7 @@ namespace Simulators { - namespace StreamSpeed + namespace StreamVelocity { namespace StreamGenerator { @@ -137,5 +137,5 @@ namespace Simulators return {v_val, u_val, 0.0}; } } // namespace StreamGenerator - } // namespace StreamSpeed + } // namespace StreamVelocity } // namespace Simulators diff --git a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp similarity index 98% rename from src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp rename to src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp index 7f9afee1d5..8a40f1794f 100644 --- a/src/Simulators/StreamSpeed/ModelDataStreamGenerator.hpp +++ b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp @@ -40,7 +40,7 @@ namespace Simulators { - namespace StreamSpeed + namespace StreamVelocity { namespace StreamGenerator { @@ -101,7 +101,7 @@ namespace Simulators DUNE::Math::Grid<3> m_grid; }; } // namespace StreamGenerator - } // namespace StreamSpeed + } // namespace StreamVelocity } // namespace Simulators #endif diff --git a/src/Simulators/StreamSpeed/StreamGenerator.cpp b/src/Simulators/StreamVelocity/StreamGenerator.cpp similarity index 97% rename from src/Simulators/StreamSpeed/StreamGenerator.cpp rename to src/Simulators/StreamVelocity/StreamGenerator.cpp index 740a954c38..039c2c7c3c 100644 --- a/src/Simulators/StreamSpeed/StreamGenerator.cpp +++ b/src/Simulators/StreamVelocity/StreamGenerator.cpp @@ -31,7 +31,7 @@ namespace Simulators { - namespace StreamSpeed + namespace StreamVelocity { namespace StreamGenerator { @@ -62,5 +62,5 @@ namespace Simulators m_wz = wz; } } // namespace StreamGenerator - } // namespace StreamSpeed + } // namespace StreamVelocity } // namespace Simulators diff --git a/src/Simulators/StreamSpeed/StreamGenerator.hpp b/src/Simulators/StreamVelocity/StreamGenerator.hpp similarity index 98% rename from src/Simulators/StreamSpeed/StreamGenerator.hpp rename to src/Simulators/StreamVelocity/StreamGenerator.hpp index f97fd50575..af22d6570f 100644 --- a/src/Simulators/StreamSpeed/StreamGenerator.hpp +++ b/src/Simulators/StreamVelocity/StreamGenerator.hpp @@ -34,7 +34,7 @@ namespace Simulators { - namespace StreamSpeed + namespace StreamVelocity { namespace StreamGenerator { @@ -83,7 +83,7 @@ namespace Simulators double m_wz; }; } // namespace StreamGenerator - } // namespace StreamSpeed + } // namespace StreamVelocity } // namespace Simulators #endif diff --git a/src/Simulators/StreamSpeed/StreamGeneratorFactory.hpp b/src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp similarity index 98% rename from src/Simulators/StreamSpeed/StreamGeneratorFactory.hpp rename to src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp index 3164892585..de58dea796 100644 --- a/src/Simulators/StreamSpeed/StreamGeneratorFactory.hpp +++ b/src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp @@ -39,7 +39,7 @@ namespace Simulators { - namespace StreamSpeed + namespace StreamVelocity { namespace StreamGenerator { @@ -77,7 +77,7 @@ namespace Simulators throw std::runtime_error(DTR("Unknown stream velocity source type.")); } } // namespace StreamGenerator - } // namespace StreamSpeed + } // namespace StreamVelocity } // namespace Simulators #endif diff --git a/src/Simulators/StreamSpeed/Task.cmake b/src/Simulators/StreamVelocity/Task.cmake similarity index 100% rename from src/Simulators/StreamSpeed/Task.cmake rename to src/Simulators/StreamVelocity/Task.cmake diff --git a/src/Simulators/StreamSpeed/Task.cpp b/src/Simulators/StreamVelocity/Task.cpp similarity index 99% rename from src/Simulators/StreamSpeed/Task.cpp rename to src/Simulators/StreamVelocity/Task.cpp index 3ab88ed756..a4300925ce 100644 --- a/src/Simulators/StreamSpeed/Task.cpp +++ b/src/Simulators/StreamVelocity/Task.cpp @@ -47,7 +47,7 @@ namespace Simulators //! message. //! Currently supports only a single vehicle. //! @author Miguel Aguiar - namespace StreamSpeed + namespace StreamVelocity { using DUNE_NAMESPACES; @@ -226,7 +226,7 @@ namespace Simulators dispatch(m_esv); } }; - } // namespace StreamSpeed + } // namespace StreamVelocity } // namespace Simulators DUNE_TASK From 0600c2541fa3f27d2d7766fe3f50ef72d2a19fd3 Mon Sep 17 00:00:00 2001 From: mcpca Date: Fri, 19 Jul 2019 18:52:28 +0100 Subject: [PATCH 44/51] Simulators/StreamVelocity: Change speed to velocity everywhere. --- .../ModelDataStreamGenerator.cpp | 12 ++++++------ .../ModelDataStreamGenerator.hpp | 12 ++++++------ .../StreamVelocity/StreamGenerator.cpp | 11 ++++------- .../StreamVelocity/StreamGenerator.hpp | 19 +++++++++++-------- .../StreamVelocity/StreamGeneratorFactory.hpp | 4 ++-- src/Simulators/StreamVelocity/Task.cpp | 12 ++++++------ 6 files changed, 35 insertions(+), 35 deletions(-) diff --git a/src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp index b4b2220a59..9ae03cc07e 100644 --- a/src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp +++ b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.cpp @@ -88,19 +88,19 @@ namespace Simulators } std::array - Gridded2DModelDataStreamGenerator::getSpeed(double lat, - double lon, - double, - double time) const + Gridded2DModelDataStreamGenerator::getVelocity(double lat, + double lon, + double, + double time) const { // Find closest grid cell auto corner_indices = m_grid.getCorner({lat, lon, time + m_grid.getLower(2)}); - // If vehicles is outside the grid, fall back to default stream speed + // If vehicles is outside the grid, fall back to default stream velocity for (auto i = 0; i < 3; ++i) if (corner_indices[i] >= m_grid.getDimensions(i)) - return getDefaultSpeed(); + return getDefaultVelocity(); auto corner_point = m_grid.getCoordinates(corner_indices); diff --git a/src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp index 8a40f1794f..c543c23621 100644 --- a/src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp +++ b/src/Simulators/StreamVelocity/ModelDataStreamGenerator.hpp @@ -27,8 +27,8 @@ // Author: Miguel Aguiar * //*************************************************************************** -#ifndef SIMULATORS_STREAM_SPEED_MODEL_DATA_STREAM_GENERATOR_HPP_INCLUDED_ -#define SIMULATORS_STREAM_SPEED_MODEL_DATA_STREAM_GENERATOR_HPP_INCLUDED_ +#ifndef SIMULATORS_STREAM_VELOCITY_MODEL_DATA_STREAM_GENERATOR_HPP_INCLUDED_ +#define SIMULATORS_STREAM_VELOCITY_MODEL_DATA_STREAM_GENERATOR_HPP_INCLUDED_ #include #include @@ -86,10 +86,10 @@ namespace Simulators ~Gridded2DModelDataStreamGenerator() = default; virtual std::array - getSpeed(double lat, - double lon, - double depth, - double time = 0.0) const override; + getVelocity(double lat, + double lon, + double depth, + double time = 0.0) const override; private: DUNE::Parsers::HDF5Reader m_file; diff --git a/src/Simulators/StreamVelocity/StreamGenerator.cpp b/src/Simulators/StreamVelocity/StreamGenerator.cpp index 039c2c7c3c..0f29ced480 100644 --- a/src/Simulators/StreamVelocity/StreamGenerator.cpp +++ b/src/Simulators/StreamVelocity/StreamGenerator.cpp @@ -40,22 +40,19 @@ namespace Simulators {} std::array - StreamGenerator::getSpeed(double, - double, - double, - double) const + StreamGenerator::getVelocity(double, double, double, double) const { - return getDefaultSpeed(); + return getDefaultVelocity(); } std::array - StreamGenerator::getDefaultSpeed() const + StreamGenerator::getDefaultVelocity() const { return {m_wx, m_wy, m_wz}; } void - StreamGenerator::setSpeed(double wx, double wy, double wz) + StreamGenerator::setVelocity(double wx, double wy, double wz) { m_wx = wx; m_wy = wy; diff --git a/src/Simulators/StreamVelocity/StreamGenerator.hpp b/src/Simulators/StreamVelocity/StreamGenerator.hpp index af22d6570f..0a9607e90e 100644 --- a/src/Simulators/StreamVelocity/StreamGenerator.hpp +++ b/src/Simulators/StreamVelocity/StreamGenerator.hpp @@ -27,8 +27,8 @@ // Author: Miguel Aguiar * //*************************************************************************** -#ifndef SIMULATORS_STREAM_SPEED_STREAM_GENERATOR_HPP_INCLUDED_ -#define SIMULATORS_STREAM_SPEED_STREAM_GENERATOR_HPP_INCLUDED_ +#ifndef SIMULATORS_STREAM_VELOCITY_STREAM_GENERATOR_HPP_INCLUDED_ +#define SIMULATORS_STREAM_VELOCITY_STREAM_GENERATOR_HPP_INCLUDED_ #include @@ -51,7 +51,7 @@ namespace Simulators ~StreamGenerator() = default; - //! Get a stream speed value. + //! Get a stream velocity value. //! @param[in] lat WGS84 latitude in degrees. //! @param[in] lon WGS84 longitude in degrees. //! @param[in] depth depth in meters. @@ -59,20 +59,23 @@ namespace Simulators //! @return 3-dimensional array containing the stream velocity in the //! North, East and Down directions in meters per second. virtual std::array - getSpeed(double lat, double lon, double depth, double time = 0.0) const; + getVelocity(double lat, + double lon, + double depth, + double time = 0.0) const; - //! Get the default stream speed. + //! Get the default stream velocity. //! @return 3-dimensional array containing the default stream velocity //! value in the North, East and Down directions in meters per second. std::array - getDefaultSpeed() const; + getDefaultVelocity() const; - //! Set the default stream speed. + //! Set the default stream velocity. //! @param[in] wx default stream speed in the North direction (m/s). //! @param[in] wy default stream speed in the East direction (m/s). //! @param[in] wz default stream speed in the Down direction (m/s). void - setSpeed(double wx, double wy, double wz = 0.0); + setVelocity(double wx, double wy, double wz = 0.0); private: //! Default speed North. diff --git a/src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp b/src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp index de58dea796..0ec7c92884 100644 --- a/src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp +++ b/src/Simulators/StreamVelocity/StreamGeneratorFactory.hpp @@ -27,8 +27,8 @@ // Author: Miguel Aguiar * //*************************************************************************** -#ifndef SIMULATORS_STREAM_SPEED_STREAM_GENERATOR_FACTORY_HPP_INCLUDED_ -#define SIMULATORS_STREAM_SPEED_STREAM_GENERATOR_FACTORY_HPP_INCLUDED_ +#ifndef SIMULATORS_STREAM_VELOCITY_STREAM_GENERATOR_FACTORY_HPP_INCLUDED_ +#define SIMULATORS_STREAM_VELOCITY_STREAM_GENERATOR_FACTORY_HPP_INCLUDED_ #include diff --git a/src/Simulators/StreamVelocity/Task.cpp b/src/Simulators/StreamVelocity/Task.cpp index a4300925ce..6128e18a78 100644 --- a/src/Simulators/StreamVelocity/Task.cpp +++ b/src/Simulators/StreamVelocity/Task.cpp @@ -150,7 +150,7 @@ namespace Simulators void onResourceInitialization(void) { - debug(DTR("Setting default stream speed values: %f m/s N : %f m/s E : " + debug(DTR("Setting default stream velocity: %f m/s N : %f m/s E : " "%f m/s D"), m_args.default_wx, m_args.default_wy, @@ -196,17 +196,17 @@ namespace Simulators Coordinates::WGS84::displace( m_state.x, m_state.y, m_state.z, &lat, &lon, &height); - auto p = m_ssg->getSpeed(Angles::degrees(lat), - Angles::degrees(lon), - -height, - DUNE::Time::Clock::get() + m_time0); + auto p = m_ssg->getVelocity(Angles::degrees(lat), + Angles::degrees(lon), + -height, + DUNE::Time::Clock::get() + m_time0); //! Fill EstimatedStreamVelocity. m_esv.x = std::isnan(p[0]) ? m_args.default_wx : p[0]; m_esv.y = std::isnan(p[1]) ? m_args.default_wy : p[1]; m_esv.z = std::isnan(p[2]) ? m_args.default_wz : p[2]; - debug(DTR("Stream speed is %f m/s N : %f m/s E : %f m/s D"), + debug(DTR("Stream velocity is %f m/s N : %f m/s E : %f m/s D"), m_esv.x, m_esv.y, m_esv.z); From 80211ee66925000e157d3b9aaf2a100b0ff5917e Mon Sep 17 00:00:00 2001 From: mcpca Date: Fri, 19 Jul 2019 19:08:17 +0100 Subject: [PATCH 45/51] Config: auv/simulator: Added StreamVelocity task. --- etc/auv/simulator.ini | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/etc/auv/simulator.ini b/etc/auv/simulator.ini index f8537d17e7..d570b847d1 100644 --- a/etc/auv/simulator.ini +++ b/etc/auv/simulator.ini @@ -156,3 +156,13 @@ Name of message to produce = SoundSpeed Value at peak = 1522 Value away from peak = 1500 Invalid at Surface = true + +[Simulators.StreamVelocity] +Enabled = Never +Entity Label = Stream Velocity Simulator +Default Speed North = 0.0 +Default Speed East = 0.0 +Default Speed Down = 0.0 +Stream Velocity Source = Constant +# Stream Velocity Source = Gridded 2D Model Data +Execution Frequency = 1 From 813170eb35a8183fd705b3e2ddc1e0ec6fa4b811 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Tue, 23 Jul 2019 23:01:39 +0200 Subject: [PATCH 46/51] Control/UAV/Ardupilot: Ensure hae_offset is not reset upon losing GPS fix Previously, m_hae_offset would be reset to zero if a single GpsFix message indicated poor Gps conditions, and since it did not reset the m_offset_st flag, it would remain zero forever, causing the "WGS84 height" in DUNE to be equal to MSL. Now, the offset is only set to zero if the conversion flag is false, in which case m_offset_st is also reset to false. If Gps is lost, the old value of the offset is used. --- src/Control/UAV/Ardupilot/Task.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/Control/UAV/Ardupilot/Task.cpp b/src/Control/UAV/Ardupilot/Task.cpp index e5bad52720..8ac4a18b50 100644 --- a/src/Control/UAV/Ardupilot/Task.cpp +++ b/src/Control/UAV/Ardupilot/Task.cpp @@ -1811,12 +1811,16 @@ namespace Control m_hae_offset = wmm.height(m_lat, m_lon); m_reboot = false; m_offset_st = true; + debug("Height offset at %3.10f/%3.10f is %f", Angles::degrees(m_lat), Angles::degrees(m_lon), m_hae_offset); } } - else + else if (m_args.convert_msl == false) { m_hae_offset = 0; + m_offset_st = false; + debug("Height offset set to zero (convert?%d, gpstype: %d)",m_args.convert_msl, m_fix.type); } + //else: m_args.convert_msl is true, but we do not have/have lost GPS: leave m_hae_offset as is m_estate.lat = m_lat; m_estate.lon = m_lon; From 96195b34f1bf0c0dc6c28dd775ed4abe9dad1b76 Mon Sep 17 00:00:00 2001 From: Kristoffer Gryte Date: Tue, 23 Jul 2019 23:10:56 +0200 Subject: [PATCH 47/51] Control/UAV/Ardupilot: Only change to GUIDED if not in GUIDED --- src/Control/UAV/Ardupilot/Task.cpp | 25 ++++++++++++++----------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/src/Control/UAV/Ardupilot/Task.cpp b/src/Control/UAV/Ardupilot/Task.cpp index e5bad52720..80ef629c63 100644 --- a/src/Control/UAV/Ardupilot/Task.cpp +++ b/src/Control/UAV/Ardupilot/Task.cpp @@ -932,20 +932,23 @@ namespace Control return; } - uint8_t buf[512], mode; + uint8_t buf[512]; mavlink_message_t msg; uint16_t n; - // Copters must first be set to guided as of AC 3.2 - // Planes must first be set to guided as of AP 3.3.0 - mode = (m_vehicle_type == VEHICLE_COPTER) ? (uint8_t)CP_MODE_GUIDED : (uint8_t)PL_MODE_GUIDED; - mavlink_msg_set_mode_pack(255, 0, &msg, - m_sysid, - 1, - mode); - n = mavlink_msg_to_send_buffer(buf, &msg); - sendData(buf, n); - debug("Guided MODE on ardupilot is set."); + if (!((m_mode == CP_MODE_GUIDED) || (m_mode == PL_MODE_GUIDED))) + { + // Copters must first be set to guided as of AC 3.2 + // Planes must first be set to guided as of AP 3.3.0 + uint8_t mode = (m_vehicle_type == VEHICLE_COPTER) ? (uint8_t)CP_MODE_GUIDED : (uint8_t)PL_MODE_GUIDED; + mavlink_msg_set_mode_pack(255, 0, &msg, + m_sysid, + 1, + mode); + n = mavlink_msg_to_send_buffer(buf, &msg); + sendData(buf, n); + debug("Guided MODE on ardupilot is set."); + } //! Setting airspeed parameter if (m_vehicle_type == VEHICLE_COPTER) From a4ed45466671bf5c2ae0c4a65421f1fd27519f92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Wed, 24 Jul 2019 17:54:29 +0100 Subject: [PATCH 48/51] Hardware/PWM: update Copyright. --- src/DUNE/Hardware/PWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/Hardware/PWM.cpp b/src/DUNE/Hardware/PWM.cpp index e04b6a1278..c21eeee8b4 100644 --- a/src/DUNE/Hardware/PWM.cpp +++ b/src/DUNE/Hardware/PWM.cpp @@ -1,5 +1,5 @@ //*************************************************************************** -// Copyright 2007-2017 Universidade do Porto - Faculdade de Engenharia * +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * From 30c47090dac9ca75dcf39a667e93693824565edf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Wed, 24 Jul 2019 17:55:05 +0100 Subject: [PATCH 49/51] Hardware/PWM: update Copyright of PWM.hpp --- src/DUNE/Hardware/PWM.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/Hardware/PWM.hpp b/src/DUNE/Hardware/PWM.hpp index 19ea563651..9ea6ad2cda 100644 --- a/src/DUNE/Hardware/PWM.hpp +++ b/src/DUNE/Hardware/PWM.hpp @@ -1,5 +1,5 @@ //*************************************************************************** -// Copyright 2007-2017 Universidade do Porto - Faculdade de Engenharia * +// Copyright 2007-2019 Universidade do Porto - Faculdade de Engenharia * // Laboratório de Sistemas e Tecnologia Subaquática (LSTS) * //*************************************************************************** // This file is part of DUNE: Unified Navigation Environment. * From 57ac62574718a753d98aca138e92733169b5d6d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Wed, 24 Jul 2019 18:00:41 +0100 Subject: [PATCH 50/51] Hardware/PWM: fix include path of PWM.hpp --- src/DUNE/Hardware/PWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/DUNE/Hardware/PWM.cpp b/src/DUNE/Hardware/PWM.cpp index c21eeee8b4..8e55a281b3 100644 --- a/src/DUNE/Hardware/PWM.cpp +++ b/src/DUNE/Hardware/PWM.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include namespace DUNE { From d01a87e5618fa3747a411a20703c370cdc3671a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Gon=C3=A7alves?= Date: Wed, 24 Jul 2019 18:01:40 +0100 Subject: [PATCH 51/51] Hardware/PWM: change name of ifndef to DUNE. --- src/DUNE/Hardware/PWM.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/DUNE/Hardware/PWM.hpp b/src/DUNE/Hardware/PWM.hpp index 9ea6ad2cda..ea611b0bc0 100644 --- a/src/DUNE/Hardware/PWM.hpp +++ b/src/DUNE/Hardware/PWM.hpp @@ -27,8 +27,8 @@ // Author: Kristoffer Gryte * //*************************************************************************** -#ifndef USER2_HARDWARE_PWM_HPP_INCLUDED_ -#define USER2_HARDWARE_PWM_HPP_INCLUDED_ +#ifndef DUNE_HARDWARE_PWM_HPP_INCLUDED_ +#define DUNE_HARDWARE_PWM_HPP_INCLUDED_ #include