fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 16:44:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

tecs and l1 switch to matrix math library (#409)

Browse Source
This commit is contained in:
Daniel Agar 2018-03-18 21:59:41 -04:00 committed by GitHub
parent 1bc7378414
commit 35bc2cfcd9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
4 changed files with 108 additions and 163 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

146
l1/ecl_l1_pos_controller.cpp
View File

@ -42,6 +42,8 @@
#include "ecl_l1_pos_controller.h"
using matrix::Vector2f;
float ECL_L1_Pos_Controller::nav_roll()
{
float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
@ -49,54 +51,24 @@ float ECL_L1_Pos_Controller::nav_roll()
return ret;
}
float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand()
{
return _lateral_accel;
}
float ECL_L1_Pos_Controller::nav_bearing()
{
return _wrap_pi(_nav_bearing);
}
float ECL_L1_Pos_Controller::bearing_error()
{
return _bearing_error;
}
float ECL_L1_Pos_Controller::target_bearing()
{
return _target_bearing;
}
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{
/* following [2], switching on L1 distance */
return math::min(wp_radius, _L1_distance);
}
bool ECL_L1_Pos_Controller::reached_loiter_target()
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)
{
return _circle_mode;
}
float ECL_L1_Pos_Controller::crosstrack_error()
{
return _crosstrack_error;
}
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
const math::Vector<2> &ground_speed_vector)
{
/* this follows the logic presented in [1] */
float eta;
float xtrack_vel;
float ltrack_vel;
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(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0),
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);
@ -105,7 +77,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
_L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */
math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
/*
* check if waypoints are on top of each other. If yes,
@ -118,7 +90,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
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;
@ -132,36 +104,37 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */
math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
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) {
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 */
math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
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));
_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.
*/
/*
* 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)
@ -169,14 +142,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
*/
/* 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));
_nav_bearing = atan2f(-vector_B_to_P_unit(1), -vector_B_to_P_unit(0));
} else {
@ -190,7 +163,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
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);
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);
@ -211,8 +184,9 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A,
_bearing_error = eta;
}
void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector<2> &ground_speed_vector)
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] */
@ -222,23 +196,25 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
float K_velocity = 2.0f * _L1_damping * omega;
/* update bearing to next waypoint */
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0),
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);
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 */
math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
math::Vector<2> vector_A_to_airplane_unit;
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;
}
@ -274,11 +250,12 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
/* 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);
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));
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);
@ -290,41 +267,39 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
// 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_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));
_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));
_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
}
}
void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
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] */
float eta;
/*
/*
* 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);
eta = _target_bearing - _wrap_pi(current_heading);
float eta = _target_bearing - _wrap_pi(current_heading);
eta = _wrap_pi(eta);
/* consequently the bearing error is exactly eta: */
_bearing_error = eta;
_bearing_error = eta;
/* ground speed is the length of the ground speed vector */
float ground_speed = ground_speed_vector.length();
@ -357,16 +332,35 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
/* not circling a waypoint when flying level */
_circle_mode = false;
}
math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
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] */
math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
Vector2f out(math::radians((target(0) - origin(0))),
math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
}
void ECL_L1_Pos_Controller::set_l1_period(float period)
{
_L1_period = period;
/* calculate the ratio introduced in [2] */
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
/* calculate normalized frequency for heading tracking */
_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
}
void ECL_L1_Pos_Controller::set_l1_damping(float damping)
{
_L1_damping = damping;
/* calculate the ratio introduced in [2] */
_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
/* calculate the L1 gain (following [2]) */
_K_L1 = 4.0f * _L1_damping * _L1_damping;
}

109
l1/ecl_l1_pos_controller.h
View File

