From a8bb8ea99fc92389a5cdd44cd7108416e0c49fb6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 15 Jun 2020 14:15:02 -0400 Subject: [PATCH] move TECS, L1, and validation to PX4/Firmware --- CMakeLists.txt | 3 - l1/CMakeLists.txt | 40 -- l1/ecl_l1_pos_controller.cpp | 383 ----------- l1/ecl_l1_pos_controller.h | 251 -------- tecs/CMakeLists.txt | 39 -- tecs/tecs.cpp | 608 ------------------ tecs/tecs.h | 327 ---------- tools/format.sh | 2 - validation/CMakeLists.txt | 46 -- validation/data_validator.cpp | 149 ----- validation/data_validator.h | 199 ------ validation/data_validator_group.cpp | 297 --------- validation/data_validator_group.h | 144 ----- validation/tests/CMakeLists.txt | 46 -- validation/tests/test_data_validator.cpp | 302 --------- .../tests/test_data_validator_group.cpp | 385 ----------- validation/tests/tests_common.cpp | 103 --- validation/tests/tests_common.h | 68 -- 18 files changed, 3392 deletions(-) delete mode 100644 l1/CMakeLists.txt delete mode 100644 l1/ecl_l1_pos_controller.cpp delete mode 100644 l1/ecl_l1_pos_controller.h delete mode 100644 tecs/CMakeLists.txt delete mode 100644 tecs/tecs.cpp delete mode 100644 tecs/tecs.h delete mode 100644 validation/CMakeLists.txt delete mode 100644 validation/data_validator.cpp delete mode 100644 validation/data_validator.h delete mode 100644 validation/data_validator_group.cpp delete mode 100644 validation/data_validator_group.h delete mode 100644 validation/tests/CMakeLists.txt delete mode 100644 validation/tests/test_data_validator.cpp delete mode 100644 validation/tests/test_data_validator_group.cpp delete mode 100644 validation/tests/tests_common.cpp delete mode 100644 validation/tests/tests_common.h diff --git a/CMakeLists.txt b/CMakeLists.txt index f48190f21f..6b3db82918 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -166,9 +166,6 @@ add_subdirectory(airdata) add_subdirectory(EKF) add_subdirectory(geo) add_subdirectory(geo_lookup) -add_subdirectory(l1) -add_subdirectory(tecs) -add_subdirectory(validation) if(BUILD_TESTING AND CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) add_subdirectory(test) diff --git a/l1/CMakeLists.txt b/l1/CMakeLists.txt deleted file mode 100644 index e888778755..0000000000 --- a/l1/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 ECL Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name ECL nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -add_library(ecl_l1 - ecl_l1_pos_controller.cpp - ) -add_dependencies(ecl_l1 prebuild_targets) -target_compile_definitions(ecl_l1 PRIVATE -DMODULE_NAME="ecl/l1") -target_include_directories(ecl_l1 PUBLIC ${ECL_SOURCE_DIR}) -target_link_libraries(ecl_l1 PRIVATE ecl_geo) diff --git a/l1/ecl_l1_pos_controller.cpp b/l1/ecl_l1_pos_controller.cpp deleted file mode 100644 index a9fa980af7..0000000000 --- a/l1/ecl_l1_pos_controller.cpp +++ /dev/null @@ -1,383 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_l1_pos_controller.h - * Implementation of L1 position control. - * Authors and acknowledgements in header. - * - */ - -#include - -#include "ecl_l1_pos_controller.h" - -using matrix::Vector2f; -using matrix::wrap_pi; - - -void ECL_L1_Pos_Controller::update_roll_setpoint() -{ - float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G); - roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad); - - if (_dt > 0.0f && _roll_slew_rate > 0.0f) { - // slew rate limiting active - roll_new = math::constrain(roll_new, _roll_setpoint - _roll_slew_rate * _dt, _roll_setpoint + _roll_slew_rate * _dt); - } - - if (ISFINITE(roll_new)) { - _roll_setpoint = roll_new; - } - -} - -float ECL_L1_Pos_Controller::switch_distance(float wp_radius) -{ - /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); -} - -void -ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, - const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) -{ - /* this follows the logic presented in [1] */ - float eta = 0.0f; - float xtrack_vel = 0.0f; - float ltrack_vel = 0.0f; - - /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), (double)vector_B(0), (double)vector_B(1)); - - /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length(), 0.1f); - - /* calculate the L1 length required for the desired period */ - _L1_distance = _L1_ratio * ground_speed; - - /* calculate vector from A to B */ - Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); - - /* - * check if waypoints are on top of each other. If yes, - * skip A and directly continue to B - */ - if (vector_AB.length() < 1.0e-6f) { - vector_AB = get_local_planar_vector(vector_curr_position, vector_B); - } - - vector_AB.normalize(); - - /* calculate the vector from waypoint A to the aircraft */ - Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - - /* calculate crosstrack error (output only) */ - _crosstrack_error = vector_AB % vector_A_to_airplane; - - /* - * If the current position is in a +-135 degree angle behind waypoint A - * and further away from A than the L1 distance, then A becomes the L1 point. - * If the aircraft is already between A and B normal L1 logic is applied. - */ - float distance_A_to_airplane = vector_A_to_airplane.length(); - float alongTrackDist = vector_A_to_airplane * vector_AB; - - /* estimate airplane position WRT to B */ - Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); - - /* calculate angle of airplane position vector relative to line) */ - - // XXX this could probably also be based solely on the dot product - float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); - - /* extension from [2], fly directly to A */ - if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) { - - /* calculate eta to fly to waypoint A */ - - /* unit vector from waypoint A to current position */ - Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); - /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); - eta = atan2f(xtrack_vel, ltrack_vel); - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); - - /* - * If the AB vector and the vector from B to airplane point in the same - * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. - */ - - } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { - /* - * Extension, fly back to waypoint. - * - * This corner case is possible if the system was following - * the AB line from waypoint A to waypoint B, then is - * switched to manual mode (or otherwise misses the waypoint) - * and behind the waypoint continues to follow the AB line. - */ - - /* calculate eta to fly to waypoint B */ - - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); - /* velocity along line */ - ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); - eta = atan2f(xtrack_vel, ltrack_vel); - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0)); - - } else { - - /* calculate eta to fly along the line between A and B */ - - /* velocity across / orthogonal to line */ - xtrack_vel = ground_speed_vector % vector_AB; - /* velocity along line */ - ltrack_vel = ground_speed_vector * vector_AB; - /* calculate eta2 (angle of velocity vector relative to line) */ - float eta2 = atan2f(xtrack_vel, ltrack_vel); - /* calculate eta1 (angle to L1 point) */ - float xtrackErr = vector_A_to_airplane % vector_AB; - float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f); - /* limit output to 45 degrees */ - sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 - float eta1 = asinf(sine_eta1); - eta = eta1 + eta2; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; - - } - - /* limit angle to +-90 degrees */ - eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); - _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); - - /* flying to waypoints, not circling them */ - _circle_mode = false; - - /* the bearing angle, in NED frame */ - _bearing_error = eta; - - update_roll_setpoint(); -} - -void -ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius, - int8_t loiter_direction, const Vector2f &ground_speed_vector) -{ - /* the complete guidance logic in this section was proposed by [2] */ - - /* calculate the gains for the PD loop (circle tracking) */ - float omega = (2.0f * M_PI_F / _L1_period); - float K_crosstrack = omega * omega; - float K_velocity = 2.0f * _L1_damping * omega; - - /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), (double)vector_A(0), (double)vector_A(1)); - - /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ - float ground_speed = math::max(ground_speed_vector.length(), 0.1f); - - /* calculate the L1 length required for the desired period */ - _L1_distance = _L1_ratio * ground_speed; - - /* calculate the vector from waypoint A to current position */ - Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); - - Vector2f vector_A_to_airplane_unit; - - /* prevent NaN when normalizing */ - if (vector_A_to_airplane.length() > FLT_EPSILON) { - /* store the normalized vector from waypoint A to current position */ - vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); - - } else { - vector_A_to_airplane_unit = vector_A_to_airplane; - } - - /* calculate eta angle towards the loiter center */ - - /* velocity across / orthogonal to line from waypoint to current position */ - float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; - /* velocity along line from waypoint to current position */ - float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); - float eta = atan2f(xtrack_vel_center, ltrack_vel_center); - /* limit eta to 90 degrees */ - eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); - - /* calculate the lateral acceleration to capture the center point */ - float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); - - /* for PD control: Calculate radial position and velocity errors */ - - /* radial velocity error */ - float xtrack_vel_circle = -ltrack_vel_center; - /* radial distance from the loiter circle (not center) */ - float xtrack_err_circle = vector_A_to_airplane.length() - radius; - - /* cross track error for feedback */ - _crosstrack_error = xtrack_err_circle; - - /* calculate PD update to circle waypoint */ - float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); - - /* calculate velocity on circle / along tangent */ - float tangent_vel = xtrack_vel_center * loiter_direction; - - /* prevent PD output from turning the wrong way */ - if (tangent_vel < 0.0f) { - lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f); - } - - /* calculate centripetal acceleration setpoint */ - float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius), - (radius + xtrack_err_circle)); - - /* add PD control on circle and centripetal acceleration for total circle command */ - float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); - - /* - * Switch between circle (loiter) and capture (towards waypoint center) mode when - * the commands switch over. Only fly towards waypoint if outside the circle. - */ - - // XXX check switch over - if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || - (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { - _lateral_accel = lateral_accel_sp_center; - _circle_mode = false; - /* angle between requested and current velocity vector */ - _bearing_error = eta; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); - - } else { - _lateral_accel = lateral_accel_sp_circle; - _circle_mode = true; - _bearing_error = 0.0f; - /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); - } - - update_roll_setpoint(); -} - -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, - const Vector2f &ground_speed_vector) -{ - /* the complete guidance logic in this section was proposed by [2] */ - - /* - * As the commanded heading is the only reference - * (and no crosstrack correction occurs), - * target and navigation bearing become the same - */ - _target_bearing = _nav_bearing = wrap_pi(navigation_heading); - - float eta = wrap_pi(_target_bearing - wrap_pi(current_heading)); - - /* consequently the bearing error is exactly eta: */ - _bearing_error = eta; - - /* ground speed is the length of the ground speed vector */ - float ground_speed = ground_speed_vector.length(); - - /* adjust L1 distance to keep constant frequency */ - _L1_distance = ground_speed / _heading_omega; - float omega_vel = ground_speed * _heading_omega; - - /* not circling a waypoint */ - _circle_mode = false; - - /* navigating heading means by definition no crosstrack error */ - _crosstrack_error = 0; - - /* limit eta to 90 degrees */ - eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); - _lateral_accel = 2.0f * sinf(eta) * omega_vel; - - update_roll_setpoint(); -} - -void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) -{ - /* the logic in this section is trivial, but originally proposed by [2] */ - - /* reset all heading / error measures resulting in zero roll */ - _target_bearing = current_heading; - _nav_bearing = current_heading; - _bearing_error = 0; - _crosstrack_error = 0; - _lateral_accel = 0; - - /* not circling a waypoint when flying level */ - _circle_mode = false; - - update_roll_setpoint(); -} - -Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const -{ - /* this is an approximation for small angles, proposed by [2] */ - Vector2f out(math::radians((target(0) - origin(0))), - math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); - - return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); -} - -void ECL_L1_Pos_Controller::set_l1_period(float period) -{ - _L1_period = period; - - /* calculate the ratio introduced in [2] */ - _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; - - /* calculate normalized frequency for heading tracking */ - _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; -} - -void ECL_L1_Pos_Controller::set_l1_damping(float damping) -{ - _L1_damping = damping; - - /* calculate the ratio introduced in [2] */ - _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; - - /* calculate the L1 gain (following [2]) */ - _K_L1 = 4.0f * _L1_damping * _L1_damping; -} diff --git a/l1/ecl_l1_pos_controller.h b/l1/ecl_l1_pos_controller.h deleted file mode 100644 index 610a54d503..0000000000 --- a/l1/ecl_l1_pos_controller.h +++ /dev/null @@ -1,251 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ecl_l1_pos_control.h - * Implementation of L1 position control. - * - * - * Acknowledgements and References: - * - * This implementation has been built for PX4 based on the original - * publication from [1] and does include a lot of the ideas (not code) - * from [2]. - * - * - * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," - * Proceedings of the AIAA Guidance, Navigation and Control - * Conference, Aug 2004. AIAA-2004-4900. - * - * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. - * - Explicit control over frequency and damping - * - Explicit control over track capture angle - * - Ability to use loiter radius smaller than L1 length - * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length - * - Modified to enable period and damping of guidance loop to be set explicitly - * - Modified to provide explicit control over capture angle - * - */ - -#ifndef ECL_L1_POS_CONTROLLER_H -#define ECL_L1_POS_CONTROLLER_H - -#include -#include -#include -#include - -/** - * L1 Nonlinear Guidance Logic - */ -class ECL_L1_Pos_Controller -{ -public: - /** - * The current target bearing - * - * @return bearing angle (-pi..pi, in NED frame) - */ - float nav_bearing() { return matrix::wrap_pi(_nav_bearing); } - - /** - * Get lateral acceleration demand. - * - * @return Lateral acceleration in m/s^2 - */ - float nav_lateral_acceleration_demand() { return _lateral_accel; } - - /** - * Heading error. - * - * The heading error is either compared to the current track - * or to the tangent of the current loiter radius. - */ - float bearing_error() { return _bearing_error; } - - /** - * Bearing from aircraft to current target. - * - * @return bearing angle (-pi..pi, in NED frame) - */ - float target_bearing() { return _target_bearing; } - - /** - * Get roll angle setpoint for fixed wing. - * - * @return Roll angle (in NED frame) - */ - float get_roll_setpoint(){ return _roll_setpoint; } - - /** - * Get the current crosstrack error. - * - * @return Crosstrack error in meters. - */ - float crosstrack_error() { return _crosstrack_error; } - - /** - * Returns true if the loiter waypoint has been reached - */ - bool reached_loiter_target() { return _circle_mode; } - - /** - * Returns true if following a circle (loiter) - */ - bool circle_mode() { return _circle_mode; } - - /** - * Get the switch distance - * - * This is the distance at which the system will - * switch to the next waypoint. This depends on the - * period and damping - * - * @param waypoint_switch_radius The switching radius the waypoint has set. - */ - float switch_distance(float waypoint_switch_radius); - - /** - * Navigate between two waypoints - * - * Calling this function with two waypoints results in the - * control outputs to fly to the line segment defined by - * the points and once captured following the line segment. - * This follows the logic in [1]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, - const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed); - - /** - * Navigate on an orbit around a loiter waypoint. - * - * This allow orbits smaller than the L1 length, - * this modification was introduced in [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius, - int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector); - - /** - * Navigate on a fixed bearing. - * - * This only holds a certain direction and does not perform cross - * track correction. Helpful for semi-autonomous modes. Introduced - * by [2]. - * - * @return sets _lateral_accel setpoint - */ - void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed); - - /** - * Keep the wings level. - * - * This is typically needed for maximum-lift-demand situations, - * such as takeoff or near stall. Introduced in [2]. - */ - void navigate_level_flight(float current_heading); - - /** - * Set the L1 period. - */ - void set_l1_period(float period); - - /** - * Set the L1 damping factor. - * - * The original publication recommends a default of sqrt(2) / 2 = 0.707 - */ - void set_l1_damping(float damping); - - /** - * Set the maximum roll angle output in radians - */ - void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; } - - /** - * Set roll angle slew rate. Set to zero to deactivate. - */ - void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; } - - /** - * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting. - */ - void set_dt(float dt) { _dt = dt;} - -private: - - float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2 - float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping - bool _circle_mode{false}; ///< flag for loiter mode - float _nav_bearing{0.0f}; ///< bearing to L1 reference point - float _bearing_error{0.0f}; ///< bearing error - float _crosstrack_error{0.0f}; ///< crosstrack error in meters - float _target_bearing{0.0f}; ///< the heading setpoint - - float _L1_period{25.0f}; ///< L1 tracking period in seconds - float _L1_damping{0.75f}; ///< L1 damping ratio - float _L1_ratio{5.0f}; ///< L1 ratio for navigation - float _K_L1{2.0f}; ///< L1 control gain for _L1_damping - float _heading_omega{1.0f}; ///< Normalized frequency - - float _roll_lim_rad{math::radians(30.0f)}; /// -#include - -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 - * - * @author Paul Riseborough - */ - -/* - * This function implements a complementary filter to estimate the climb rate when - * inertial nav data is not available. It also calculates a true airspeed derivative - * which is used by the airspeed complimentary filter. - */ -void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, - const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, - float altitude, float vz) -{ - // calculate the time lapsed since the last update - uint64_t now = ecl_absolute_time(); - float dt = constrain((now - _state_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX); - - bool reset_altitude = false; - - if (_state_update_timestamp == 0 || dt > DT_MAX) { - dt = DT_DEFAULT; - reset_altitude = true; - } - - if (!altitude_lock || !in_air) { - reset_altitude = true; - } - - if (reset_altitude) { - _states_initialized = false; - } - - _state_update_timestamp = now; - _EAS = airspeed; - - _in_air = in_air; - - // Set the velocity and position state to the the INS data - _vert_vel_state = -vz; - _vert_pos_state = altitude; - - // Update and average speed rate of change if airspeed is being measured - if (ISFINITE(airspeed) && airspeed_sensor_enabled()) { - // Assuming the vehicle is flying X axis forward, use the X axis measured acceleration - // compensated for gravity to estimate the rate of change of speed - float speed_deriv_raw = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0); - - // Apply some noise filtering - _speed_derivative = 0.95f * _speed_derivative + 0.05f * speed_deriv_raw; - - } else { - _speed_derivative = 0.0f; - } - - if (!_in_air) { - _states_initialized = false; - } - -} - -void TECS::_update_speed_states(float airspeed_setpoint, float indicated_airspeed, float EAS2TAS) -{ - // 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(); - 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; - _TAS_setpoint = _EAS_setpoint * EAS2TAS; - _TAS_max = _indicated_airspeed_max * EAS2TAS; - _TAS_min = _indicated_airspeed_min * EAS2TAS; - - // If airspeed measurements are not being used, fix the airspeed estimate to halfway between - // min and max limits - if (!ISFINITE(indicated_airspeed) || !airspeed_sensor_enabled()) { - _EAS = 0.5f * (_indicated_airspeed_min + _indicated_airspeed_max); - - } 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 - - // Update TAS rate state - float tas_error = (_EAS * EAS2TAS) - _tas_state; - float tas_rate_state_input = tas_error * _tas_estimate_freq * _tas_estimate_freq; - - // limit integrator input to prevent windup - if (_tas_state < 3.1f) { - tas_rate_state_input = max(tas_rate_state_input, 0.0f); - - } - - // Update TAS state - _tas_rate_state = _tas_rate_state + tas_rate_state_input * dt; - float tas_state_input = _tas_rate_state + _speed_derivative + tas_error * _tas_estimate_freq * 1.4142f; - _tas_state = _tas_state + tas_state_input * dt; - - // Limit the airspeed state to a minimum of 3 m/s - _tas_state = max(_tas_state, 3.0f); - _speed_update_timestamp = now; - -} - -void TECS::_update_speed_setpoint() -{ - // Set the airspeed demand to the minimum value if an underspeed or - // or a uncontrolled descent condition exists to maximise climb rate - if ((_uncommanded_descent_recovery) || (_underspeed_detected)) { - _TAS_setpoint = _TAS_min; - } - - _TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max); - - // Calculate limits for the demanded rate of change of speed based on physical performance limits - // with a 50% margin to allow the total energy controller to correct for errors. - float velRateMax = 0.5f * _STE_rate_max / _tas_state; - float velRateMin = 0.5f * _STE_rate_min / _tas_state; - - _TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max); - - // calculate the demanded rate of change of speed proportional to speed error - // and apply performance limits - _TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _speed_error_gain, velRateMin, velRateMax); - -} - -void TECS::_update_height_setpoint(float desired, float state) -{ - // Detect first time through and initialize previous value to demand - if (ISFINITE(desired) && fabsf(_hgt_setpoint_in_prev) < 0.1f) { - _hgt_setpoint_in_prev = desired; - } - - // Apply a 2 point moving average to demanded height to reduce - // intersampling noise effects. - if (ISFINITE(desired)) { - _hgt_setpoint = 0.5f * (desired + _hgt_setpoint_in_prev); - - } else { - _hgt_setpoint = _hgt_setpoint_in_prev; - } - - _hgt_setpoint_in_prev = _hgt_setpoint; - - // Apply a rate limit to respect vehicle performance limitations - if ((_hgt_setpoint - _hgt_setpoint_prev) > (_max_climb_rate * _dt)) { - _hgt_setpoint = _hgt_setpoint_prev + _max_climb_rate * _dt; - - } else if ((_hgt_setpoint - _hgt_setpoint_prev) < (-_max_sink_rate * _dt)) { - _hgt_setpoint = _hgt_setpoint_prev - _max_sink_rate * _dt; - } - - _hgt_setpoint_prev = _hgt_setpoint; - - // Apply a first order noise filter - _hgt_setpoint_adj = 0.1f * _hgt_setpoint + 0.9f * _hgt_setpoint_adj_prev; - - // Calculate the demanded climb rate proportional to height error plus a feedforward term to provide - // tight tracking during steady climb and descent manoeuvres. - _hgt_rate_setpoint = (_hgt_setpoint_adj - state) * _height_error_gain + _height_setpoint_gain_ff * - (_hgt_setpoint_adj - _hgt_setpoint_adj_prev) / _dt; - _hgt_setpoint_adj_prev = _hgt_setpoint_adj; - - // Limit the rate of change of height demand to respect vehicle performance limits - if (_hgt_rate_setpoint > _max_climb_rate) { - _hgt_rate_setpoint = _max_climb_rate; - - } else if (_hgt_rate_setpoint < -_max_sink_rate) { - _hgt_rate_setpoint = -_max_sink_rate; - } -} - -void TECS::_detect_underspeed() -{ - if (!_detect_underspeed_enabled) { - _underspeed_detected = false; - return; - } - - if (((_tas_state < _TAS_min * 0.9f) && (_throttle_setpoint >= _throttle_setpoint_max * 0.95f)) - || ((_vert_pos_state < _hgt_setpoint_adj) && _underspeed_detected)) { - - _underspeed_detected = true; - - } else { - _underspeed_detected = false; - } -} - -void TECS::_update_energy_estimates() -{ - // Calculate specific energy demands in units of (m**2/sec**2) - _SPE_setpoint = _hgt_setpoint_adj * CONSTANTS_ONE_G; // potential energy - _SKE_setpoint = 0.5f * _TAS_setpoint_adj * _TAS_setpoint_adj; // kinetic energy - - // Calculate specific energy rate demands in units of (m**2/sec**3) - _SPE_rate_setpoint = _hgt_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change - _SKE_rate_setpoint = _tas_state * _TAS_rate_setpoint; // kinetic energy rate of change - - // Calculate specific energies in units of (m**2/sec**2) - _SPE_estimate = _vert_pos_state * CONSTANTS_ONE_G; // potential energy - _SKE_estimate = 0.5f * _tas_state * _tas_state; // kinetic energy - - // Calculate specific energy rates in units of (m**2/sec**3) - _SPE_rate = _vert_vel_state * CONSTANTS_ONE_G; // potential energy rate of change - _SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change -} - -void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat) -{ - // Calculate total energy error - _STE_error = _SPE_setpoint - _SPE_estimate + _SKE_setpoint - _SKE_estimate; - - // Calculate demanded rate of change of total energy, respecting vehicle limits - float STE_rate_setpoint = constrain((_SPE_rate_setpoint + _SKE_rate_setpoint), _STE_rate_min, _STE_rate_max); - - // Calculate the total energy rate error, applying a first order IIR filter - // to reduce the effect of accelerometer noise - _STE_rate_error = 0.2f * (STE_rate_setpoint - _SPE_rate - _SKE_rate) + 0.8f * _STE_rate_error; - - // Calculate the throttle demand - if (_underspeed_detected) { - // always use full throttle to recover from an underspeed condition - _throttle_setpoint = 1.0f; - - } else { - // Adjust the demanded total energy rate to compensate for induced drag rise in turns. - // Assume induced drag scales linearly with normal load factor. - // The additional normal load factor is given by (1/cos(bank angle) - 1) - float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))); - STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / constrain(cosPhi, 0.1f, 1.0f) - 1.0f); - - // Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle - // as the starting point. Assume: - // Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max - // Specific total energy rate = 0 at cruise throttle - // Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min - float throttle_predicted = 0.0f; - - if (STE_rate_setpoint >= 0) { - // throttle is between cruise and maximum - throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise); - - } else { - // throttle is between cruise and minimum - throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise); - - } - - // Calculate gain scaler from specific energy error to throttle - float STE_to_throttle = 1.0f / (_throttle_time_constant * (_STE_rate_max - _STE_rate_min)); - - // Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits - _throttle_setpoint = (_STE_error + _STE_rate_error * _throttle_damping_gain) * STE_to_throttle + throttle_predicted; - _throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); - - // Rate limit the throttle demand - if (fabsf(_throttle_slewrate) > 0.01f) { - float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate; - _throttle_setpoint = constrain(_throttle_setpoint, _last_throttle_setpoint - throttle_increment_limit, - _last_throttle_setpoint + throttle_increment_limit); - } - - _last_throttle_setpoint = _throttle_setpoint; - - if (_integrator_gain > 0.0f) { - // Calculate throttle integrator state upper and lower limits with allowance for - // 10% throttle saturation to accommodate noise on the demand. - float integ_state_max = _throttle_setpoint_max - _throttle_setpoint + 0.1f; - float integ_state_min = _throttle_setpoint_min - _throttle_setpoint - 0.1f; - - // Calculate a throttle demand from the integrated total energy error - // This will be added to the total throttle demand to compensate for steady state errors - _throttle_integ_state = _throttle_integ_state + (_STE_error * _integrator_gain) * _dt * STE_to_throttle; - - if (_climbout_mode_active) { - // During climbout, set the integrator to maximum throttle to prevent transient throttle drop - // at end of climbout when we transition to closed loop throttle control - _throttle_integ_state = integ_state_max; - - } else { - // Respect integrator limits during closed loop operation. - _throttle_integ_state = constrain(_throttle_integ_state, integ_state_min, integ_state_max); - } - - } else { - _throttle_integ_state = 0.0f; - } - - if (airspeed_sensor_enabled()) { - // Add the integrator feedback during closed loop operation with an airspeed sensor - _throttle_setpoint = _throttle_setpoint + _throttle_integ_state; - - } else { - // when flying without an airspeed sensor, use the predicted throttle only - _throttle_setpoint = throttle_predicted; - - } - - _throttle_setpoint = constrain(_throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); - } -} - -void TECS::_detect_uncommanded_descent() -{ - /* - * This function detects a condition that can occur when the demanded airspeed is greater than the - * aircraft can achieve in level flight. When this occurs, the vehicle will continue to reduce height - * while attempting to maintain speed. - */ - - // Calculate rate of change of total specific energy - float STE_rate = _SPE_rate + _SKE_rate; - - // If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode - bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f) && (STE_rate < 0.0f) - && (_throttle_setpoint >= _throttle_setpoint_max * 0.9f); - - // If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode - bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f)); - - if (enter_mode) { - _uncommanded_descent_recovery = true; - - } else if (exit_mode) { - _uncommanded_descent_recovery = false; - - } -} - -void TECS::_update_pitch_setpoint() -{ - /* - * The SKE_weighting variable controls how speed and height control are prioritised by the pitch demand calculation. - * A weighting of 1 givea equal speed and height priority - * A weighting of 0 gives 100% priority to height control and must be used when no airspeed measurement is available. - * A weighting of 2 provides 100% priority to speed control and is used when: - * a) an underspeed condition is detected. - * b) during climbout where a minimum pitch angle has been set to ensure height is gained. If the airspeed - * rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding. - * The weighting can be adjusted between 0 and 2 depending on speed and height accuracy requirements. - */ - - // Calculate the weighting applied to control of specific kinetic energy error - float SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f); - - if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) { - SKE_weighting = 2.0f; - - } else if (!airspeed_sensor_enabled()) { - SKE_weighting = 0.0f; - } - - // Calculate the weighting applied to control of specific potential energy error - float SPE_weighting = 2.0f - SKE_weighting; - - // Calculate the specific energy balance demand which specifies how the available total - // energy should be allocated to speed (kinetic energy) and height (potential energy) - float SEB_setpoint = _SPE_setpoint * SPE_weighting - _SKE_setpoint * SKE_weighting; - - // Calculate the specific energy balance rate demand - float SEB_rate_setpoint = _SPE_rate_setpoint * SPE_weighting - _SKE_rate_setpoint * SKE_weighting; - - // Calculate the specific energy balance and balance rate error - _SEB_error = SEB_setpoint - (_SPE_estimate * SPE_weighting - _SKE_estimate * SKE_weighting); - _SEB_rate_error = SEB_rate_setpoint - (_SPE_rate * SPE_weighting - _SKE_rate * SKE_weighting); - - // Calculate derivative from change in climb angle to rate of change of specific energy balance - float climb_angle_to_SEB_rate = _tas_state * _pitch_time_constant * CONSTANTS_ONE_G; - - if (_integrator_gain > 0.0f) { - // Calculate pitch integrator input term - float pitch_integ_input = _SEB_error * _integrator_gain; - - // Prevent the integrator changing in a direction that will increase pitch demand saturation - // Decay the integrator at the control loop time constant if the pitch demand from the previous time step is saturated - if (_pitch_setpoint_unc > _pitch_setpoint_max) { - pitch_integ_input = min(pitch_integ_input, - min((_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f)); - - } else if (_pitch_setpoint_unc < _pitch_setpoint_min) { - pitch_integ_input = max(pitch_integ_input, - max((_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate / _pitch_time_constant, 0.0f)); - } - - // Update the pitch integrator state. - _pitch_integ_state = _pitch_integ_state + pitch_integ_input * _dt; - - } else { - _pitch_integ_state = 0.0f; - } - - // Calculate a specific energy correction that doesn't include the integrator contribution - float SEB_correction = _SEB_error + _SEB_rate_error * _pitch_damping_gain + SEB_rate_setpoint * _pitch_time_constant; - - // During climbout, bias the demanded pitch angle so that a zero speed error produces a pitch angle - // demand equal to the minimum pitch angle set by the mission plan. This prevents the integrator - // having to catch up before the nose can be raised to reduce excess speed during climbout. - if (_climbout_mode_active) { - SEB_correction += _pitch_setpoint_min * climb_angle_to_SEB_rate; - } - - // Sum the correction terms and convert to a pitch angle demand. This calculation assumes: - // a) The climb angle follows pitch angle with a lag that is small enough not to destabilise the control loop. - // b) The offset between climb angle and pitch angle (angle of attack) is constant, excluding the effect of - // pitch transients due to control action or turbulence. - _pitch_setpoint_unc = (SEB_correction + _pitch_integ_state) / climb_angle_to_SEB_rate; - _pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max); - - // Comply with the specified vertical acceleration limit by applying a pitch rate limit - float ptchRateIncr = _dt * _vert_accel_limit / _tas_state; - - if ((_pitch_setpoint - _last_pitch_setpoint) > ptchRateIncr) { - _pitch_setpoint = _last_pitch_setpoint + ptchRateIncr; - - } else if ((_pitch_setpoint - _last_pitch_setpoint) < -ptchRateIncr) { - _pitch_setpoint = _last_pitch_setpoint - ptchRateIncr; - } - - _last_pitch_setpoint = _pitch_setpoint; -} - -void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout, - float EAS2TAS) -{ - if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) { - // On first time through or when not using TECS of if there has been a large time slip, - // states must be reset to allow filters to a clean start - _vert_vel_state = 0.0f; - _vert_pos_state = baro_altitude; - _tas_rate_state = 0.0f; - _tas_state = _EAS * EAS2TAS; - _throttle_integ_state = 0.0f; - _pitch_integ_state = 0.0f; - _last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);; - _last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max); - _pitch_setpoint_unc = _last_pitch_setpoint; - _hgt_setpoint_adj_prev = baro_altitude; - _hgt_setpoint_adj = _hgt_setpoint_adj_prev; - _hgt_setpoint_prev = _hgt_setpoint_adj_prev; - _hgt_setpoint_in_prev = _hgt_setpoint_adj_prev; - _TAS_setpoint_last = _EAS * EAS2TAS; - _TAS_setpoint_adj = _TAS_setpoint_last; - _underspeed_detected = false; - _uncommanded_descent_recovery = false; - _STE_rate_error = 0.0f; - - if (_dt > DT_MAX || _dt < DT_MIN) { - _dt = DT_DEFAULT; - } - - } else if (_climbout_mode_active) { - // During climbout use the lower pitch angle limit specified by the - // calling controller - _pitch_setpoint_min = pitch_min_climbout; - - // throttle lower limit is set to a value that prevents throttle reduction - _throttle_setpoint_min = _throttle_setpoint_max - 0.01f; - - // height demand and associated states are set to track the measured height - _hgt_setpoint_adj_prev = baro_altitude; - _hgt_setpoint_adj = _hgt_setpoint_adj_prev; - _hgt_setpoint_prev = _hgt_setpoint_adj_prev; - - // airspeed demand states are set to track the measured airspeed - _TAS_setpoint_last = _EAS * EAS2TAS; - _TAS_setpoint_adj = _EAS * EAS2TAS; - - // disable speed and decent error condition checks - _underspeed_detected = false; - _uncommanded_descent_recovery = false; - } - - _states_initialized = true; -} - -void TECS::_update_STE_rate_lim() -{ - // Calculate the specific total energy upper rate limits from the max throttle climb rate - _STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G; - - // Calculate the specific total energy lower rate limits from the min throttle sink rate - _STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G; -} - -void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, - 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 = constrain((now - _pitch_update_timestamp) * 1e-6f, DT_MIN, DT_MAX); - - // Set class variables from inputs - _throttle_setpoint_max = throttle_max; - _throttle_setpoint_min = throttle_min; - _pitch_setpoint_max = pitch_limit_max; - _pitch_setpoint_min = pitch_limit_min; - _climbout_mode_active = climb_out_setpoint; - - // Initialize selected states and variables as required - _initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas); - - // Don't run TECS control algorithms when not in flight - if (!_in_air) { - return; - } - - // Update the true airspeed state estimate - _update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas); - - // Calculate rate limits for specific total energy - _update_STE_rate_lim(); - - // Detect an underspeed condition - _detect_underspeed(); - - // Detect an uncommanded descent caused by an unachievable airspeed demand - _detect_uncommanded_descent(); - - // Calculate the demanded true airspeed - _update_speed_setpoint(); - - // Calculate the demanded height - _update_height_setpoint(hgt_setpoint, baro_altitude); - - // Calculate the specific energy values required by the control loop - _update_energy_estimates(); - - // Calculate the throttle demand - _update_throttle_setpoint(throttle_cruise, rotMat); - - // Calculate the pitch demand - _update_pitch_setpoint(); - - // Update time stamps - _pitch_update_timestamp = now; - - // Set TECS mode for next frame - if (_underspeed_detected) { - _tecs_mode = ECL_TECS_MODE_UNDERSPEED; - - } else if (_uncommanded_descent_recovery) { - _tecs_mode = ECL_TECS_MODE_BAD_DESCENT; - - } else if (_climbout_mode_active) { - _tecs_mode = ECL_TECS_MODE_CLIMBOUT; - - } else { - // This is the default operation mode - _tecs_mode = ECL_TECS_MODE_NORMAL; - } - -} diff --git a/tecs/tecs.h b/tecs/tecs.h deleted file mode 100644 index 03660564b4..0000000000 --- a/tecs/tecs.h +++ /dev/null @@ -1,327 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2017 Estimation and Control Library (ECL). All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name ECL nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file tecs.cpp - * - * @author Paul Riseborough - */ - -#pragma once - -#include -#include - -class TECS -{ -public: - TECS() = default; - ~TECS() = default; - - // no copy, assignment, move, move assignment - TECS(const TECS &) = delete; - TECS &operator=(const TECS &) = delete; - TECS(TECS &&) = delete; - TECS &operator=(TECS &&) = delete; - - /** - * Get the current airspeed status - * - * @return true if airspeed is enabled for control - */ - bool airspeed_sensor_enabled() { return _airspeed_enabled; } - - /** - * Set the airspeed enable state - */ - void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; } - - /** - * Updates the following vehicle kineamtic state estimates: - * Vertical position, velocity and acceleration. - * Speed derivative - * Must be called prior to udating tecs control loops - * Must be called at 50Hz or greater - */ - void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, - const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, - float altitude, float vz); - - /** - * Update the control loop calculations - */ - void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, - float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, - float throttle_min, float throttle_setpoint_max, float throttle_cruise, - float pitch_limit_min, float pitch_limit_max); - - float get_throttle_setpoint(void) { return _throttle_setpoint; } - float get_pitch_setpoint() { return _pitch_setpoint; } - float get_speed_weight() { return _pitch_speed_weight; } - - void reset_state() { _states_initialized = false; } - - enum ECL_TECS_MODE { - ECL_TECS_MODE_NORMAL = 0, - ECL_TECS_MODE_UNDERSPEED, - ECL_TECS_MODE_BAD_DESCENT, - ECL_TECS_MODE_CLIMBOUT - }; - - void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; } - - // setters for controller parameters - void set_time_const(float time_const) { _pitch_time_constant = time_const; } - void set_integrator_gain(float gain) { _integrator_gain = gain; } - - void set_min_sink_rate(float rate) { _min_sink_rate = rate; } - void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; } - void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; } - - void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; } - void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; } - - void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; } - void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; } - - void set_pitch_damping(float damping) { _pitch_damping_gain = damping; } - void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } - - void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; } - void set_speed_weight(float weight) { _pitch_speed_weight = weight; } - void set_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; } - - void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; } - void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; } - void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; } - - void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; } - - // TECS status - uint64_t timestamp() { return _pitch_update_timestamp; } - ECL_TECS_MODE tecs_mode() { return _tecs_mode; } - - float hgt_setpoint_adj() { return _hgt_setpoint_adj; } - float vert_pos_state() { return _vert_pos_state; } - - float TAS_setpoint_adj() { return _TAS_setpoint_adj; } - float tas_state() { return _tas_state; } - - float hgt_rate_setpoint() { return _hgt_rate_setpoint; } - float vert_vel_state() { return _vert_vel_state; } - - float TAS_rate_setpoint() { return _TAS_rate_setpoint; } - float speed_derivative() { return _speed_derivative; } - - float STE_error() { return _STE_error; } - float STE_rate_error() { return _STE_rate_error; } - - float SEB_error() { return _SEB_error; } - float SEB_rate_error() { return _SEB_rate_error; } - - float throttle_integ_state() { return _throttle_integ_state; } - float pitch_integ_state() { return _pitch_integ_state; } - - /** - * Handle the altitude reset - * - * If the estimation system resets the height in one discrete step this - * will gracefully even out the reset over time. - */ - void handle_alt_step(float delta_alt, float altitude) - { - // add height reset delta to all variables involved - // in filtering the demanded height - _hgt_setpoint_in_prev += delta_alt; - _hgt_setpoint_prev += delta_alt; - _hgt_setpoint_adj_prev += delta_alt; - - // reset height states - _vert_pos_state = altitude; - _vert_vel_state = 0.0f; - } - -private: - - enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; - - // timestamps - uint64_t _state_update_timestamp{0}; ///< last timestamp of the 50 Hz function call - uint64_t _speed_update_timestamp{0}; ///< last timestamp of the speed function call - uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call - - // controller parameters - float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec) - float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec) - float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec) - float _max_sink_rate{2.0f}; ///< maximum safe sink rate (m/sec) - float _pitch_time_constant{5.0f}; ///< control time constant used by the pitch demand calculation (sec) - float _throttle_time_constant{8.0f}; ///< control time constant used by the throttle demand calculation (sec) - float _pitch_damping_gain{0.0f}; ///< damping gain of the pitch demand calculation (sec) - float _throttle_damping_gain{0.0f}; ///< damping gain of the throttle demand calculation (sec) - float _integrator_gain{0.0f}; ///< integrator gain used by the throttle and pitch demand calculation - float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2) - float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3) - float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation - float _height_error_gain{0.0f}; ///< gain from height error to demanded climb rate (1/sec) - float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate - float _speed_error_gain{0.0f}; ///< gain from speed error to demanded speed rate (1/sec) - float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) - float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) - float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec) - - // controller outputs - float _throttle_setpoint{0.0f}; ///< normalized throttle demand (0..1) - float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians) - - // complimentary filter states - float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) - float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m) - float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2) - float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec) - - // controller states - float _throttle_integ_state{0.0f}; ///< throttle integrator state - float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad) - float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec) - float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec) - float _speed_derivative{0.0f}; ///< rate of change of speed along X axis (m/sec**2) - - // speed demand calculations - float _EAS{0.0f}; ///< equivalent airspeed (m/sec) - float _TAS_max{30.0f}; ///< true airpeed demand upper limit (m/sec) - float _TAS_min{3.0f}; ///< true airpeed demand lower limit (m/sec) - float _TAS_setpoint{0.0f}; ///< current airpeed demand (m/sec) - float _TAS_setpoint_last{0.0f}; ///< previous true airpeed demand (m/sec) - float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec) - float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec) - float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2) - - // height demand calculations - float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m) - float _hgt_setpoint_in_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering (m) - float _hgt_setpoint_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering and rate limiting (m) - float _hgt_setpoint_adj{0.0f}; ///< demanded height used by the control loops after all filtering has been applied (m) - float _hgt_setpoint_adj_prev{0.0f}; ///< value of _hgt_setpoint_adj from previous frame (m) - float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm - - // vehicle physical limits - float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad) - float _STE_rate_max{0.0f}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3) - float _STE_rate_min{0.0f}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3) - float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit - float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit - float _pitch_setpoint_max{0.5f}; ///< pitch demand upper limit (rad) - float _pitch_setpoint_min{-0.5f}; ///< pitch demand lower limit (rad) - - // specific energy quantities - float _SPE_setpoint{0.0f}; ///< specific potential energy demand (m**2/sec**2) - float _SKE_setpoint{0.0f}; ///< specific kinetic energy demand (m**2/sec**2) - float _SPE_rate_setpoint{0.0f}; ///< specific potential energy rate demand (m**2/sec**3) - float _SKE_rate_setpoint{0.0f}; ///< specific kinetic energy rate demand (m**2/sec**3) - float _SPE_estimate{0.0f}; ///< specific potential energy estimate (m**2/sec**2) - float _SKE_estimate{0.0f}; ///< specific kinetic energy estimate (m**2/sec**2) - float _SPE_rate{0.0f}; ///< specific potential energy rate estimate (m**2/sec**3) - float _SKE_rate{0.0f}; ///< specific kinetic energy rate estimate (m**2/sec**3) - - // specific energy error quantities - float _STE_error{0.0f}; ///< specific total energy error (m**2/sec**2) - float _STE_rate_error{0.0f}; ///< specific total energy rate error (m**2/sec**3) - float _SEB_error{0.0f}; ///< specific energy balance error (m**2/sec**2) - float _SEB_rate_error{0.0f}; ///< specific energy balance rate error (m**2/sec**3) - - // time steps (non-fixed) - float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec) - static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec) - - // controller mode logic - bool _underspeed_detected{false}; ///< true when an underspeed condition has been detected - bool _detect_underspeed_enabled{true}; ///< true when underspeed detection is enabled - bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected - bool _climbout_mode_active{false}; ///< true when in climbout mode - bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled - bool _states_initialized{false}; ///< true when TECS states have been iniitalized - bool _in_air{false}; ///< true when the vehicle is flying - - /** - * Update the airspeed internal state using a second order complementary filter - */ - void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas); - - /** - * Update the desired airspeed - */ - void _update_speed_setpoint(); - - /** - * Update the desired height - */ - void _update_height_setpoint(float desired, float state); - - /** - * Detect if the system is not capable of maintaining airspeed - */ - void _detect_underspeed(); - - /** - * Update specific energy - */ - void _update_energy_estimates(); - - /** - * Update throttle setpoint - */ - void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat); - - /** - * Detect an uncommanded descent - */ - void _detect_uncommanded_descent(); - - /** - * Update the pitch setpoint - */ - void _update_pitch_setpoint(); - - /** - * Initialize the controller - */ - void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout, - float eas_to_tas); - - /** - * Calculate specific total energy rate limits - */ - void _update_STE_rate_lim(); - -}; diff --git a/tools/format.sh b/tools/format.sh index 3a33c30df9..da4c7620e4 100755 --- a/tools/format.sh +++ b/tools/format.sh @@ -7,8 +7,6 @@ EKF/vel_pos_fusion.cpp EKF/imu_down_sampler.*pp mathlib/*.cpp mathlib/*.h -validation/*.cpp -validation/*.h """ RED='\033[0;31m' GREEN='\033[0;32m' diff --git a/validation/CMakeLists.txt b/validation/CMakeLists.txt deleted file mode 100644 index d121aeed0d..0000000000 --- a/validation/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2018 ECL Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name ECL nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -add_library(ecl_validation - data_validator.cpp - data_validator_group.cpp - ) -add_dependencies(ecl_validation prebuild_targets) -target_compile_definitions(ecl_validation PRIVATE -DMODULE_NAME="ecl/validation") -target_include_directories(ecl_validation PUBLIC ${ECL_SOURCE_DIR}) - -if(ECL_TESTS) - add_definitions(-UNDEBUG) # keep assert - - add_subdirectory(tests) -endif() diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp deleted file mode 100644 index ab4b7d1061..0000000000 --- a/validation/data_validator.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file data_validator.c - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "data_validator.h" - -#include - -void DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in) { - float data[dimensions] = {val}; // sets the first value and all others to 0 - put(timestamp, data, error_count_in, priority_in); -} - -void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in) { - - _event_count++; - - if (error_count_in > _error_count) { - _error_density += (error_count_in - _error_count); - - } else if (_error_density > 0) { - _error_density--; - } - - _error_count = error_count_in; - _priority = priority_in; - - for (unsigned i = 0; i < dimensions; i++) { - if (_time_last == 0) { - _mean[i] = 0; - _lp[i] = val[i]; - _M2[i] = 0; - - } else { - float lp_val = val[i] - _lp[i]; - - float delta_val = lp_val - _mean[i]; - _mean[i] += delta_val / _event_count; - _M2[i] += delta_val * (lp_val - _mean[i]); - _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); - - if (fabsf(_value[i] - val[i]) < 0.000001f) { - _value_equal_count++; - - } else { - _value_equal_count = 0; - } - } - - // XXX replace with better filter, make it auto-tune to update rate - _lp[i] = _lp[i] * 0.99f + 0.01f * val[i]; - - _value[i] = val[i]; - } - - _time_last = timestamp; -} - -float DataValidator::confidence(uint64_t timestamp) { - - float ret = 1.0f; - - /* check if we have any data */ - if (_time_last == 0) { - _error_mask |= ERROR_FLAG_NO_DATA; - ret = 0.0f; - - } else if (timestamp - _time_last > _timeout_interval) { - /* timed out - that's it */ - _error_mask |= ERROR_FLAG_TIMEOUT; - ret = 0.0f; - - } else if (_value_equal_count > _value_equal_count_threshold) { - /* we got the exact same sensor value N times in a row */ - _error_mask |= ERROR_FLAG_STALE_DATA; - ret = 0.0f; - - } else if (_error_count > NORETURN_ERRCOUNT) { - /* check error count limit */ - _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; - ret = 0.0f; - - } else if (_error_density > ERROR_DENSITY_WINDOW) { - /* cap error density counter at window size */ - _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; - _error_density = ERROR_DENSITY_WINDOW; - } - - /* no critical errors */ - if (ret > 0.0f) { - /* return local error density for last N measurements */ - ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); - - if (ret > 0.0f) { - _error_mask = ERROR_FLAG_NO_ERROR; - } - } - - return ret; -} - -void DataValidator::print() { - if (_time_last == 0) { - ECL_INFO("\tno data"); - return; - } - - for (unsigned i = 0; i < dimensions; i++) { - ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", (double)_value[i], - (double)_lp[i], (double)_mean[i], (double)_rms[i], (double)confidence(ecl_absolute_time())); - } -} diff --git a/validation/data_validator.h b/validation/data_validator.h deleted file mode 100644 index 13ada5d38c..0000000000 --- a/validation/data_validator.h +++ /dev/null @@ -1,199 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file data_validator.h - * - * A data validation class to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include -#include - -class DataValidator { -public: - static const unsigned dimensions = 3; - - DataValidator() = default; - ~DataValidator() = default; - - /** - * Put an item into the validator. - * - * @param val Item to put - */ - void put(uint64_t timestamp, float val, uint64_t error_count, int priority); - - /** - * Put a 3D item into the validator. - * - * @param val Item to put - */ - void put(uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority); - - /** - * Get the next sibling in the group - * - * @return the next sibling - */ - DataValidator *sibling() { return _sibling; } - - /** - * Set the sibling to the next node in the group - * - */ - void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; } - - /** - * Get the confidence of this validator - * @return the confidence between 0 and 1 - */ - float confidence(uint64_t timestamp); - - /** - * Get the error count of this validator - * @return the error count - */ - uint64_t error_count() const { return _error_count; } - - /** - * Get the values of this validator - * @return the stored value - */ - float *value() { return _value; } - - /** - * Get the used status of this validator - * @return true if this validator ever saw data - */ - bool used() const { return (_time_last > 0); } - - /** - * Get the priority of this validator - * @return the stored priority - */ - int priority() const { return _priority; } - - /** - * Get the error state of this validator - * @return the bitmask with the error status - */ - uint32_t state() const { return _error_mask; } - - /** - * Reset the error state of this validator - */ - void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } - - /** - * Get the RMS values of this validator - * @return the stored RMS - */ - float *rms() { return _rms; } - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } - - /** - * Set the equal count threshold - * - * @param threshold The number of equal values before considering the sensor stale - */ - void set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; } - - /** - * Get the timeout value - * - * @return The timeout interval in microseconds - */ - uint32_t get_timeout() const { return _timeout_interval; } - - /** - * Data validator error states - */ - static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); - static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); - static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); - static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); - static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); - -private: - uint32_t _error_mask{ERROR_FLAG_NO_ERROR}; /**< sensor error state */ - - uint32_t _timeout_interval{20000}; /**< interval in which the datastream times out in us */ - - uint64_t _time_last{0}; /**< last timestamp */ - uint64_t _event_count{0}; /**< total data counter */ - uint64_t _error_count{0}; /**< error count */ - - int _error_density{0}; /**< ratio between successful reads and errors */ - - int _priority{0}; /**< sensor nominal priority */ - - float _mean[dimensions]{}; /**< mean of value */ - float _lp[dimensions]{}; /**< low pass value */ - float _M2[dimensions]{}; /**< RMS component value */ - float _rms[dimensions]{}; /**< root mean square error */ - float _value[dimensions]{}; /**< last value */ - - unsigned _value_equal_count{0}; /**< equal values in a row */ - unsigned _value_equal_count_threshold{ - VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */ - - DataValidator *_sibling{nullptr}; /**< 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 */ - - /* we don't want this class to be copied */ - DataValidator(const DataValidator &) = delete; - DataValidator operator=(const DataValidator &) = delete; -}; diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp deleted file mode 100644 index f369a862cd..0000000000 --- a/validation/data_validator_group.cpp +++ /dev/null @@ -1,297 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file data_validator_group.cpp - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#include "data_validator_group.h" -#include -#include - -DataValidatorGroup::DataValidatorGroup(unsigned siblings) { - - DataValidator *next = nullptr; - DataValidator *prev = nullptr; - - for (unsigned i = 0; i < siblings; i++) { - next = new DataValidator(); - - if (i == 0) { - _first = next; - - } else { - prev->setSibling(next); - } - - prev = next; - } - - _last = next; - - if (_first) { - _timeout_interval_us = _first->get_timeout(); - } -} - -DataValidatorGroup::~DataValidatorGroup() { - while (_first) { - DataValidator *next = _first->sibling(); - delete (_first); - _first = next; - } -} - -DataValidator *DataValidatorGroup::add_new_validator() { - - DataValidator *validator = new DataValidator(); - - if (!validator) { - return nullptr; - } - - _last->setSibling(validator); - _last = validator; - _last->set_timeout(_timeout_interval_us); - return _last; -} - -void DataValidatorGroup::set_timeout(uint32_t timeout_interval_us) { - - DataValidator *next = _first; - - while (next != nullptr) { - next->set_timeout(timeout_interval_us); - next = next->sibling(); - } - - _timeout_interval_us = timeout_interval_us; -} - -void DataValidatorGroup::set_equal_value_threshold(uint32_t threshold) { - - DataValidator *next = _first; - - while (next != nullptr) { - next->set_equal_value_threshold(threshold); - next = next->sibling(); - } -} - -void DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, - int priority) { - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - next->put(timestamp, val, error_count, priority); - break; - } - - next = next->sibling(); - i++; - } -} - -float *DataValidatorGroup::get_best(uint64_t timestamp, int *index) { - - DataValidator *next = _first; - - // XXX This should eventually also include voting - int pre_check_best = _curr_best; - float pre_check_confidence = 1.0f; - int pre_check_prio = -1; - float max_confidence = -1.0f; - int max_priority = -1000; - int max_index = -1; - DataValidator *best = nullptr; - - int i = 0; - - while (next != nullptr) { - float confidence = next->confidence(timestamp); - - if (i == pre_check_best) { - pre_check_prio = next->priority(); - pre_check_confidence = confidence; - } - - /* - * Switch if: - * 1) the confidence is higher and priority is equal or higher - * 2) the confidence is no less than 1% different and the priority is higher - */ - if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || - (confidence > max_confidence && (next->priority() >= max_priority)) || - (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) && - (confidence > 0.0f)) { - max_index = i; - max_confidence = confidence; - max_priority = next->priority(); - best = next; - } - - next = next->sibling(); - i++; - } - - /* the current best sensor is not matching the previous best sensor, - * or the only sensor went bad */ - if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) { - bool true_failsafe = true; - - /* check whether the switch was a failsafe or preferring a higher priority sensor */ - if (pre_check_prio != -1 && pre_check_prio < max_priority && - fabsf(pre_check_confidence - max_confidence) < 0.1f) { - /* this is not a failover */ - true_failsafe = false; - - /* reset error flags, this is likely a hotplug sensor coming online late */ - if (best != nullptr) { - best->reset_state(); - } - } - - /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ - if (_curr_best < 0) { - _prev_best = max_index; - - } else { - /* we were initialized before, this is a real failsafe */ - _prev_best = pre_check_best; - - if (true_failsafe) { - _toggle_count++; - - /* if this is the first time, log when we failed */ - if (_first_failover_time == 0) { - _first_failover_time = timestamp; - } - } - } - - /* for all cases we want to keep a record of the best index */ - _curr_best = max_index; - } - - *index = max_index; - return (best) ? best->value() : nullptr; -} - -void DataValidatorGroup::print() { - - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, - (_toggle_count > 0) ? "YES" : "NO", _toggle_count); - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used()) { - uint32_t flags = next->state(); - - ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), - ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), - ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), - ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), - ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), - ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); - - next->print(); - } - - next = next->sibling(); - i++; - } -} - -int DataValidatorGroup::failover_index() { - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && - (i == (unsigned)_prev_best)) { - return i; - } - - next = next->sibling(); - i++; - } - - return -1; -} - -uint32_t DataValidatorGroup::failover_state() { - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && - (i == (unsigned)_prev_best)) { - return next->state(); - } - - next = next->sibling(); - i++; - } - - return DataValidator::ERROR_FLAG_NO_ERROR; -} - -uint32_t DataValidatorGroup::get_sensor_state(unsigned index) { - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - return next->state(); - } - - next = next->sibling(); - i++; - } - - // sensor index not found - return UINT32_MAX; -} diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h deleted file mode 100644 index b9d1a0e6ed..0000000000 --- a/validation/data_validator_group.h +++ /dev/null @@ -1,144 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file data_validator_group.h - * - * A data validation group to identify anomalies in data streams - * - * @author Lorenz Meier - */ - -#pragma once - -#include "data_validator.h" - -class DataValidatorGroup { -public: - /** - * @param siblings initial number of DataValidator's. Must be > 0. - */ - DataValidatorGroup(unsigned siblings); - ~DataValidatorGroup(); - - /** - * Create a new Validator (with index equal to the number of currently existing validators) - * @return the newly created DataValidator or nullptr on error - */ - DataValidator *add_new_validator(); - - /** - * Put an item into the validator group. - * - * @param index Sensor index - * @param timestamp The timestamp of the measurement - * @param val The 3D vector - * @param error_count The current error count of the sensor - * @param priority The priority of the sensor - */ - void put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority); - - /** - * Get the best data triplet of the group - * - * @return pointer to the array of best values - */ - float *get_best(uint64_t timestamp, int *index); - - /** - * Get the number of failover events - * - * @return the number of failovers - */ - unsigned failover_count() const { return _toggle_count; } - - /** - * Get the index of the failed sensor in the group - * - * @return index of the failed sensor - */ - int failover_index(); - - /** - * Get the error state of the failed sensor in the group - * - * @return bitmask with erro states of the failed sensor - */ - uint32_t failover_state(); - - /** - * Get the error state of the sensor with the specified index - * - * @return bitmask with error states of the sensor - */ - uint32_t get_sensor_state(unsigned index); - - /** - * Print the validator value - * - */ - void print(); - - /** - * Set the timeout value for the whole group - * - * @param timeout_interval_us The timeout interval in microseconds - */ - void set_timeout(uint32_t timeout_interval_us); - - /** - * Set the equal count threshold for the whole group - * - * @param threshold The number of equal values before considering the sensor stale - */ - void set_equal_value_threshold(uint32_t threshold); - -private: - DataValidator *_first{nullptr}; /**< first node in the group */ - DataValidator *_last{nullptr}; /**< last node in the group */ - - uint32_t _timeout_interval_us{0}; /**< currently set timeout */ - - int _curr_best{-1}; /**< currently best index */ - int _prev_best{-1}; /**< the previous best index */ - - uint64_t _first_failover_time{0}; /**< timestamp where the first failover occured or zero if none occured */ - - unsigned _toggle_count{0}; /**< number of back and forth switches between two sensors */ - - static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; - - /* we don't want this class to be copied */ - DataValidatorGroup(const DataValidatorGroup &); - DataValidatorGroup operator=(const DataValidatorGroup &); -}; diff --git a/validation/tests/CMakeLists.txt b/validation/tests/CMakeLists.txt deleted file mode 100644 index d0540fa412..0000000000 --- a/validation/tests/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2019 ECL Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name ECL nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -add_executable(ecl_tests_data_validator test_data_validator.cpp tests_common.cpp) -target_link_libraries(ecl_tests_data_validator ecl_validation) - -add_test(NAME ecl_tests_data_validator - COMMAND ecl_tests_data_validator - ) - -add_executable(ecl_tests_data_validator_group test_data_validator_group.cpp tests_common.cpp) -target_link_libraries(ecl_tests_data_validator_group ecl_validation) - -add_test(NAME ecl_tests_data_validator_group - COMMAND ecl_tests_data_validator_group - ) diff --git a/validation/tests/test_data_validator.cpp b/validation/tests/test_data_validator.cpp deleted file mode 100644 index fa16fc8951..0000000000 --- a/validation/tests/test_data_validator.cpp +++ /dev/null @@ -1,302 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file test_data_validator.cpp - * Testing the DataValidator class - * - * @author Todd Stellanova - */ - -#include -#include -#include -#include -#include -#include -#include - - -void test_init() -{ - printf("\n--- test_init ---\n"); - - uint64_t fake_timestamp = 666; - const uint32_t timeout_usec = 2000;//from original private value - - DataValidator *validator = new DataValidator; - // initially there should be no siblings - assert(nullptr == validator->sibling()); - // initially we should have zero confidence - assert(0.0f == validator->confidence(fake_timestamp)); - // initially the error count should be zero - assert(0 == validator->error_count()); - // initially unused - assert(!validator->used()); - // initially no priority - assert(0 == validator->priority()); - validator->set_timeout(timeout_usec); - assert(validator->get_timeout() == timeout_usec); - - - DataValidator *sibling_validator = new DataValidator; - validator->setSibling(sibling_validator); - assert(sibling_validator == validator->sibling()); - - //verify that with no data, confidence is zero and error mask is set - assert(0.0f == validator->confidence(fake_timestamp + 1)); - uint32_t state = validator->state(); - assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state)); - - //verify that calling print doesn't crash tests - validator->print(); - - delete validator; //force delete -} - -void test_put() -{ - printf("\n--- test_put ---\n"); - - uint64_t timestamp = 500; - const uint32_t timeout_usec = 2000;//derived from class-private value - float val = 3.14159f; - //derived from class-private value: this is min change needed to avoid stale detection - const float sufficient_incr_value = (1.1f * 1E-6f); - - DataValidator *validator = new DataValidator; - fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); - - assert(validator->used()); - //verify that the last value we inserted is the current validator value - float last_val = val - sufficient_incr_value; - assert(validator->value()[0] == last_val); - - // we've just provided a bunch of valid data: should be fully confident - float conf = validator->confidence(timestamp); - - if (1.0f != conf) { - printf("conf: %f\n", (double)conf); - dump_validator_state(validator); - } - - assert(1.0f == conf); - // should be no errors - assert(0 == validator->state()); - - //now check confidence much beyond the timeout window-- should timeout - conf = validator->confidence(timestamp + (1.1 * timeout_usec)); - - if (0.0f != conf) { - printf("conf: %f\n", (double)conf); - dump_validator_state(validator); - } - - assert(0.0f == conf); - assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); - - delete validator; //force delete -} - -/** - * Verify that the DataValidator detects sensor data that does not vary sufficiently - */ -void test_stale_detector() -{ - printf("\n--- test_stale_detector ---\n"); - - uint64_t timestamp = 500; - float val = 3.14159f; - //derived from class-private value, this is insufficient to avoid stale detection: - const float insufficient_incr_value = (0.99 * 1E-6f); - - DataValidator *validator = new DataValidator; - fill_validator_with_samples(validator, insufficient_incr_value, &val, ×tamp); - - // data is stale: should have no confidence - assert(0.0f == validator->confidence(timestamp)); - - // should be a stale error - uint32_t state = validator->state(); - - if (DataValidator::ERROR_FLAG_STALE_DATA != state) { - dump_validator_state(validator); - } - - assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state)); - - delete validator; //force delete -} - -/** - * Verify the RMS error calculated by the DataValidator for a series of samples - */ -void test_rms_calculation() -{ - printf("\n--- test_rms_calculation ---\n"); - const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT - const float mean_value = 3.14159f; - const uint32_t sample_count = 1000; - float expected_rms_err = 0.0f; - uint64_t timestamp = 500; - - DataValidator *validator = new DataValidator; - validator->set_equal_value_threshold(equal_value_count); - - insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, ×tamp); - float *rms = validator->rms(); - assert(nullptr != rms); - float calc_rms_err = rms[0]; - float diff = fabsf(calc_rms_err - expected_rms_err); - float diff_frac = (diff / expected_rms_err); - printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err, - (double)diff, (double)diff_frac); - assert(diff_frac < 0.03f); - - delete validator; //force delete -} - -/** - * Verify error tracking performed by DataValidator::put - */ -void test_error_tracking() -{ - printf("\n--- test_error_tracking ---\n"); - uint64_t timestamp = 500; - uint64_t timestamp_incr = 5; - const uint32_t timeout_usec = 2000;//from original private value - float val = 3.14159f; - uint64_t error_count = 0; - int expected_error_density = 0; - int priority = 50; - //from private value: this is min change needed to avoid stale detection - const float sufficient_incr_value = (1.1f * 1E-6f); - //default is private VALUE_EQUAL_COUNT_DEFAULT - const int equal_value_count = 50000; - //should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT - const int total_iterations = 1000; - - DataValidator *validator = new DataValidator; - validator->set_timeout(timeout_usec); - validator->set_equal_value_threshold(equal_value_count); - - //put a bunch of values that are all different - for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) { - timestamp += timestamp_incr; - - //up to a 50% random error rate appears to pass the error density filter - if ((((float)rand() / (float)RAND_MAX)) < 0.500f) { - error_count += 1; - expected_error_density += 1; - - } else if (expected_error_density > 0) { - expected_error_density -= 1; - } - - validator->put(timestamp, val, error_count, priority); - } - - assert(validator->used()); - //at this point, error_count should be less than NORETURN_ERRCOUNT - assert(validator->error_count() == error_count); - - // we've just provided a bunch of valid data with some errors: - // confidence should be reduced by the number of errors - float conf = validator->confidence(timestamp); - printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf); - assert(1.0f != conf); //we should not be fully confident - assert(0.0f != conf); //neither should we be completely unconfident - // should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT - assert(0 == validator->state()); - - // the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW) - // ERROR_DENSITY_WINDOW is currently private, but == 100.0f - float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f); - double diff = fabs(reduced_conf - conf); - - if (reduced_conf != conf) { - printf("conf: %f reduced_conf: %f diff: %f\n", - (double)conf, (double)reduced_conf, diff); - dump_validator_state(validator); - } - - assert(diff < 1E-6f); - - //Now, insert a series of errors and ensure we trip the error detector - for (int i = 0; i < 250; i++, val += sufficient_incr_value) { - timestamp += timestamp_incr; - //100% error rate - error_count += 1; - expected_error_density += 1; - validator->put(timestamp, val, error_count, priority); - } - - conf = validator->confidence(timestamp); - assert(0.0f == conf); // should we be completely unconfident - // we should have triggered the high error density detector - assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state())); - - - validator->reset_state(); - - //Now insert so many errors that we exceed private NORETURN_ERRCOUNT - for (int i = 0; i < 10000; i++, val += sufficient_incr_value) { - timestamp += timestamp_incr; - //100% error rate - error_count += 1; - expected_error_density += 1; - validator->put(timestamp, val, error_count, priority); - } - - conf = validator->confidence(timestamp); - assert(0.0f == conf); // should we be completely unconfident - // we should have triggered the high error count detector - assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state())); - - delete validator; //force delete - -} - -int main(int argc, char *argv[]) -{ - (void)argc; // unused - (void)argv; // unused - - srand(666); - test_init(); - test_put(); - test_stale_detector(); - test_rms_calculation(); - test_error_tracking(); - - return 0; //passed -} diff --git a/validation/tests/test_data_validator_group.cpp b/validation/tests/test_data_validator_group.cpp deleted file mode 100644 index 11f8950e11..0000000000 --- a/validation/tests/test_data_validator_group.cpp +++ /dev/null @@ -1,385 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file test_data_validator_group.cpp - * Testing the DataValidatorGroup class - * - * @author Todd Stellanova - */ - -#include -#include -#include -#include -#include -#include -#include -#include - - -const uint32_t base_timeout_usec = 2000;//from original private value -const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT -const uint64_t base_timestamp = 666; -const unsigned base_num_siblings = 4; - - -/** - * Initialize a DataValidatorGroup with some common settings; - * @param sibling_count (out) the number of siblings initialized - */ -DataValidatorGroup *setup_base_group(unsigned *sibling_count) -{ - unsigned num_siblings = base_num_siblings; - - DataValidatorGroup *group = new DataValidatorGroup(num_siblings); - assert(nullptr != group); - //verify that calling print doesn't crash the tests - group->print(); - printf("\n"); - - //should be no failovers yet - assert(0 == group->failover_count()); - assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); - assert(-1 == group->failover_index()); - - //this sets the timeout on all current members of the group, as well as members added later - group->set_timeout(base_timeout_usec); - //the following sets the threshold on all CURRENT members of the group, but not any added later - //TODO This is likely a bug in DataValidatorGroup - group->set_equal_value_threshold(equal_value_count); - - //return values - *sibling_count = num_siblings; - - return group; -} - -/** - * Fill one DataValidator with samples, by index. - * - * @param group - * @param val1_idx Index of the validator to fill with samples - * @param num_samples - */ -void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples) -{ - uint64_t timestamp = base_timestamp; - uint64_t error_count = 0; - float last_best_val = 0.0f; - - for (uint32_t i = 0; i < num_samples; i++) { - float val = ((float) rand() / (float) RAND_MAX); - float data[DataValidator::dimensions] = {val}; - group->put(val1_idx, timestamp, data, error_count, 100); - last_best_val = val; - } - - int best_idx = 0; - float *best_data = group->get_best(timestamp, &best_idx); - assert(last_best_val == best_data[0]); - assert(best_idx == val1_idx); -} - - - -/** - * Fill two validators in the group with samples, by index. - * Both validators will be filled with the same data, but - * the priority of the first validator will be higher than the second. - * - * @param group - * @param val1_idx index of the first validator to fill - * @param val2_idx index of the second validator to fill - * @param num_samples - */ -void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples) -{ - uint64_t timestamp = base_timestamp; - uint64_t error_count = 0; - float last_best_val = 0.0f; - - for (uint32_t i = 0; i < num_samples; i++) { - float val = ((float) rand() / (float) RAND_MAX); - float data[DataValidator::dimensions] = {val}; - //two sensors with identical values, but different priorities - group->put(val1_idx, timestamp, data, error_count, 100); - group->put(val2_idx, timestamp, data, error_count, 10); - last_best_val = val; - } - - int best_idx = 0; - float *best_data = group->get_best(timestamp, &best_idx); - assert(last_best_val == best_data[0]); - assert(best_idx == val1_idx); - -} - -/** - * Dynamically add a validator to the group after construction - * @param group - * @return - */ -DataValidator *add_validator_to_group(DataValidatorGroup *group) -{ - DataValidator *validator = group->add_new_validator(); - //verify the previously set timeout applies to the new group member - assert(validator->get_timeout() == base_timeout_usec); - //for testing purposes, ensure this newly added member is consistent with the rest of the group - //TODO this is likely a bug in DataValidatorGroup - validator->set_equal_value_threshold(equal_value_count); - - return validator; -} - -/** - * Create a DataValidatorGroup and tack on two additional DataValidators - * - * @param validator1_handle (out) first DataValidator added to the group - * @param validator2_handle (out) second DataValidator added to the group - * @param sibling_count (in/out) in: number of initial siblings to create, out: total - * @return - */ -DataValidatorGroup *setup_group_with_two_validator_handles( - DataValidator **validator1_handle, - DataValidator **validator2_handle, - unsigned *sibling_count) -{ - DataValidatorGroup *group = setup_base_group(sibling_count); - - //now we add validators - *validator1_handle = add_validator_to_group(group); - *validator2_handle = add_validator_to_group(group); - *sibling_count += 2; - - return group; -} - - -void test_init() -{ - unsigned num_siblings = 0; - - DataValidatorGroup *group = setup_base_group(&num_siblings); - - //should not yet be any best value - int best_index = -1; - assert(nullptr == group->get_best(base_timestamp, &best_index)); - - delete group; //force cleanup -} - - -/** - * Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority - */ -void test_put() -{ - unsigned num_siblings = 0; - DataValidator *validator1 = nullptr; - DataValidator *validator2 = nullptr; - - uint64_t timestamp = base_timestamp; - - DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); - printf("num_siblings: %d \n", num_siblings); - unsigned val1_idx = num_siblings - 2; - unsigned val2_idx = num_siblings - 1; - - fill_two_with_valid_data(group, val1_idx, val2_idx, 500); - int best_idx = -1; - float *best_data = group->get_best(timestamp, &best_idx); - assert(nullptr != best_data); - float best_val = best_data[0]; - - float *cur_val1 = validator1->value(); - assert(nullptr != cur_val1); - //printf("cur_val1 %p \n", cur_val1); - assert(best_val == cur_val1[0]); - - float *cur_val2 = validator2->value(); - assert(nullptr != cur_val2); - //printf("cur_val12 %p \n", cur_val2); - assert(best_val == cur_val2[0]); - - delete group; //force cleanup -} - - -/** - * Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best". - */ -void test_priority_switch() -{ - unsigned num_siblings = 0; - DataValidator *validator1 = nullptr; - DataValidator *validator2 = nullptr; - - uint64_t timestamp = base_timestamp; - - DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); - //printf("num_siblings: %d \n",num_siblings); - int val1_idx = (int)num_siblings - 2; - int val2_idx = (int)num_siblings - 1; - uint64_t error_count = 0; - - fill_two_with_valid_data(group, val1_idx, val2_idx, 100); - - int best_idx = -1; - float *best_data = nullptr; - //now, switch the priorities, which switches "best" but doesn't trigger a failover - float new_best_val = 3.14159f; - float data[DataValidator::dimensions] = {new_best_val}; - //a single sample insertion should be sufficient to trigger a priority switch - group->put(val1_idx, timestamp, data, error_count, 1); - group->put(val2_idx, timestamp, data, error_count, 100); - best_data = group->get_best(timestamp, &best_idx); - assert(new_best_val == best_data[0]); - //the new best sensor should now be the sensor with the higher priority - assert(best_idx == val2_idx); - //should not have detected a real failover - assert(0 == group->failover_count()); - - delete group; //cleanup -} - -/** - * Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors - */ -void test_simple_failover() -{ - unsigned num_siblings = 0; - DataValidator *validator1 = nullptr; - DataValidator *validator2 = nullptr; - - uint64_t timestamp = base_timestamp; - - DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); - //printf("num_siblings: %d \n",num_siblings); - int val1_idx = (int)num_siblings - 2; - int val2_idx = (int)num_siblings - 1; - - - fill_two_with_valid_data(group, val1_idx, val2_idx, 100); - - int best_idx = -1; - float *best_data = nullptr; - - //trigger a real failover - float new_best_val = 3.14159f; - float data[DataValidator::dimensions] = {new_best_val}; - //trigger a bunch of errors on the previous best sensor - unsigned val1_err_count = 0; - - for (int i = 0; i < 25; i++) { - group->put(val1_idx, timestamp, data, ++val1_err_count, 100); - group->put(val2_idx, timestamp, data, 0, 10); - } - - assert(validator1->error_count() == val1_err_count); - - //since validator1 is experiencing errors, we should see a failover to validator2 - best_data = group->get_best(timestamp + 1, &best_idx); - assert(nullptr != best_data); - assert(new_best_val == best_data[0]); - assert(best_idx == val2_idx); - //should have detected a real failover - printf("failover_count: %d \n", group->failover_count()); - assert(1 == group->failover_count()); - - //even though validator1 has encountered a bunch of errors, it hasn't failed - assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state()); - - // although we failed over from one sensor to another, this is not the same thing tracked by failover_index - int fail_idx = group->failover_index(); - assert(-1 == fail_idx);//no failed sensor - - //since no sensor has actually hard-failed, the group failover state is NO_ERROR - assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); - - - delete group; //cleanup -} - -/** - * Force once sensor to fail and ensure that we detect it - */ -void test_sensor_failure() -{ - unsigned num_siblings = 0; - uint64_t timestamp = base_timestamp; - const float sufficient_incr_value = (1.1f * 1E-6f); - const uint32_t timeout_usec = 2000;//derived from class-private value - - float val = 3.14159f; - - DataValidatorGroup *group = setup_base_group(&num_siblings); - - //now we add validators - DataValidator *validator = add_validator_to_group(group); - assert(nullptr != validator); - num_siblings++; - int val_idx = num_siblings - 1; - - fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); - //the best should now be the one validator we've filled with samples - - int best_idx = -1; - float *best_data = group->get_best(timestamp, &best_idx); - assert(nullptr != best_data); - //printf("best_idx: %d val_idx: %d\n", best_idx, val_idx); - assert(best_idx == val_idx); - - //now force a timeout failure in the one validator, by checking confidence long past timeout - validator->confidence(timestamp + (1.1 * timeout_usec)); - assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); - - //now that the one sensor has failed, the group should detect this as well - int fail_idx = group->failover_index(); - assert(val_idx == fail_idx); - - delete group; -} - -int main(int argc, char *argv[]) -{ - (void)argc; // unused - (void)argv; // unused - - test_init(); - test_put(); - test_simple_failover(); - test_priority_switch(); - test_sensor_failure(); - - return 0; //passed -} diff --git a/validation/tests/tests_common.cpp b/validation/tests/tests_common.cpp deleted file mode 100644 index bcf18ba369..0000000000 --- a/validation/tests/tests_common.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include "tests_common.h" - - -void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, - uint64_t *timestamp_io) -{ - uint64_t timestamp = *timestamp_io; - uint64_t timestamp_incr = 5; - const uint64_t error_count = 0; - const int priority = 50; - const float swing = 1E-2f; - double sum_dev_squares = 0.0f; - - //insert a series of values that swing around the mean - for (uint32_t i = 0; i < count; i++) { - float iter_swing = (0 == (i % 2)) ? swing : -swing; - float iter_val = mean + iter_swing; - float iter_dev = iter_val - mean; - sum_dev_squares += (iter_dev * iter_dev); - timestamp += timestamp_incr; - validator->put(timestamp, iter_val, error_count, priority); - } - - double rms = sqrt(sum_dev_squares / (double)count); - //note: this should be approximately equal to "swing" - *rms_err = (float)rms; - *timestamp_io = timestamp; -} - -void dump_validator_state(DataValidator *validator) -{ - uint32_t state = validator->state(); - printf("state: 0x%x no_data: %d stale: %d timeout:%d\n", - validator->state(), - DataValidator::ERROR_FLAG_NO_DATA & state, - DataValidator::ERROR_FLAG_STALE_DATA & state, - DataValidator::ERROR_FLAG_TIMEOUT & state - ); - validator->print(); - printf("\n"); -} - -void fill_validator_with_samples(DataValidator *validator, - const float incr_value, - float *value_io, - uint64_t *timestamp_io) -{ - uint64_t timestamp = *timestamp_io; - const uint64_t timestamp_incr = 5; //usec - const uint32_t timeout_usec = 2000;//derived from class-private value - - float val = *value_io; - const uint64_t error_count = 0; - const int priority = 50; //"medium" priority - const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT - - validator->set_equal_value_threshold(equal_value_count); - validator->set_timeout(timeout_usec); - - //put a bunch of values that are all different - for (int i = 0; i < equal_value_count; i++, val += incr_value) { - timestamp += timestamp_incr; - validator->put(timestamp, val, error_count, priority); - } - - *timestamp_io = timestamp; - *value_io = val; - -} \ No newline at end of file diff --git a/validation/tests/tests_common.h b/validation/tests/tests_common.h deleted file mode 100644 index ae1351dfe1..0000000000 --- a/validation/tests/tests_common.h +++ /dev/null @@ -1,68 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2019 Todd Stellanova. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#ifndef ECL_TESTS_COMMON_H -#define ECL_TESTS_COMMON_H - -#include - -/** - * Insert a series of samples around a mean value - * @param validator The validator under test - * @param mean The mean value - * @param count The number of samples to insert in the validator - * @param rms_err (out) calculated rms error of the inserted samples - * @param timestamp_io (in/out) in: start timestamp, out: last timestamp - */ -void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, - uint64_t *timestamp_io); - -/** - * Print out the state of a DataValidator - * @param validator - */ -void dump_validator_state(DataValidator *validator); - -/** - * Insert a time series of samples into the validator - * @param validator - * @param incr_value The amount to increment the value by on each iteration - * @param value_io (in/out) in: initial value, out: final value - * @param timestamp_io (in/out) in: initial timestamp, out: final timestamp - */ -void fill_validator_with_samples(DataValidator *validator, - const float incr_value, - float *value_io, - uint64_t *timestamp_io); - -#endif //ECL_TESTS_COMMON_H