zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

move TECS, L1, and validation to PX4/Firmware

master
Daniel Agar 5 years ago
parent
a296fe7d8c
commit
a8bb8ea99f
18 changed files with 0 additions and 3392 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      CMakeLists.txt
  2. 40
      l1/CMakeLists.txt
  3. 383
      l1/ecl_l1_pos_controller.cpp
  4. 251
      l1/ecl_l1_pos_controller.h
  5. 39
      tecs/CMakeLists.txt
  6. 608
      tecs/tecs.cpp
  7. 327
      tecs/tecs.h
  8. 2
      tools/format.sh
  9. 46
      validation/CMakeLists.txt
  10. 149
      validation/data_validator.cpp
  11. 199
      validation/data_validator.h
  12. 297
      validation/data_validator_group.cpp
  13. 144
      validation/data_validator_group.h
  14. 46
      validation/tests/CMakeLists.txt
  15. 302
      validation/tests/test_data_validator.cpp
  16. 385
      validation/tests/test_data_validator_group.cpp
  17. 103
      validation/tests/tests_common.cpp
  18. 68
      validation/tests/tests_common.h

3
CMakeLists.txt
Unescape View File

@ -166,9 +166,6 @@ add_subdirectory(airdata) @@ -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)

40
l1/CMakeLists.txt
Unescape View File

@ -1,40 +0,0 @@ @@ -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)

383
l1/ecl_l1_pos_controller.cpp
Unescape View File

@ -1,383 +0,0 @@ @@ -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 <float.h>
#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<float>(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;
}

251
l1/ecl_l1_pos_controller.h
Unescape View File

@ -1,251 +0,0 @@ @@ -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 <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <geo/geo.h>
#include <ecl.h>
/**
* 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)}; ///<maximum roll angle in radians
float _roll_setpoint{0.0f}; ///< current roll angle setpoint in radians
float _roll_slew_rate{0.0f}; ///< roll angle setpoint slew rate limit in rad/s
float _dt{0}; ///< control loop time in seconds
/**
* Convert a 2D vector from WGS84 to planar coordinates.
*
* This converts from latitude and longitude to planar
* coordinates with (0,0) being at the position of ref and
* returns a vector in meters towards wp.
*
* @param ref The reference position in WGS84 coordinates
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
/**
* Update roll angle setpoint. This will also apply slew rate limits if set.
*
*/
void update_roll_setpoint();
};
#endif /* ECL_L1_POS_CONTROLLER_H */

39
tecs/CMakeLists.txt
Unescape View File

@ -1,39 +0,0 @@ @@ -1,39 +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_tecs
tecs.cpp
)
add_dependencies(ecl_tecs prebuild_targets)
target_compile_definitions(ecl_tecs PRIVATE -DMODULE_NAME="ecl/tecs")
target_include_directories(ecl_tecs PUBLIC ${ECL_SOURCE_DIR})

608
tecs/tecs.cpp
Unescape View File

@ -1,608 +0,0 @@ @@ -1,608 +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.
*
****************************************************************************/
#include "tecs.h"
#include <ecl.h>
#include <geo/geo.h>
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;
}
}

327
tecs/tecs.h
Unescape View File

@ -1,327 +0,0 @@ @@ -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 <mathlib/mathlib.h>
#include <matrix/math.hpp>
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();
};

2
tools/format.sh
Unescape View File

@ -7,8 +7,6 @@ EKF/vel_pos_fusion.cpp @@ -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'

46
validation/CMakeLists.txt
Unescape View File

@ -1,46 +0,0 @@ @@ -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()

149
validation/data_validator.cpp
Unescape View File

@ -1,149 +0,0 @@ @@ -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 <lorenz@px4.io>
*/
#include "data_validator.h"
#include <ecl.h>
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()));
}
}

199
validation/data_validator.h
Unescape View File

@ -1,199 +0,0 @@ @@ -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 <lorenz@px4.io>
*/
#pragma once
#include <math.h>
#include <stdint.h>
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;
};

297
validation/data_validator_group.cpp
Unescape View File

@ -1,297 +0,0 @@ @@ -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 <lorenz@px4.io>
*/
#include "data_validator_group.h"
#include <ecl.h>
#include <float.h>
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;
}

144
validation/data_validator_group.h
Unescape View File

@ -1,144 +0,0 @@ @@ -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 <lorenz@px4.io>
*/
#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 &);
};

46
validation/tests/CMakeLists.txt
Unescape View File

@ -1,46 +0,0 @@ @@ -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
)

302
validation/tests/test_data_validator.cpp
Unescape View File

@ -1,302 +0,0 @@ @@ -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 <stdint.h>
#include <cassert>
#include <cstdlib>
#include <stdio.h>
#include <math.h>
#include <validation/data_validator.h>
#include <validation/tests/tests_common.h>
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, &timestamp);
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, &timestamp);
// 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, &timestamp);
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
}

385
validation/tests/test_data_validator_group.cpp
Unescape View File

@ -1,385 +0,0 @@ @@ -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 <stdint.h>
#include <cassert>
#include <cstdlib>
#include <stdio.h>
#include <math.h>
#include <validation/data_validator.h>
#include <validation/data_validator_group.h>
#include <validation/tests/tests_common.h>
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, &timestamp);
//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
}

103
validation/tests/tests_common.cpp
Unescape View File

@ -1,103 +0,0 @@ @@ -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 <stdio.h>
#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;
}

68
validation/tests/tests_common.h
Unescape View File

@ -1,68 +0,0 @@ @@ -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 <validation/data_validator.h>
/**
* 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
Write Preview
Loading…
Cancel
Save