@ -70,38 +70,19 @@
class __EXPORT ECL_L1_Pos_Controller
{
public:
ECL_L1_Pos_Controller() :
_lateral_accel(0.0),
_L1_distance(20.0),
_circle_mode(false),
_nav_bearing(0.0),
_bearing_error(0.0),
_crosstrack_error(0.0),
_target_bearing(0.0),
_L1_period(25.0),
_L1_damping(0.75),
_L1_ratio(5.0),
_K_L1(2.0),
_heading_omega(1.0),
_roll_lim_rad(math::radians(10.0))
{
}
/**
* The current target bearing
*
* @return bearing angle (-pi..pi, in NED frame)
*/
float nav_bearing();
float nav_bearing() { return _wrap_pi(_nav_bearing); }
/**
* Get lateral acceleration demand.
*
* @return Lateral acceleration in m/s^2
*/
float nav_lateral_acceleration_demand();
float nav_lateral_acceleration_demand() { return _lateral_accel; }
/**
* Heading error.
@ -109,16 +90,14 @@ public:
* The heading error is either compared to the current track
* or to the tangent of the current loiter radius.
*/
float bearing_error();
float bearing_error() { return _bearing_error; }
/**
* Bearing from aircraft to current target.
*
* @return bearing angle (-pi..pi, in NED frame)
*/
float target_bearing();
float target_bearing() { return _target_bearing; }
/**
* Get roll angle setpoint for fixed wing.
@ -127,32 +106,26 @@ public:
*/
float nav_roll();
/**
* Get the current crosstrack error.
*
* @return Crosstrack error in meters.
*/
float crosstrack_error();
float crosstrack_error() { return _crosstrack_error; }
/**
* Returns true if the loiter waypoint has been reached
*/
bool reached_loiter_target();
bool reached_loiter_target() { return _circle_mode; }
/**
* Returns true if following a circle (loiter)
*/
bool circle_mode() {
return _circle_mode;
}
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
@ -161,7 +134,6 @@ public:
*/
float switch_distance(float waypoint_switch_radius);
/**
* Navigate between two waypoints
*
@ -172,9 +144,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
const math::Vector<2> &ground_speed);
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.
@ -184,9 +155,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
const math::Vector<2> &ground_speed_vector);
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.
@ -197,8 +167,7 @@ public:
*
* @return sets _lateral_accel setpoint
*/
void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
void navigate_heading(float navigation_heading, float current_heading, const matrix::Vector2f &ground_speed);
/**
* Keep the wings level.
@ -208,58 +177,40 @@ public:
*/
void navigate_level_flight(float current_heading);
/**
* Set the L1 period.
*/
void 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 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) {
_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;
}
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;
}
void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
private:
float _lateral_accel; ///< Lateral acceleration setpoint in m/s^2
float _L1_distance; ///< L1 lead distance, defined by period and damping
bool _circle_mode; ///< flag for loiter mode
float _nav_bearing; ///< bearing to L1 reference point
float _bearing_error; ///< bearing error
float _crosstrack_error; ///< crosstrack error in meters
float _target_bearing; ///< the heading setpoint
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; ///< L1 tracking period in seconds
float _L1_damping; ///< L1 damping ratio
float _L1_ratio; ///< L1 ratio for navigation
float _K_L1; ///< L1 control gain for _L1_damping
float _heading_omega; ///< Normalized frequency
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; ///<maximum roll angle
float _roll_lim_rad{math::radians(30.0f)}; ///<maximum roll angle
/**
* Convert a 2D vector from WGS84 to planar coordinates.
@ -272,7 +223,7 @@ private:
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
};

8
tecs/tecs.cpp
View File

@ -51,8 +51,8 @@ using math::min;
* 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 math::Matrix<3, 3> &rotMat,
const math::Vector<3> &accel_body, bool altitude_lock, bool in_air,
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, bool vz_valid, float vz, float az)
{
@ -306,7 +306,7 @@ void TECS::_update_energy_estimates()
_SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change
}
void TECS::_update_throttle_setpoint(const float throttle_cruise, const math::Matrix<3, 3> &rotMat)
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;
@ -575,7 +575,7 @@ void TECS::_update_STE_rate_lim()
_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
}
void TECS::update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
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)
{

8
tecs/tecs.h
View File

@ -72,14 +72,14 @@ public:
* Must be called prior to udating tecs control loops
* Must be called at 50Hz or greater
*/
void update_vehicle_state_estimates(float airspeed, const math::Matrix<3, 3> &rotMat,
const math::Vector<3> &accel_body, bool altitude_lock, bool in_air,
void update_vehicle_state_estimates(float airspeed, const matrix::Dcmf &rotMat,
const matrix::Vector3f &accel_body, bool altitude_lock, bool in_air,
float altitude, bool vz_valid, float vz, float az);
/**
* Update the control loop calculations
*/
void update_pitch_throttle(const math::Matrix<3, 3> &rotMat, float pitch, float baro_altitude, float hgt_setpoint,
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);
@ -306,7 +306,7 @@ private:
/**
* Update throttle setpoint
*/
void _update_throttle_setpoint(float throttle_cruise, const math::Matrix<3, 3> &rotMat);
void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat);
/**
* Detect an uncommanded descent