diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 107975062d2..e51475a039c 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -48,8 +48,10 @@ add_subdirectory(drivers) add_subdirectory(ecl) add_subdirectory(flight_tasks) add_subdirectory(hysteresis) +add_subdirectory(l1) add_subdirectory(landing_slope) add_subdirectory(led) +add_subdirectory(mag_compensation) add_subdirectory(mathlib) add_subdirectory(mixer) add_subdirectory(mixer_module) @@ -58,8 +60,8 @@ add_subdirectory(perf) add_subdirectory(pid) add_subdirectory(rc) add_subdirectory(systemlib) +add_subdirectory(tecs) add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(weather_vane) -add_subdirectory(mag_compensation) diff --git a/src/lib/ecl b/src/lib/ecl index a296fe7d8cb..a8bb8ea99fc 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit a296fe7d8cbac1c3e2cc1efcdc9da29758cb66a1 +Subproject commit a8bb8ea99fc92389a5cdd44cd7108416e0c49fb6 diff --git a/src/lib/l1/CMakeLists.txt b/src/lib/l1/CMakeLists.txt new file mode 100644 index 00000000000..c564b308f13 --- /dev/null +++ b/src/lib/l1/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2018-2020 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. +# +############################################################################ + +px4_add_library(l1 + ECL_L1_Pos_Controller.cpp + ECL_L1_Pos_Controller.hpp +) + +add_dependencies(l1 git_ecl) +target_link_libraries(l1 PRIVATE ecl_geo) diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp new file mode 100644 index 00000000000..fe3bf308a9d --- /dev/null +++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp @@ -0,0 +1,388 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 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 ECL_L1_Pos_Controller.cpp + * Implementation of L1 position control. + * Authors and acknowledgements in header. + * + */ + +#include "ECL_L1_Pos_Controller.hpp" + +#include + +#include + +#include + +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 (PX4_ISFINITE(roll_new)) { + _roll_setpoint = roll_new; + } + +} + +float ECL_L1_Pos_Controller::switch_distance(float wp_radius) +{ + /* following [2], switching on L1 distance */ + return math::min(wp_radius, _L1_distance); +} + +void +ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector2f &vector_B, + const Vector2f &vector_curr_position, const Vector2f &ground_speed_vector) +{ + /* this follows the logic presented in [1] */ + float eta = 0.0f; + float xtrack_vel = 0.0f; + float ltrack_vel = 0.0f; + + /* get the direction between the last (visited) and next waypoint */ + _target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), + (double)vector_B(0), (double)vector_B(1)); + + /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length(), 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate vector from A to B */ + Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + + /* + * check if waypoints are on top of each other. If yes, + * skip A and directly continue to B + */ + if (vector_AB.length() < 1.0e-6f) { + vector_AB = get_local_planar_vector(vector_curr_position, vector_B); + } + + vector_AB.normalize(); + + /* calculate the vector from waypoint A to the aircraft */ + Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + /* calculate crosstrack error (output only) */ + _crosstrack_error = vector_AB % vector_A_to_airplane; + + /* + * If the current position is in a +-135 degree angle behind waypoint A + * and further away from A than the L1 distance, then A becomes the L1 point. + * If the aircraft is already between A and B normal L1 logic is applied. + */ + float distance_A_to_airplane = vector_A_to_airplane.length(); + float alongTrackDist = vector_A_to_airplane * vector_AB; + + /* estimate airplane position WRT to B */ + Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + + /* calculate angle of airplane position vector relative to line) */ + + // XXX this could probably also be based solely on the dot product + float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB); + + /* extension from [2], fly directly to A */ + if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane, 1.0f) < -0.7071f) { + + /* calculate eta to fly to waypoint A */ + + /* unit vector from waypoint A to current position */ + Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); + + /* + * If the AB vector and the vector from B to airplane point in the same + * direction, we have missed the waypoint. At +- 90 degrees we are just passing it. + */ + + } else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) { + /* + * Extension, fly back to waypoint. + * + * This corner case is possible if the system was following + * the AB line from waypoint A to waypoint B, then is + * switched to manual mode (or otherwise misses the waypoint) + * and behind the waypoint continues to follow the AB line. + */ + + /* calculate eta to fly to waypoint B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0)); + + } else { + + /* calculate eta to fly along the line between A and B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % vector_AB; + /* velocity along line */ + ltrack_vel = ground_speed_vector * vector_AB; + /* calculate eta2 (angle of velocity vector relative to line) */ + float eta2 = atan2f(xtrack_vel, ltrack_vel); + /* calculate eta1 (angle to L1 point) */ + float xtrackErr = vector_A_to_airplane % vector_AB; + float sine_eta1 = xtrackErr / math::max(_L1_distance, 0.1f); + /* limit output to 45 degrees */ + sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071 + float eta1 = asinf(sine_eta1); + eta = eta1 + eta2; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; + + } + + /* limit angle to +-90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* flying to waypoints, not circling them */ + _circle_mode = false; + + /* the bearing angle, in NED frame */ + _bearing_error = eta; + + update_roll_setpoint(); +} + +void +ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius, + int8_t loiter_direction, const Vector2f &ground_speed_vector) +{ + /* the complete guidance logic in this section was proposed by [2] */ + + /* calculate the gains for the PD loop (circle tracking) */ + float omega = (2.0f * M_PI_F / _L1_period); + float K_crosstrack = omega * omega; + float K_velocity = 2.0f * _L1_damping * omega; + + /* update bearing to next waypoint */ + _target_bearing = get_bearing_to_next_waypoint((double)vector_curr_position(0), (double)vector_curr_position(1), + (double)vector_A(0), (double)vector_A(1)); + + /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ + float ground_speed = math::max(ground_speed_vector.length(), 0.1f); + + /* calculate the L1 length required for the desired period */ + _L1_distance = _L1_ratio * ground_speed; + + /* calculate the vector from waypoint A to current position */ + Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + + Vector2f vector_A_to_airplane_unit; + + /* prevent NaN when normalizing */ + if (vector_A_to_airplane.length() > FLT_EPSILON) { + /* store the normalized vector from waypoint A to current position */ + vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + + } else { + vector_A_to_airplane_unit = vector_A_to_airplane; + } + + /* calculate eta angle towards the loiter center */ + + /* velocity across / orthogonal to line from waypoint to current position */ + float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector; + /* velocity along line from waypoint to current position */ + float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit); + float eta = atan2f(xtrack_vel_center, ltrack_vel_center); + /* limit eta to 90 degrees */ + eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f); + + /* calculate the lateral acceleration to capture the center point */ + float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta); + + /* for PD control: Calculate radial position and velocity errors */ + + /* radial velocity error */ + float xtrack_vel_circle = -ltrack_vel_center; + /* radial distance from the loiter circle (not center) */ + float xtrack_err_circle = vector_A_to_airplane.length() - radius; + + /* cross track error for feedback */ + _crosstrack_error = xtrack_err_circle; + + /* calculate PD update to circle waypoint */ + float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity); + + /* calculate velocity on circle / along tangent */ + float tangent_vel = xtrack_vel_center * loiter_direction; + + /* prevent PD output from turning the wrong way */ + if (tangent_vel < 0.0f) { + lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f); + } + + /* calculate centripetal acceleration setpoint */ + float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius), + (radius + xtrack_err_circle)); + + /* add PD control on circle and centripetal acceleration for total circle command */ + float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal); + + /* + * Switch between circle (loiter) and capture (towards waypoint center) mode when + * the commands switch over. Only fly towards waypoint if outside the circle. + */ + + // XXX check switch over + if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) || + (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) { + _lateral_accel = lateral_accel_sp_center; + _circle_mode = false; + /* angle between requested and current velocity vector */ + _bearing_error = eta; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); + + } else { + _lateral_accel = lateral_accel_sp_circle; + _circle_mode = true; + _bearing_error = 0.0f; + /* bearing from current position to L1 point */ + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)); + } + + update_roll_setpoint(); +} + +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, + const Vector2f &ground_speed_vector) +{ + /* the complete guidance logic in this section was proposed by [2] */ + + /* + * As the commanded heading is the only reference + * (and no crosstrack correction occurs), + * target and navigation bearing become the same + */ + _target_bearing = _nav_bearing = wrap_pi(navigation_heading); + + float eta = wrap_pi(_target_bearing - wrap_pi(current_heading)); + + /* consequently the bearing error is exactly eta: */ + _bearing_error = eta; + + /* ground speed is the length of the ground speed vector */ + float ground_speed = ground_speed_vector.length(); + + /* adjust L1 distance to keep constant frequency */ + _L1_distance = ground_speed / _heading_omega; + float omega_vel = ground_speed * _heading_omega; + + /* not circling a waypoint */ + _circle_mode = false; + + /* navigating heading means by definition no crosstrack error */ + _crosstrack_error = 0; + + /* limit eta to 90 degrees */ + eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f); + _lateral_accel = 2.0f * sinf(eta) * omega_vel; + + update_roll_setpoint(); +} + +void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) +{ + /* the logic in this section is trivial, but originally proposed by [2] */ + + /* reset all heading / error measures resulting in zero roll */ + _target_bearing = current_heading; + _nav_bearing = current_heading; + _bearing_error = 0; + _crosstrack_error = 0; + _lateral_accel = 0; + + /* not circling a waypoint when flying level */ + _circle_mode = false; + + update_roll_setpoint(); +} + +Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const +{ + /* this is an approximation for small angles, proposed by [2] */ + Vector2f out(math::radians((target(0) - origin(0))), + math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); + + return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); +} + +void ECL_L1_Pos_Controller::set_l1_period(float period) +{ + _L1_period = period; + + /* calculate the ratio introduced in [2] */ + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + + /* calculate normalized frequency for heading tracking */ + _heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period; +} + +void ECL_L1_Pos_Controller::set_l1_damping(float damping) +{ + _L1_damping = damping; + + /* calculate the ratio introduced in [2] */ + _L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period; + + /* calculate the L1 gain (following [2]) */ + _K_L1 = 4.0f * _L1_damping * _L1_damping; +} diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp new file mode 100644 index 00000000000..1cdaa3f06c1 --- /dev/null +++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp @@ -0,0 +1,249 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2020 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 ecl_l1_pos_control.h + * Implementation of L1 position control. + * + * + * Acknowledgements and References: + * + * This implementation has been built for PX4 based on the original + * publication from [1] and does include a lot of the ideas (not code) + * from [2]. + * + * + * [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking," + * Proceedings of the AIAA Guidance, Navigation and Control + * Conference, Aug 2004. AIAA-2004-4900. + * + * [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013. + * - Explicit control over frequency and damping + * - Explicit control over track capture angle + * - Ability to use loiter radius smaller than L1 length + * - Modified to use PD control for circle tracking to enable loiter radius less than L1 length + * - Modified to enable period and damping of guidance loop to be set explicitly + * - Modified to provide explicit control over capture angle + * + */ + +#ifndef ECL_L1_POS_CONTROLLER_H +#define ECL_L1_POS_CONTROLLER_H + +#include +#include + +/** + * L1 Nonlinear Guidance Logic + */ +class ECL_L1_Pos_Controller +{ +public: + /** + * The current target bearing + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float nav_bearing() { return matrix::wrap_pi(_nav_bearing); } + + /** + * Get lateral acceleration demand. + * + * @return Lateral acceleration in m/s^2 + */ + float nav_lateral_acceleration_demand() { return _lateral_accel; } + + /** + * Heading error. + * + * The heading error is either compared to the current track + * or to the tangent of the current loiter radius. + */ + float bearing_error() { return _bearing_error; } + + /** + * Bearing from aircraft to current target. + * + * @return bearing angle (-pi..pi, in NED frame) + */ + float target_bearing() { return _target_bearing; } + + /** + * Get roll angle setpoint for fixed wing. + * + * @return Roll angle (in NED frame) + */ + float get_roll_setpoint() { return _roll_setpoint; } + + /** + * Get the current crosstrack error. + * + * @return Crosstrack error in meters. + */ + float crosstrack_error() { return _crosstrack_error; } + + /** + * Returns true if the loiter waypoint has been reached + */ + bool reached_loiter_target() { return _circle_mode; } + + /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { return _circle_mode; } + + /** + * Get the switch distance + * + * This is the distance at which the system will + * switch to the next waypoint. This depends on the + * period and damping + * + * @param waypoint_switch_radius The switching radius the waypoint has set. + */ + float switch_distance(float waypoint_switch_radius); + + /** + * Navigate between two waypoints + * + * Calling this function with two waypoints results in the + * control outputs to fly to the line segment defined by + * the points and once captured following the line segment. + * This follows the logic in [1]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_waypoints(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_B, + const matrix::Vector2f &vector_curr_position, const matrix::Vector2f &ground_speed); + + /** + * Navigate on an orbit around a loiter waypoint. + * + * This allow orbits smaller than the L1 length, + * this modification was introduced in [2]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius, + int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector); + + /** + * Navigate on a fixed bearing. + * + * This only holds a certain direction and does not perform cross + * track correction. Helpful for semi-autonomous modes. Introduced + * by [2]. + * + * @return sets _lateral_accel setpoint + */ + void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed); + + /** + * Keep the wings level. + * + * This is typically needed for maximum-lift-demand situations, + * such as takeoff or near stall. Introduced in [2]. + */ + void navigate_level_flight(float current_heading); + + /** + * Set the L1 period. + */ + void set_l1_period(float period); + + /** + * Set the L1 damping factor. + * + * The original publication recommends a default of sqrt(2) / 2 = 0.707 + */ + void set_l1_damping(float damping); + + /** + * Set the maximum roll angle output in radians + */ + void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; } + + /** + * Set roll angle slew rate. Set to zero to deactivate. + */ + void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; } + + /** + * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting. + */ + void set_dt(float dt) { _dt = dt;} + +private: + + float _lateral_accel{0.0f}; ///< Lateral acceleration setpoint in m/s^2 + float _L1_distance{20.0f}; ///< L1 lead distance, defined by period and damping + bool _circle_mode{false}; ///< flag for loiter mode + float _nav_bearing{0.0f}; ///< bearing to L1 reference point + float _bearing_error{0.0f}; ///< bearing error + float _crosstrack_error{0.0f}; ///< crosstrack error in meters + float _target_bearing{0.0f}; ///< the heading setpoint + + float _L1_period{25.0f}; ///< L1 tracking period in seconds + float _L1_damping{0.75f}; ///< L1 damping ratio + float _L1_ratio{5.0f}; ///< L1 ratio for navigation + float _K_L1{2.0f}; ///< L1 control gain for _L1_damping + float _heading_omega{1.0f}; ///< Normalized frequency + + float _roll_lim_rad{math::radians(30.0f)}; /// + +#include + +using math::constrain; +using math::max; +using math::min; + +static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) +static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) + +/** + * @file TECS.cpp + * + * @author Paul Riseborough + */ + +/* + * This function implements a complementary filter to estimate the climb rate when + * inertial nav data is not available. It also calculates a true airspeed derivative + * which is used by the airspeed complimentary filter. + */ +void TECS::update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, + const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, + float altitude, float vz) +{ + // calculate the time lapsed since the last update + uint64_t now = hrt_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 (PX4_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 = hrt_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 (!PX4_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 (PX4_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 (PX4_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 = hrt_absolute_time(); + _dt = constrain((now - _pitch_update_timestamp) * 1e-6f, DT_MIN, DT_MAX); + + // Set class variables from inputs + _throttle_setpoint_max = throttle_max; + _throttle_setpoint_min = throttle_min; + _pitch_setpoint_max = pitch_limit_max; + _pitch_setpoint_min = pitch_limit_min; + _climbout_mode_active = climb_out_setpoint; + + // Initialize selected states and variables as required + _initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas); + + // Don't run TECS control algorithms when not in flight + if (!_in_air) { + return; + } + + // Update the true airspeed state estimate + _update_speed_states(EAS_setpoint, indicated_airspeed, eas_to_tas); + + // Calculate rate limits for specific total energy + _update_STE_rate_lim(); + + // Detect an underspeed condition + _detect_underspeed(); + + // Detect an uncommanded descent caused by an unachievable airspeed demand + _detect_uncommanded_descent(); + + // Calculate the demanded true airspeed + _update_speed_setpoint(); + + // Calculate the demanded height + _update_height_setpoint(hgt_setpoint, baro_altitude); + + // Calculate the specific energy values required by the control loop + _update_energy_estimates(); + + // Calculate the throttle demand + _update_throttle_setpoint(throttle_cruise, rotMat); + + // Calculate the pitch demand + _update_pitch_setpoint(); + + // Update time stamps + _pitch_update_timestamp = now; + + // Set TECS mode for next frame + if (_underspeed_detected) { + _tecs_mode = ECL_TECS_MODE_UNDERSPEED; + + } else if (_uncommanded_descent_recovery) { + _tecs_mode = ECL_TECS_MODE_BAD_DESCENT; + + } else if (_climbout_mode_active) { + _tecs_mode = ECL_TECS_MODE_CLIMBOUT; + + } else { + // This is the default operation mode + _tecs_mode = ECL_TECS_MODE_NORMAL; + } + +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp new file mode 100644 index 00000000000..ef491ecbe76 --- /dev/null +++ b/src/lib/tecs/TECS.hpp @@ -0,0 +1,327 @@ +/**************************************************************************** + * + * Copyright (c) 2017-2020 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 tecs.cpp + * + * @author Paul Riseborough + */ + +#pragma once + +#include +#include + +class TECS +{ +public: + TECS() = default; + ~TECS() = default; + + // no copy, assignment, move, move assignment + TECS(const TECS &) = delete; + TECS &operator=(const TECS &) = delete; + TECS(TECS &&) = delete; + TECS &operator=(TECS &&) = delete; + + /** + * Get the current airspeed status + * + * @return true if airspeed is enabled for control + */ + bool airspeed_sensor_enabled() { return _airspeed_enabled; } + + /** + * Set the airspeed enable state + */ + void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; } + + /** + * Updates the following vehicle kineamtic state estimates: + * Vertical position, velocity and acceleration. + * Speed derivative + * Must be called prior to udating tecs control loops + * Must be called at 50Hz or greater + */ + void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat, + const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air, + float altitude, float vz); + + /** + * Update the control loop calculations + */ + void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, + float EAS_setpoint, float indicated_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, + float throttle_min, float throttle_setpoint_max, float throttle_cruise, + float pitch_limit_min, float pitch_limit_max); + + float get_throttle_setpoint() { return _throttle_setpoint; } + float get_pitch_setpoint() { return _pitch_setpoint; } + float get_speed_weight() { return _pitch_speed_weight; } + + void reset_state() { _states_initialized = false; } + + enum ECL_TECS_MODE { + ECL_TECS_MODE_NORMAL = 0, + ECL_TECS_MODE_UNDERSPEED, + ECL_TECS_MODE_BAD_DESCENT, + ECL_TECS_MODE_CLIMBOUT + }; + + void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; } + + // setters for controller parameters + void set_time_const(float time_const) { _pitch_time_constant = time_const; } + void set_integrator_gain(float gain) { _integrator_gain = gain; } + + void set_min_sink_rate(float rate) { _min_sink_rate = rate; } + void set_max_sink_rate(float sink_rate) { _max_sink_rate = sink_rate; } + void set_max_climb_rate(float climb_rate) { _max_climb_rate = climb_rate; } + + void set_heightrate_ff(float heightrate_ff) { _height_setpoint_gain_ff = heightrate_ff; } + void set_heightrate_p(float heightrate_p) { _height_error_gain = heightrate_p; } + + void set_indicated_airspeed_max(float airspeed) { _indicated_airspeed_max = airspeed; } + void set_indicated_airspeed_min(float airspeed) { _indicated_airspeed_min = airspeed; } + + void set_pitch_damping(float damping) { _pitch_damping_gain = damping; } + void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; } + + void set_speed_comp_filter_omega(float omega) { _tas_estimate_freq = omega; } + void set_speed_weight(float weight) { _pitch_speed_weight = weight; } + void set_speedrate_p(float speedrate_p) { _speed_error_gain = speedrate_p; } + + void set_time_const_throt(float time_const_throt) { _throttle_time_constant = time_const_throt; } + void set_throttle_damp(float throttle_damp) { _throttle_damping_gain = throttle_damp; } + void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; } + + void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; } + + // TECS status + uint64_t timestamp() { return _pitch_update_timestamp; } + ECL_TECS_MODE tecs_mode() { return _tecs_mode; } + + float hgt_setpoint_adj() { return _hgt_setpoint_adj; } + float vert_pos_state() { return _vert_pos_state; } + + float TAS_setpoint_adj() { return _TAS_setpoint_adj; } + float tas_state() { return _tas_state; } + + float hgt_rate_setpoint() { return _hgt_rate_setpoint; } + float vert_vel_state() { return _vert_vel_state; } + + float TAS_rate_setpoint() { return _TAS_rate_setpoint; } + float speed_derivative() { return _speed_derivative; } + + float STE_error() { return _STE_error; } + float STE_rate_error() { return _STE_rate_error; } + + float SEB_error() { return _SEB_error; } + float SEB_rate_error() { return _SEB_rate_error; } + + float throttle_integ_state() { return _throttle_integ_state; } + float pitch_integ_state() { return _pitch_integ_state; } + + /** + * Handle the altitude reset + * + * If the estimation system resets the height in one discrete step this + * will gracefully even out the reset over time. + */ + void handle_alt_step(float delta_alt, float altitude) + { + // add height reset delta to all variables involved + // in filtering the demanded height + _hgt_setpoint_in_prev += delta_alt; + _hgt_setpoint_prev += delta_alt; + _hgt_setpoint_adj_prev += delta_alt; + + // reset height states + _vert_pos_state = altitude; + _vert_vel_state = 0.0f; + } + +private: + + enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; + + // timestamps + uint64_t _state_update_timestamp{0}; ///< last timestamp of the 50 Hz function call + uint64_t _speed_update_timestamp{0}; ///< last timestamp of the speed function call + uint64_t _pitch_update_timestamp{0}; ///< last timestamp of the pitch function call + + // controller parameters + float _tas_estimate_freq{0.0f}; ///< cross-over frequency of the true airspeed complementary filter (rad/sec) + float _max_climb_rate{2.0f}; ///< climb rate produced by max allowed throttle (m/sec) + float _min_sink_rate{1.0f}; ///< sink rate produced by min allowed throttle (m/sec) + float _max_sink_rate{2.0f}; ///< maximum safe sink rate (m/sec) + float _pitch_time_constant{5.0f}; ///< control time constant used by the pitch demand calculation (sec) + float _throttle_time_constant{8.0f}; ///< control time constant used by the throttle demand calculation (sec) + float _pitch_damping_gain{0.0f}; ///< damping gain of the pitch demand calculation (sec) + float _throttle_damping_gain{0.0f}; ///< damping gain of the throttle demand calculation (sec) + float _integrator_gain{0.0f}; ///< integrator gain used by the throttle and pitch demand calculation + float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2) + float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3) + float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation + float _height_error_gain{0.0f}; ///< gain from height error to demanded climb rate (1/sec) + float _height_setpoint_gain_ff{0.0f}; ///< gain from height demand derivative to demanded climb rate + float _speed_error_gain{0.0f}; ///< gain from speed error to demanded speed rate (1/sec) + float _indicated_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) + float _indicated_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) + float _throttle_slewrate{0.0f}; ///< throttle demand slew rate limit (1/sec) + + // controller outputs + float _throttle_setpoint{0.0f}; ///< normalized throttle demand (0..1) + float _pitch_setpoint{0.0f}; ///< pitch angle demand (radians) + + // complimentary filter states + float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec) + float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m) + float _tas_rate_state{0.0f}; ///< complimentary filter state - true airspeed first derivative (m/sec**2) + float _tas_state{0.0f}; ///< complimentary filter state - true airspeed (m/sec) + + // controller states + float _throttle_integ_state{0.0f}; ///< throttle integrator state + float _pitch_integ_state{0.0f}; ///< pitch integrator state (rad) + float _last_throttle_setpoint{0.0f}; ///< throttle demand rate limiter state (1/sec) + float _last_pitch_setpoint{0.0f}; ///< pitch demand rate limiter state (rad/sec) + float _speed_derivative{0.0f}; ///< rate of change of speed along X axis (m/sec**2) + + // speed demand calculations + float _EAS{0.0f}; ///< equivalent airspeed (m/sec) + float _TAS_max{30.0f}; ///< true airpeed demand upper limit (m/sec) + float _TAS_min{3.0f}; ///< true airpeed demand lower limit (m/sec) + float _TAS_setpoint{0.0f}; ///< current airpeed demand (m/sec) + float _TAS_setpoint_last{0.0f}; ///< previous true airpeed demand (m/sec) + float _EAS_setpoint{0.0f}; ///< Equivalent airspeed demand (m/sec) + float _TAS_setpoint_adj{0.0f}; ///< true airspeed demand tracked by the TECS algorithm (m/sec) + float _TAS_rate_setpoint{0.0f}; ///< true airspeed rate demand tracked by the TECS algorithm (m/sec**2) + + // height demand calculations + float _hgt_setpoint{0.0f}; ///< demanded height tracked by the TECS algorithm (m) + float _hgt_setpoint_in_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering (m) + float _hgt_setpoint_prev{0.0f}; ///< previous value of _hgt_setpoint after noise filtering and rate limiting (m) + float _hgt_setpoint_adj{0.0f}; ///< demanded height used by the control loops after all filtering has been applied (m) + float _hgt_setpoint_adj_prev{0.0f}; ///< value of _hgt_setpoint_adj from previous frame (m) + float _hgt_rate_setpoint{0.0f}; ///< demanded climb rate tracked by the TECS algorithm + + // vehicle physical limits + float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad) + float _STE_rate_max{0.0f}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3) + float _STE_rate_min{0.0f}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3) + float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit + float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit + float _pitch_setpoint_max{0.5f}; ///< pitch demand upper limit (rad) + float _pitch_setpoint_min{-0.5f}; ///< pitch demand lower limit (rad) + + // specific energy quantities + float _SPE_setpoint{0.0f}; ///< specific potential energy demand (m**2/sec**2) + float _SKE_setpoint{0.0f}; ///< specific kinetic energy demand (m**2/sec**2) + float _SPE_rate_setpoint{0.0f}; ///< specific potential energy rate demand (m**2/sec**3) + float _SKE_rate_setpoint{0.0f}; ///< specific kinetic energy rate demand (m**2/sec**3) + float _SPE_estimate{0.0f}; ///< specific potential energy estimate (m**2/sec**2) + float _SKE_estimate{0.0f}; ///< specific kinetic energy estimate (m**2/sec**2) + float _SPE_rate{0.0f}; ///< specific potential energy rate estimate (m**2/sec**3) + float _SKE_rate{0.0f}; ///< specific kinetic energy rate estimate (m**2/sec**3) + + // specific energy error quantities + float _STE_error{0.0f}; ///< specific total energy error (m**2/sec**2) + float _STE_rate_error{0.0f}; ///< specific total energy rate error (m**2/sec**3) + float _SEB_error{0.0f}; ///< specific energy balance error (m**2/sec**2) + float _SEB_rate_error{0.0f}; ///< specific energy balance rate error (m**2/sec**3) + + // time steps (non-fixed) + float _dt{DT_DEFAULT}; ///< Time since last update of main TECS loop (sec) + static constexpr float DT_DEFAULT = 0.02f; ///< default value for _dt (sec) + + // controller mode logic + bool _underspeed_detected{false}; ///< true when an underspeed condition has been detected + bool _detect_underspeed_enabled{true}; ///< true when underspeed detection is enabled + bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected + bool _climbout_mode_active{false}; ///< true when in climbout mode + bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled + bool _states_initialized{false}; ///< true when TECS states have been iniitalized + bool _in_air{false}; ///< true when the vehicle is flying + + /** + * Update the airspeed internal state using a second order complementary filter + */ + void _update_speed_states(float airspeed_setpoint, float indicated_airspeed, float eas_to_tas); + + /** + * Update the desired airspeed + */ + void _update_speed_setpoint(); + + /** + * Update the desired height + */ + void _update_height_setpoint(float desired, float state); + + /** + * Detect if the system is not capable of maintaining airspeed + */ + void _detect_underspeed(); + + /** + * Update specific energy + */ + void _update_energy_estimates(); + + /** + * Update throttle setpoint + */ + void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat); + + /** + * Detect an uncommanded descent + */ + void _detect_uncommanded_descent(); + + /** + * Update the pitch setpoint + */ + void _update_pitch_setpoint(); + + /** + * Initialize the controller + */ + void _initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout, + float eas_to_tas); + + /** + * Calculate specific total energy rate limits + */ + void _update_STE_rate_lim(); + +}; diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 920a635ee39..68e5b0a0f87 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -39,11 +39,11 @@ px4_add_module( MAIN fw_pos_control_l1 SRCS FixedwingPositionControl.cpp + FixedwingPositionControl.hpp DEPENDS - git_ecl - ecl_l1 - ecl_tecs + l1 launchdetection landing_slope runway_takeoff + tecs ) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index e21fb3e3ea9..7d4fadb3936 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -55,8 +55,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/src/modules/rover_pos_control/CMakeLists.txt b/src/modules/rover_pos_control/CMakeLists.txt index f63908a2567..5e093862e76 100644 --- a/src/modules/rover_pos_control/CMakeLists.txt +++ b/src/modules/rover_pos_control/CMakeLists.txt @@ -35,8 +35,8 @@ px4_add_module( MAIN rover_pos_control SRCS RoverPositionControl.cpp + RoverPositionControl.hpp DEPENDS - git_ecl - ecl_l1 + l1 pid ) diff --git a/src/modules/rover_pos_control/RoverPositionControl.hpp b/src/modules/rover_pos_control/RoverPositionControl.hpp index 1e1c71fe0a0..62622ef15d0 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.hpp +++ b/src/modules/rover_pos_control/RoverPositionControl.hpp @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include #include diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 7859d4d839c..71f67fb571e 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +add_subdirectory(data_validator) + add_subdirectory(sensor_calibration) # used by vehicle_{acceleration, angular_velocity, imu} include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(vehicle_acceleration) @@ -48,9 +50,9 @@ px4_add_module( DEPENDS airspeed conversion + data_validator drivers__device git_ecl - ecl_validation mathlib vehicle_acceleration vehicle_angular_velocity diff --git a/src/modules/sensors/data_validator/CMakeLists.txt b/src/modules/sensors/data_validator/CMakeLists.txt new file mode 100644 index 00000000000..e60b18d4b30 --- /dev/null +++ b/src/modules/sensors/data_validator/CMakeLists.txt @@ -0,0 +1,39 @@ +############################################################################ +# +# Copyright (c) 2018-2020 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. +# +############################################################################ + +px4_add_library(data_validator + DataValidator.cpp + DataValidator.hpp + DataValidatorGroup.cpp + DataValidatorGroup.hpp +) diff --git a/src/modules/sensors/data_validator/DataValidator.cpp b/src/modules/sensors/data_validator/DataValidator.cpp new file mode 100644 index 00000000000..3b0b506a70e --- /dev/null +++ b/src/modules/sensors/data_validator/DataValidator.cpp @@ -0,0 +1,153 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2020 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 DataValidator.cpp + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "DataValidator.hpp" + +#include + +void DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in) +{ + float data[dimensions] = {val}; // sets the first value and all others to 0 + put(timestamp, data, error_count_in, priority_in); +} + +void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in) +{ + + _event_count++; + + if (error_count_in > _error_count) { + _error_density += (error_count_in - _error_count); + + } else if (_error_density > 0) { + _error_density--; + } + + _error_count = error_count_in; + _priority = priority_in; + + for (unsigned i = 0; i < dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.99f + 0.01f * val[i]; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} + +float DataValidator::confidence(uint64_t timestamp) +{ + + float ret = 1.0f; + + /* check if we have any data */ + if (_time_last == 0) { + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; + + } else if (timestamp - _time_last > _timeout_interval) { + /* timed out - that's it */ + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 0.0f; + + } else if (_value_equal_count > _value_equal_count_threshold) { + /* we got the exact same sensor value N times in a row */ + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; + + } else if (_error_count > NORETURN_ERRCOUNT) { + /* check error count limit */ + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; + + } else if (_error_density > ERROR_DENSITY_WINDOW) { + /* cap error density counter at window size */ + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; + _error_density = ERROR_DENSITY_WINDOW; + } + + /* no critical errors */ + if (ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + + if (ret > 0.0f) { + _error_mask = ERROR_FLAG_NO_ERROR; + } + } + + return ret; +} + +void DataValidator::print() +{ + if (_time_last == 0) { + PX4_INFO("\tno data"); + return; + } + + for (unsigned i = 0; i < dimensions; i++) { + PX4_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(hrt_absolute_time())); + } +} diff --git a/src/modules/sensors/data_validator/DataValidator.hpp b/src/modules/sensors/data_validator/DataValidator.hpp new file mode 100644 index 00000000000..662a3032510 --- /dev/null +++ b/src/modules/sensors/data_validator/DataValidator.hpp @@ -0,0 +1,200 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2020 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 DataValidator.hpp + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include +#include + +class DataValidator +{ +public: + static const unsigned dimensions = 3; + + DataValidator() = default; + ~DataValidator() = default; + + /** + * Put an item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, float val, uint64_t error_count, int priority); + + /** + * Put a 3D item into the validator. + * + * @param val Item to put + */ + void put(uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority); + + /** + * Get the next sibling in the group + * + * @return the next sibling + */ + DataValidator *sibling() { return _sibling; } + + /** + * Set the sibling to the next node in the group + * + */ + void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; } + + /** + * Get the confidence of this validator + * @return the confidence between 0 and 1 + */ + float confidence(uint64_t timestamp); + + /** + * Get the error count of this validator + * @return the error count + */ + uint64_t error_count() const { return _error_count; } + + /** + * Get the values of this validator + * @return the stored value + */ + float *value() { return _value; } + + /** + * Get the used status of this validator + * @return true if this validator ever saw data + */ + bool used() const { return (_time_last > 0); } + + /** + * Get the priority of this validator + * @return the stored priority + */ + int priority() const { return _priority; } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() const { return _error_mask; } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } + + /** + * Get the RMS values of this validator + * @return the stored RMS + */ + float *rms() { return _rms; } + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * Set the equal count threshold + * + * @param threshold The number of equal values before considering the sensor stale + */ + void set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; } + + /** + * Get the timeout value + * + * @return The timeout interval in microseconds + */ + uint32_t get_timeout() const { return _timeout_interval; } + + /** + * Data validator error states + */ + static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U); + static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U); + static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1); + static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3); + static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); + +private: + uint32_t _error_mask{ERROR_FLAG_NO_ERROR}; /**< sensor error state */ + + uint32_t _timeout_interval{20000}; /**< interval in which the datastream times out in us */ + + uint64_t _time_last{0}; /**< last timestamp */ + uint64_t _event_count{0}; /**< total data counter */ + uint64_t _error_count{0}; /**< error count */ + + int _error_density{0}; /**< ratio between successful reads and errors */ + + int _priority{0}; /**< sensor nominal priority */ + + float _mean[dimensions] {}; /**< mean of value */ + float _lp[dimensions] {}; /**< low pass value */ + float _M2[dimensions] {}; /**< RMS component value */ + float _rms[dimensions] {}; /**< root mean square error */ + float _value[dimensions] {}; /**< last value */ + + unsigned _value_equal_count{0}; /**< equal values in a row */ + unsigned _value_equal_count_threshold{ + VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */ + + DataValidator *_sibling{nullptr}; /**< sibling in the group */ + + static const constexpr unsigned NORETURN_ERRCOUNT = + 10000; /**< if the error count reaches this value, return sensor as invalid */ + static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ + static const constexpr unsigned VALUE_EQUAL_COUNT_DEFAULT = + 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ + + /* we don't want this class to be copied */ + DataValidator(const DataValidator &) = delete; + DataValidator operator=(const DataValidator &) = delete; +}; diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.cpp b/src/modules/sensors/data_validator/DataValidatorGroup.cpp new file mode 100644 index 00000000000..e9f10269e31 --- /dev/null +++ b/src/modules/sensors/data_validator/DataValidatorGroup.cpp @@ -0,0 +1,307 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2020 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 DataValidatorGroup.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#include "DataValidatorGroup.hpp" + +#include + +#include + +DataValidatorGroup::DataValidatorGroup(unsigned siblings) +{ + + DataValidator *next = nullptr; + DataValidator *prev = nullptr; + + for (unsigned i = 0; i < siblings; i++) { + next = new DataValidator(); + + if (i == 0) { + _first = next; + + } else { + prev->setSibling(next); + } + + prev = next; + } + + _last = next; + + if (_first) { + _timeout_interval_us = _first->get_timeout(); + } +} + +DataValidatorGroup::~DataValidatorGroup() +{ + while (_first) { + DataValidator *next = _first->sibling(); + delete (_first); + _first = next; + } +} + +DataValidator *DataValidatorGroup::add_new_validator() +{ + + DataValidator *validator = new DataValidator(); + + if (!validator) { + return nullptr; + } + + _last->setSibling(validator); + _last = validator; + _last->set_timeout(_timeout_interval_us); + return _last; +} + +void DataValidatorGroup::set_timeout(uint32_t timeout_interval_us) +{ + + DataValidator *next = _first; + + while (next != nullptr) { + next->set_timeout(timeout_interval_us); + next = next->sibling(); + } + + _timeout_interval_us = timeout_interval_us; +} + +void DataValidatorGroup::set_equal_value_threshold(uint32_t threshold) +{ + + DataValidator *next = _first; + + while (next != nullptr) { + next->set_equal_value_threshold(threshold); + next = next->sibling(); + } +} + +void DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, + int priority) +{ + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + next->put(timestamp, val, error_count, priority); + break; + } + + next = next->sibling(); + i++; + } +} + +float *DataValidatorGroup::get_best(uint64_t timestamp, int *index) +{ + + DataValidator *next = _first; + + // XXX This should eventually also include voting + int pre_check_best = _curr_best; + float pre_check_confidence = 1.0f; + int pre_check_prio = -1; + float max_confidence = -1.0f; + int max_priority = -1000; + int max_index = -1; + DataValidator *best = nullptr; + + int i = 0; + + while (next != nullptr) { + float confidence = next->confidence(timestamp); + + if (i == pre_check_best) { + pre_check_prio = next->priority(); + pre_check_confidence = confidence; + } + + /* + * Switch if: + * 1) the confidence is higher and priority is equal or higher + * 2) the confidence is no less than 1% different and the priority is higher + */ + if ((((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || + (confidence > max_confidence && (next->priority() >= max_priority)) || + (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) && + (confidence > 0.0f)) { + max_index = i; + max_confidence = confidence; + max_priority = next->priority(); + best = next; + } + + next = next->sibling(); + i++; + } + + /* the current best sensor is not matching the previous best sensor, + * or the only sensor went bad */ + if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) { + bool true_failsafe = true; + + /* check whether the switch was a failsafe or preferring a higher priority sensor */ + if (pre_check_prio != -1 && pre_check_prio < max_priority && + fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ + true_failsafe = false; + + /* reset error flags, this is likely a hotplug sensor coming online late */ + if (best != nullptr) { + best->reset_state(); + } + } + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + + if (true_failsafe) { + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + + *index = max_index; + return (best) ? best->value() : nullptr; +} + +void DataValidatorGroup::print() +{ + PX4_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(); + + PX4_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ECNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " EDNST" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + + next->print(); + } + + next = next->sibling(); + i++; + } +} + +int DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && + (i == (unsigned)_prev_best)) { + return i; + } + + next = next->sibling(); + i++; + } + + return -1; +} + +uint32_t DataValidatorGroup::failover_state() +{ + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && + (i == (unsigned)_prev_best)) { + return next->state(); + } + + next = next->sibling(); + i++; + } + + return DataValidator::ERROR_FLAG_NO_ERROR; +} + +uint32_t DataValidatorGroup::get_sensor_state(unsigned index) +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + return next->state(); + } + + next = next->sibling(); + i++; + } + + // sensor index not found + return UINT32_MAX; +} diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.hpp b/src/modules/sensors/data_validator/DataValidatorGroup.hpp new file mode 100644 index 00000000000..6dc7d766c9f --- /dev/null +++ b/src/modules/sensors/data_validator/DataValidatorGroup.hpp @@ -0,0 +1,145 @@ +/**************************************************************************** + * + * Copyright (c) 2015-2020 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 DataValidatorGroup.hpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +#pragma once + +#include "DataValidator.hpp" + +class DataValidatorGroup +{ +public: + /** + * @param siblings initial number of DataValidator's. Must be > 0. + */ + DataValidatorGroup(unsigned siblings); + ~DataValidatorGroup(); + + /** + * Create a new Validator (with index equal to the number of currently existing validators) + * @return the newly created DataValidator or nullptr on error + */ + DataValidator *add_new_validator(); + + /** + * Put an item into the validator group. + * + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor + */ + void put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority); + + /** + * Get the best data triplet of the group + * + * @return pointer to the array of best values + */ + float *get_best(uint64_t timestamp, int *index); + + /** + * Get the number of failover events + * + * @return the number of failovers + */ + unsigned failover_count() const { return _toggle_count; } + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + int failover_index(); + + /** + * Get the error state of the failed sensor in the group + * + * @return bitmask with erro states of the failed sensor + */ + uint32_t failover_state(); + + /** + * Get the error state of the sensor with the specified index + * + * @return bitmask with error states of the sensor + */ + uint32_t get_sensor_state(unsigned index); + + /** + * Print the validator value + * + */ + void print(); + + /** + * Set the timeout value for the whole group + * + * @param timeout_interval_us The timeout interval in microseconds + */ + void set_timeout(uint32_t timeout_interval_us); + + /** + * Set the equal count threshold for the whole group + * + * @param threshold The number of equal values before considering the sensor stale + */ + void set_equal_value_threshold(uint32_t threshold); + +private: + DataValidator *_first{nullptr}; /**< first node in the group */ + DataValidator *_last{nullptr}; /**< last node in the group */ + + uint32_t _timeout_interval_us{0}; /**< currently set timeout */ + + int _curr_best{-1}; /**< currently best index */ + int _prev_best{-1}; /**< the previous best index */ + + uint64_t _first_failover_time{0}; /**< timestamp where the first failover occured or zero if none occured */ + + unsigned _toggle_count{0}; /**< number of back and forth switches between two sensors */ + + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; + + /* we don't want this class to be copied */ + DataValidatorGroup(const DataValidatorGroup &); + DataValidatorGroup operator=(const DataValidatorGroup &); +}; diff --git a/src/modules/sensors/data_validator/tests/CMakeLists.txt b/src/modules/sensors/data_validator/tests/CMakeLists.txt new file mode 100644 index 00000000000..641b178b636 --- /dev/null +++ b/src/modules/sensors/data_validator/tests/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +add_executable(ecl_tests_data_validator test_data_validator.cpp tests_common.cpp) +target_link_libraries(ecl_tests_data_validator ecl_validation) + +add_test(NAME ecl_tests_data_validator + COMMAND ecl_tests_data_validator + ) + +add_executable(ecl_tests_data_validator_group test_data_validator_group.cpp tests_common.cpp) +target_link_libraries(ecl_tests_data_validator_group ecl_validation) + +add_test(NAME ecl_tests_data_validator_group + COMMAND ecl_tests_data_validator_group + ) diff --git a/src/modules/sensors/data_validator/tests/test_data_validator.cpp b/src/modules/sensors/data_validator/tests/test_data_validator.cpp new file mode 100644 index 00000000000..fa16fc89519 --- /dev/null +++ b/src/modules/sensors/data_validator/tests/test_data_validator.cpp @@ -0,0 +1,302 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file test_data_validator.cpp + * Testing the DataValidator class + * + * @author Todd Stellanova + */ + +#include +#include +#include +#include +#include +#include +#include + + +void test_init() +{ + printf("\n--- test_init ---\n"); + + uint64_t fake_timestamp = 666; + const uint32_t timeout_usec = 2000;//from original private value + + DataValidator *validator = new DataValidator; + // initially there should be no siblings + assert(nullptr == validator->sibling()); + // initially we should have zero confidence + assert(0.0f == validator->confidence(fake_timestamp)); + // initially the error count should be zero + assert(0 == validator->error_count()); + // initially unused + assert(!validator->used()); + // initially no priority + assert(0 == validator->priority()); + validator->set_timeout(timeout_usec); + assert(validator->get_timeout() == timeout_usec); + + + DataValidator *sibling_validator = new DataValidator; + validator->setSibling(sibling_validator); + assert(sibling_validator == validator->sibling()); + + //verify that with no data, confidence is zero and error mask is set + assert(0.0f == validator->confidence(fake_timestamp + 1)); + uint32_t state = validator->state(); + assert(DataValidator::ERROR_FLAG_NO_DATA == (DataValidator::ERROR_FLAG_NO_DATA & state)); + + //verify that calling print doesn't crash tests + validator->print(); + + delete validator; //force delete +} + +void test_put() +{ + printf("\n--- test_put ---\n"); + + uint64_t timestamp = 500; + const uint32_t timeout_usec = 2000;//derived from class-private value + float val = 3.14159f; + //derived from class-private value: this is min change needed to avoid stale detection + const float sufficient_incr_value = (1.1f * 1E-6f); + + DataValidator *validator = new DataValidator; + fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); + + assert(validator->used()); + //verify that the last value we inserted is the current validator value + float last_val = val - sufficient_incr_value; + assert(validator->value()[0] == last_val); + + // we've just provided a bunch of valid data: should be fully confident + float conf = validator->confidence(timestamp); + + if (1.0f != conf) { + printf("conf: %f\n", (double)conf); + dump_validator_state(validator); + } + + assert(1.0f == conf); + // should be no errors + assert(0 == validator->state()); + + //now check confidence much beyond the timeout window-- should timeout + conf = validator->confidence(timestamp + (1.1 * timeout_usec)); + + if (0.0f != conf) { + printf("conf: %f\n", (double)conf); + dump_validator_state(validator); + } + + assert(0.0f == conf); + assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); + + delete validator; //force delete +} + +/** + * Verify that the DataValidator detects sensor data that does not vary sufficiently + */ +void test_stale_detector() +{ + printf("\n--- test_stale_detector ---\n"); + + uint64_t timestamp = 500; + float val = 3.14159f; + //derived from class-private value, this is insufficient to avoid stale detection: + const float insufficient_incr_value = (0.99 * 1E-6f); + + DataValidator *validator = new DataValidator; + fill_validator_with_samples(validator, insufficient_incr_value, &val, ×tamp); + + // data is stale: should have no confidence + assert(0.0f == validator->confidence(timestamp)); + + // should be a stale error + uint32_t state = validator->state(); + + if (DataValidator::ERROR_FLAG_STALE_DATA != state) { + dump_validator_state(validator); + } + + assert(DataValidator::ERROR_FLAG_STALE_DATA == (DataValidator::ERROR_FLAG_STALE_DATA & state)); + + delete validator; //force delete +} + +/** + * Verify the RMS error calculated by the DataValidator for a series of samples + */ +void test_rms_calculation() +{ + printf("\n--- test_rms_calculation ---\n"); + const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT + const float mean_value = 3.14159f; + const uint32_t sample_count = 1000; + float expected_rms_err = 0.0f; + uint64_t timestamp = 500; + + DataValidator *validator = new DataValidator; + validator->set_equal_value_threshold(equal_value_count); + + insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, ×tamp); + float *rms = validator->rms(); + assert(nullptr != rms); + float calc_rms_err = rms[0]; + float diff = fabsf(calc_rms_err - expected_rms_err); + float diff_frac = (diff / expected_rms_err); + printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err, + (double)diff, (double)diff_frac); + assert(diff_frac < 0.03f); + + delete validator; //force delete +} + +/** + * Verify error tracking performed by DataValidator::put + */ +void test_error_tracking() +{ + printf("\n--- test_error_tracking ---\n"); + uint64_t timestamp = 500; + uint64_t timestamp_incr = 5; + const uint32_t timeout_usec = 2000;//from original private value + float val = 3.14159f; + uint64_t error_count = 0; + int expected_error_density = 0; + int priority = 50; + //from private value: this is min change needed to avoid stale detection + const float sufficient_incr_value = (1.1f * 1E-6f); + //default is private VALUE_EQUAL_COUNT_DEFAULT + const int equal_value_count = 50000; + //should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT + const int total_iterations = 1000; + + DataValidator *validator = new DataValidator; + validator->set_timeout(timeout_usec); + validator->set_equal_value_threshold(equal_value_count); + + //put a bunch of values that are all different + for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) { + timestamp += timestamp_incr; + + //up to a 50% random error rate appears to pass the error density filter + if ((((float)rand() / (float)RAND_MAX)) < 0.500f) { + error_count += 1; + expected_error_density += 1; + + } else if (expected_error_density > 0) { + expected_error_density -= 1; + } + + validator->put(timestamp, val, error_count, priority); + } + + assert(validator->used()); + //at this point, error_count should be less than NORETURN_ERRCOUNT + assert(validator->error_count() == error_count); + + // we've just provided a bunch of valid data with some errors: + // confidence should be reduced by the number of errors + float conf = validator->confidence(timestamp); + printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf); + assert(1.0f != conf); //we should not be fully confident + assert(0.0f != conf); //neither should we be completely unconfident + // should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT + assert(0 == validator->state()); + + // the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW) + // ERROR_DENSITY_WINDOW is currently private, but == 100.0f + float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f); + double diff = fabs(reduced_conf - conf); + + if (reduced_conf != conf) { + printf("conf: %f reduced_conf: %f diff: %f\n", + (double)conf, (double)reduced_conf, diff); + dump_validator_state(validator); + } + + assert(diff < 1E-6f); + + //Now, insert a series of errors and ensure we trip the error detector + for (int i = 0; i < 250; i++, val += sufficient_incr_value) { + timestamp += timestamp_incr; + //100% error rate + error_count += 1; + expected_error_density += 1; + validator->put(timestamp, val, error_count, priority); + } + + conf = validator->confidence(timestamp); + assert(0.0f == conf); // should we be completely unconfident + // we should have triggered the high error density detector + assert(DataValidator::ERROR_FLAG_HIGH_ERRDENSITY == (DataValidator::ERROR_FLAG_HIGH_ERRDENSITY & validator->state())); + + + validator->reset_state(); + + //Now insert so many errors that we exceed private NORETURN_ERRCOUNT + for (int i = 0; i < 10000; i++, val += sufficient_incr_value) { + timestamp += timestamp_incr; + //100% error rate + error_count += 1; + expected_error_density += 1; + validator->put(timestamp, val, error_count, priority); + } + + conf = validator->confidence(timestamp); + assert(0.0f == conf); // should we be completely unconfident + // we should have triggered the high error count detector + assert(DataValidator::ERROR_FLAG_HIGH_ERRCOUNT == (DataValidator::ERROR_FLAG_HIGH_ERRCOUNT & validator->state())); + + delete validator; //force delete + +} + +int main(int argc, char *argv[]) +{ + (void)argc; // unused + (void)argv; // unused + + srand(666); + test_init(); + test_put(); + test_stale_detector(); + test_rms_calculation(); + test_error_tracking(); + + return 0; //passed +} diff --git a/src/modules/sensors/data_validator/tests/test_data_validator_group.cpp b/src/modules/sensors/data_validator/tests/test_data_validator_group.cpp new file mode 100644 index 00000000000..11f8950e110 --- /dev/null +++ b/src/modules/sensors/data_validator/tests/test_data_validator_group.cpp @@ -0,0 +1,385 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file test_data_validator_group.cpp + * Testing the DataValidatorGroup class + * + * @author Todd Stellanova + */ + +#include +#include +#include +#include +#include +#include +#include +#include + + +const uint32_t base_timeout_usec = 2000;//from original private value +const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT +const uint64_t base_timestamp = 666; +const unsigned base_num_siblings = 4; + + +/** + * Initialize a DataValidatorGroup with some common settings; + * @param sibling_count (out) the number of siblings initialized + */ +DataValidatorGroup *setup_base_group(unsigned *sibling_count) +{ + unsigned num_siblings = base_num_siblings; + + DataValidatorGroup *group = new DataValidatorGroup(num_siblings); + assert(nullptr != group); + //verify that calling print doesn't crash the tests + group->print(); + printf("\n"); + + //should be no failovers yet + assert(0 == group->failover_count()); + assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); + assert(-1 == group->failover_index()); + + //this sets the timeout on all current members of the group, as well as members added later + group->set_timeout(base_timeout_usec); + //the following sets the threshold on all CURRENT members of the group, but not any added later + //TODO This is likely a bug in DataValidatorGroup + group->set_equal_value_threshold(equal_value_count); + + //return values + *sibling_count = num_siblings; + + return group; +} + +/** + * Fill one DataValidator with samples, by index. + * + * @param group + * @param val1_idx Index of the validator to fill with samples + * @param num_samples + */ +void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples) +{ + uint64_t timestamp = base_timestamp; + uint64_t error_count = 0; + float last_best_val = 0.0f; + + for (uint32_t i = 0; i < num_samples; i++) { + float val = ((float) rand() / (float) RAND_MAX); + float data[DataValidator::dimensions] = {val}; + group->put(val1_idx, timestamp, data, error_count, 100); + last_best_val = val; + } + + int best_idx = 0; + float *best_data = group->get_best(timestamp, &best_idx); + assert(last_best_val == best_data[0]); + assert(best_idx == val1_idx); +} + + + +/** + * Fill two validators in the group with samples, by index. + * Both validators will be filled with the same data, but + * the priority of the first validator will be higher than the second. + * + * @param group + * @param val1_idx index of the first validator to fill + * @param val2_idx index of the second validator to fill + * @param num_samples + */ +void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples) +{ + uint64_t timestamp = base_timestamp; + uint64_t error_count = 0; + float last_best_val = 0.0f; + + for (uint32_t i = 0; i < num_samples; i++) { + float val = ((float) rand() / (float) RAND_MAX); + float data[DataValidator::dimensions] = {val}; + //two sensors with identical values, but different priorities + group->put(val1_idx, timestamp, data, error_count, 100); + group->put(val2_idx, timestamp, data, error_count, 10); + last_best_val = val; + } + + int best_idx = 0; + float *best_data = group->get_best(timestamp, &best_idx); + assert(last_best_val == best_data[0]); + assert(best_idx == val1_idx); + +} + +/** + * Dynamically add a validator to the group after construction + * @param group + * @return + */ +DataValidator *add_validator_to_group(DataValidatorGroup *group) +{ + DataValidator *validator = group->add_new_validator(); + //verify the previously set timeout applies to the new group member + assert(validator->get_timeout() == base_timeout_usec); + //for testing purposes, ensure this newly added member is consistent with the rest of the group + //TODO this is likely a bug in DataValidatorGroup + validator->set_equal_value_threshold(equal_value_count); + + return validator; +} + +/** + * Create a DataValidatorGroup and tack on two additional DataValidators + * + * @param validator1_handle (out) first DataValidator added to the group + * @param validator2_handle (out) second DataValidator added to the group + * @param sibling_count (in/out) in: number of initial siblings to create, out: total + * @return + */ +DataValidatorGroup *setup_group_with_two_validator_handles( + DataValidator **validator1_handle, + DataValidator **validator2_handle, + unsigned *sibling_count) +{ + DataValidatorGroup *group = setup_base_group(sibling_count); + + //now we add validators + *validator1_handle = add_validator_to_group(group); + *validator2_handle = add_validator_to_group(group); + *sibling_count += 2; + + return group; +} + + +void test_init() +{ + unsigned num_siblings = 0; + + DataValidatorGroup *group = setup_base_group(&num_siblings); + + //should not yet be any best value + int best_index = -1; + assert(nullptr == group->get_best(base_timestamp, &best_index)); + + delete group; //force cleanup +} + + +/** + * Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority + */ +void test_put() +{ + unsigned num_siblings = 0; + DataValidator *validator1 = nullptr; + DataValidator *validator2 = nullptr; + + uint64_t timestamp = base_timestamp; + + DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); + printf("num_siblings: %d \n", num_siblings); + unsigned val1_idx = num_siblings - 2; + unsigned val2_idx = num_siblings - 1; + + fill_two_with_valid_data(group, val1_idx, val2_idx, 500); + int best_idx = -1; + float *best_data = group->get_best(timestamp, &best_idx); + assert(nullptr != best_data); + float best_val = best_data[0]; + + float *cur_val1 = validator1->value(); + assert(nullptr != cur_val1); + //printf("cur_val1 %p \n", cur_val1); + assert(best_val == cur_val1[0]); + + float *cur_val2 = validator2->value(); + assert(nullptr != cur_val2); + //printf("cur_val12 %p \n", cur_val2); + assert(best_val == cur_val2[0]); + + delete group; //force cleanup +} + + +/** + * Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best". + */ +void test_priority_switch() +{ + unsigned num_siblings = 0; + DataValidator *validator1 = nullptr; + DataValidator *validator2 = nullptr; + + uint64_t timestamp = base_timestamp; + + DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); + //printf("num_siblings: %d \n",num_siblings); + int val1_idx = (int)num_siblings - 2; + int val2_idx = (int)num_siblings - 1; + uint64_t error_count = 0; + + fill_two_with_valid_data(group, val1_idx, val2_idx, 100); + + int best_idx = -1; + float *best_data = nullptr; + //now, switch the priorities, which switches "best" but doesn't trigger a failover + float new_best_val = 3.14159f; + float data[DataValidator::dimensions] = {new_best_val}; + //a single sample insertion should be sufficient to trigger a priority switch + group->put(val1_idx, timestamp, data, error_count, 1); + group->put(val2_idx, timestamp, data, error_count, 100); + best_data = group->get_best(timestamp, &best_idx); + assert(new_best_val == best_data[0]); + //the new best sensor should now be the sensor with the higher priority + assert(best_idx == val2_idx); + //should not have detected a real failover + assert(0 == group->failover_count()); + + delete group; //cleanup +} + +/** + * Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors + */ +void test_simple_failover() +{ + unsigned num_siblings = 0; + DataValidator *validator1 = nullptr; + DataValidator *validator2 = nullptr; + + uint64_t timestamp = base_timestamp; + + DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings); + //printf("num_siblings: %d \n",num_siblings); + int val1_idx = (int)num_siblings - 2; + int val2_idx = (int)num_siblings - 1; + + + fill_two_with_valid_data(group, val1_idx, val2_idx, 100); + + int best_idx = -1; + float *best_data = nullptr; + + //trigger a real failover + float new_best_val = 3.14159f; + float data[DataValidator::dimensions] = {new_best_val}; + //trigger a bunch of errors on the previous best sensor + unsigned val1_err_count = 0; + + for (int i = 0; i < 25; i++) { + group->put(val1_idx, timestamp, data, ++val1_err_count, 100); + group->put(val2_idx, timestamp, data, 0, 10); + } + + assert(validator1->error_count() == val1_err_count); + + //since validator1 is experiencing errors, we should see a failover to validator2 + best_data = group->get_best(timestamp + 1, &best_idx); + assert(nullptr != best_data); + assert(new_best_val == best_data[0]); + assert(best_idx == val2_idx); + //should have detected a real failover + printf("failover_count: %d \n", group->failover_count()); + assert(1 == group->failover_count()); + + //even though validator1 has encountered a bunch of errors, it hasn't failed + assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state()); + + // although we failed over from one sensor to another, this is not the same thing tracked by failover_index + int fail_idx = group->failover_index(); + assert(-1 == fail_idx);//no failed sensor + + //since no sensor has actually hard-failed, the group failover state is NO_ERROR + assert(DataValidator::ERROR_FLAG_NO_ERROR == group->failover_state()); + + + delete group; //cleanup +} + +/** + * Force once sensor to fail and ensure that we detect it + */ +void test_sensor_failure() +{ + unsigned num_siblings = 0; + uint64_t timestamp = base_timestamp; + const float sufficient_incr_value = (1.1f * 1E-6f); + const uint32_t timeout_usec = 2000;//derived from class-private value + + float val = 3.14159f; + + DataValidatorGroup *group = setup_base_group(&num_siblings); + + //now we add validators + DataValidator *validator = add_validator_to_group(group); + assert(nullptr != validator); + num_siblings++; + int val_idx = num_siblings - 1; + + fill_validator_with_samples(validator, sufficient_incr_value, &val, ×tamp); + //the best should now be the one validator we've filled with samples + + int best_idx = -1; + float *best_data = group->get_best(timestamp, &best_idx); + assert(nullptr != best_data); + //printf("best_idx: %d val_idx: %d\n", best_idx, val_idx); + assert(best_idx == val_idx); + + //now force a timeout failure in the one validator, by checking confidence long past timeout + validator->confidence(timestamp + (1.1 * timeout_usec)); + assert(DataValidator::ERROR_FLAG_TIMEOUT == (DataValidator::ERROR_FLAG_TIMEOUT & validator->state())); + + //now that the one sensor has failed, the group should detect this as well + int fail_idx = group->failover_index(); + assert(val_idx == fail_idx); + + delete group; +} + +int main(int argc, char *argv[]) +{ + (void)argc; // unused + (void)argv; // unused + + test_init(); + test_put(); + test_simple_failover(); + test_priority_switch(); + test_sensor_failure(); + + return 0; //passed +} diff --git a/src/modules/sensors/data_validator/tests/tests_common.cpp b/src/modules/sensors/data_validator/tests/tests_common.cpp new file mode 100644 index 00000000000..bcf18ba3696 --- /dev/null +++ b/src/modules/sensors/data_validator/tests/tests_common.cpp @@ -0,0 +1,103 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include "tests_common.h" + + +void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, + uint64_t *timestamp_io) +{ + uint64_t timestamp = *timestamp_io; + uint64_t timestamp_incr = 5; + const uint64_t error_count = 0; + const int priority = 50; + const float swing = 1E-2f; + double sum_dev_squares = 0.0f; + + //insert a series of values that swing around the mean + for (uint32_t i = 0; i < count; i++) { + float iter_swing = (0 == (i % 2)) ? swing : -swing; + float iter_val = mean + iter_swing; + float iter_dev = iter_val - mean; + sum_dev_squares += (iter_dev * iter_dev); + timestamp += timestamp_incr; + validator->put(timestamp, iter_val, error_count, priority); + } + + double rms = sqrt(sum_dev_squares / (double)count); + //note: this should be approximately equal to "swing" + *rms_err = (float)rms; + *timestamp_io = timestamp; +} + +void dump_validator_state(DataValidator *validator) +{ + uint32_t state = validator->state(); + printf("state: 0x%x no_data: %d stale: %d timeout:%d\n", + validator->state(), + DataValidator::ERROR_FLAG_NO_DATA & state, + DataValidator::ERROR_FLAG_STALE_DATA & state, + DataValidator::ERROR_FLAG_TIMEOUT & state + ); + validator->print(); + printf("\n"); +} + +void fill_validator_with_samples(DataValidator *validator, + const float incr_value, + float *value_io, + uint64_t *timestamp_io) +{ + uint64_t timestamp = *timestamp_io; + const uint64_t timestamp_incr = 5; //usec + const uint32_t timeout_usec = 2000;//derived from class-private value + + float val = *value_io; + const uint64_t error_count = 0; + const int priority = 50; //"medium" priority + const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT + + validator->set_equal_value_threshold(equal_value_count); + validator->set_timeout(timeout_usec); + + //put a bunch of values that are all different + for (int i = 0; i < equal_value_count; i++, val += incr_value) { + timestamp += timestamp_incr; + validator->put(timestamp, val, error_count, priority); + } + + *timestamp_io = timestamp; + *value_io = val; + +} \ No newline at end of file diff --git a/src/modules/sensors/data_validator/tests/tests_common.h b/src/modules/sensors/data_validator/tests/tests_common.h new file mode 100644 index 00000000000..ae1351dfe11 --- /dev/null +++ b/src/modules/sensors/data_validator/tests/tests_common.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2019 Todd Stellanova. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#ifndef ECL_TESTS_COMMON_H +#define ECL_TESTS_COMMON_H + +#include + +/** + * Insert a series of samples around a mean value + * @param validator The validator under test + * @param mean The mean value + * @param count The number of samples to insert in the validator + * @param rms_err (out) calculated rms error of the inserted samples + * @param timestamp_io (in/out) in: start timestamp, out: last timestamp + */ +void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, + uint64_t *timestamp_io); + +/** + * Print out the state of a DataValidator + * @param validator + */ +void dump_validator_state(DataValidator *validator); + +/** + * Insert a time series of samples into the validator + * @param validator + * @param incr_value The amount to increment the value by on each iteration + * @param value_io (in/out) in: initial value, out: final value + * @param timestamp_io (in/out) in: initial timestamp, out: final timestamp + */ +void fill_validator_with_samples(DataValidator *validator, + const float incr_value, + float *value_io, + uint64_t *timestamp_io); + +#endif //ECL_TESTS_COMMON_H diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 9cb89b821f6..952451c6f4e 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -33,7 +33,8 @@ #pragma once -#include +#include "data_validator/DataValidatorGroup.hpp" + #include #include #include diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 24595a91caf..dd595af23ad 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -41,14 +41,15 @@ #include "parameters.h" +#include "data_validator/DataValidator.hpp" +#include "data_validator/DataValidatorGroup.hpp" + #include #include #include #include -#include -#include #include #include