dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

move TECS, L1, and validation to PX4/Firmware

This commit is contained in:
Daniel Agar 2020-06-15 14:15:02 -04:00
parent a296fe7d8c
commit a8bb8ea99f
18 changed files with 0 additions and 3392 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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
View File

@ -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