forked from Archive/PX4-Autopilot
visual studio build compatibility
This commit is contained in:
parent
3a61035ac2
commit
d6a3110918
|
@ -53,18 +53,31 @@ set(ECL_SOURCE_DIR ${CMAKE_CURRENT_SOURCE_DIR})
|
|||
if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
|
||||
# ECL standalone build
|
||||
add_definitions(-DECL_STANDALONE)
|
||||
set(ECL_STANDALONE 1)
|
||||
|
||||
add_custom_target(prebuild_targets)
|
||||
|
||||
add_compile_options(
|
||||
-pedantic
|
||||
|
||||
-Wall
|
||||
-Wextra
|
||||
-Werror
|
||||
if(MSVC)
|
||||
add_compile_options(
|
||||
/W4
|
||||
/WX
|
||||
|
||||
-Wno-unused-parameter
|
||||
)
|
||||
/D_USE_MATH_DEFINES
|
||||
|
||||
/wd4100 # warning C4100: unreferenced formal parameter
|
||||
/wd4244 # warning C4244: '=': conversion from 'double' to 'int32_t', possible loss of data
|
||||
)
|
||||
elseif(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
|
||||
add_compile_options(
|
||||
-pedantic
|
||||
|
||||
-Wall
|
||||
-Wextra
|
||||
-Werror
|
||||
|
||||
-Wno-unused-parameter
|
||||
)
|
||||
endif()
|
||||
|
||||
# fetch latest matrix from github
|
||||
include(ExternalProject)
|
||||
|
@ -87,7 +100,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
|
|||
if(BUILD_TESTING)
|
||||
|
||||
add_custom_target(check
|
||||
COMMAND env CTEST_OUTPUT_ON_FAILURE=1 ${CMAKE_CTEST_COMMAND}
|
||||
COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure -C Debug
|
||||
DEPENDS ecl_EKF
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
|
|
@ -227,9 +227,9 @@ bool Ekf::initialiseFilter()
|
|||
}
|
||||
|
||||
// check to see if we have enough measurements and return false if not
|
||||
bool hgt_count_fail = _hgt_counter <= 2 * _obs_buffer_length;
|
||||
bool mag_count_fail = _mag_counter <= 2 * _obs_buffer_length;
|
||||
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2 * _obs_buffer_length);
|
||||
bool hgt_count_fail = _hgt_counter <= 2u * _obs_buffer_length;
|
||||
bool mag_count_fail = _mag_counter <= 2u * _obs_buffer_length;
|
||||
bool ev_count_fail = ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVYAW)) && (_ev_counter <= 2u * _obs_buffer_length);
|
||||
|
||||
if (hgt_count_fail || mag_count_fail || ev_count_fail) {
|
||||
return false;
|
||||
|
|
|
@ -31,7 +31,7 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
if(BUILD_TESTING)
|
||||
if(BUILD_TESTING AND ECL_STANDALONE)
|
||||
add_subdirectory(base)
|
||||
|
||||
if(EKF_PYTHON_TESTS)
|
||||
|
|
|
@ -33,33 +33,35 @@
|
|||
|
||||
if(EKF_PYTHON_TESTS)
|
||||
|
||||
# pip3 install -r requirements.txt
|
||||
|
||||
add_custom_target(ecl_EKF_pytest
|
||||
COMMAND env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --verbose
|
||||
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --verbose
|
||||
DEPENDS ecl_EKF _ecl_EKF
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
add_custom_target(ecl_EKF_pytest-quick
|
||||
COMMAND env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --quick --verbose
|
||||
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --quick --verbose
|
||||
DEPENDS ecl_EKF _ecl_EKF
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
add_custom_target(ecl_EKF_pytest-benchmark
|
||||
COMMAND env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --benchmark
|
||||
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --benchmark
|
||||
DEPENDS ecl_EKF _ecl_EKF
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
# requires python3-tk
|
||||
add_custom_target(ecl_EKF_pytest-plots
|
||||
COMMAND env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --plots
|
||||
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/ekf_test.py --plots
|
||||
DEPENDS ecl_EKF _ecl_EKF
|
||||
USES_TERMINAL
|
||||
)
|
||||
|
||||
add_custom_target(ecl_EKF_pytest-lint
|
||||
COMMAND env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/lint.py
|
||||
COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${CMAKE_BINARY_DIR}/EKF/swig/python ${CMAKE_CURRENT_SOURCE_DIR}/lint.py
|
||||
DEPENDS ecl_EKF _ecl_EKF
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
USES_TERMINAL
|
||||
|
|
|
@ -84,13 +84,11 @@ struct globallocal_converter_reference_s {
|
|||
bool init_done;
|
||||
};
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/**
|
||||
* Checks if global projection was initialized
|
||||
* @return true if map was initialized before, false else
|
||||
*/
|
||||
bool map_projection_global_initialized(void);
|
||||
bool map_projection_global_initialized();
|
||||
|
||||
/**
|
||||
* Checks if projection given as argument was initialized
|
||||
|
@ -308,5 +306,3 @@ float _wrap_180(float bearing);
|
|||
float _wrap_360(float bearing);
|
||||
float _wrap_pi(float bearing);
|
||||
float _wrap_2pi(float bearing);
|
||||
|
||||
__END_DECLS
|
||||
|
|
|
@ -41,8 +41,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
// Return magnetic declination in degrees
|
||||
float get_mag_declination(float lat, float lon);
|
||||
|
||||
|
@ -51,5 +49,3 @@ float get_mag_inclination(float lat, float lon);
|
|||
|
||||
// return magnetic field strength in mTesla
|
||||
float get_mag_strength(float lat, float lon);
|
||||
|
||||
__END_DECLS
|
||||
|
|
|
@ -47,10 +47,6 @@
|
|||
#define M_PI_F 3.14159265358979323846f
|
||||
#endif
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI (3.14159265358979323846f)
|
||||
#endif
|
||||
|
||||
#ifndef M_PI_2_F
|
||||
#define M_PI_2_F (M_PI / 2.0f)
|
||||
#endif
|
||||
|
|
|
@ -40,6 +40,9 @@ using math::constrain;
|
|||
using math::max;
|
||||
using math::min;
|
||||
|
||||
static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec)
|
||||
static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec)
|
||||
|
||||
/**
|
||||
* @file tecs.cpp
|
||||
*
|
||||
|
@ -55,10 +58,9 @@ void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &ro
|
|||
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
|
||||
float altitude, bool vz_valid, float vz, float az)
|
||||
{
|
||||
|
||||
// calculate the time lapsed since the last update
|
||||
uint64_t now = ecl_absolute_time();
|
||||
float dt = max((now - _state_update_timestamp), static_cast<uint64_t>(0)) * 1.0e-6f;
|
||||
float dt = constrain((now - _state_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
|
||||
|
||||
bool reset_altitude = false;
|
||||
|
||||
|
@ -148,12 +150,7 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
|
|||
{
|
||||
// Calculate the time in seconds since the last update and use the default time step value if out of bounds
|
||||
uint64_t now = ecl_absolute_time();
|
||||
float dt = max((now - _speed_update_timestamp), UINT64_C(0)) * 1.0e-6f;
|
||||
|
||||
if (dt < DT_MIN || dt > DT_MAX) {
|
||||
dt = DT_DEFAULT;
|
||||
|
||||
}
|
||||
const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
|
||||
|
||||
// Convert equivalent airspeed quantities to true airspeed
|
||||
_EAS_setpoint = airspeed_setpoint;
|
||||
|
@ -168,14 +165,12 @@ void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspee
|
|||
|
||||
} else {
|
||||
_EAS = indicated_airspeed;
|
||||
|
||||
}
|
||||
|
||||
// If first time through or not flying, reset airspeed states
|
||||
if (_speed_update_timestamp == 0 || !_in_air) {
|
||||
_tas_rate_state = 0.0f;
|
||||
_tas_state = (_EAS * EAS2TAS);
|
||||
|
||||
}
|
||||
|
||||
// Obtain a smoothed airspeed estimate using a second order complementary filter
|
||||
|
@ -579,10 +574,9 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float
|
|||
float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
|
||||
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max)
|
||||
{
|
||||
|
||||
// Calculate the time since last update (seconds)
|
||||
uint64_t now = ecl_absolute_time();
|
||||
_dt = max((now - _pitch_update_timestamp), UINT64_C(0)) * 1.0e-6f;
|
||||
_dt = constrain((now - _pitch_update_timestamp) * 1e-6f, DT_MIN, DT_MAX);
|
||||
|
||||
// Set class variables from inputs
|
||||
_throttle_setpoint_max = throttle_max;
|
||||
|
|
|
@ -266,9 +266,7 @@ private:
|
|||
|
||||
// time steps (non-fixed)
|
||||
float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec)
|
||||
static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec)
|
||||
static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec)
|
||||
static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec)
|
||||
|
||||
// controller mode logic
|
||||
bool _underspeed_detected{false}; ///< true when an underspeed condition has been detected
|
||||
|
|
|
@ -183,9 +183,12 @@ private:
|
|||
float _rms[dimensions]; /**< root mean square error */
|
||||
float _value[dimensions]; /**< last value */
|
||||
float _vibe[dimensions]; /**< vibration level, in sensor unit */
|
||||
float _value_equal_count; /**< equal values in a row */
|
||||
float _value_equal_count_threshold; /**< when to consider an equal count as a problem */
|
||||
|
||||
unsigned _value_equal_count; /**< equal values in a row */
|
||||
unsigned _value_equal_count_threshold; /**< when to consider an equal count as a problem */
|
||||
|
||||
DataValidator *_sibling; /**< sibling in the group */
|
||||
|
||||
static const constexpr unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */
|
||||
static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
|
||||
static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */
|
||||
|
|
Loading…
Reference in New Issue