adjust some limits to prevent divide-by-zero

This commit is contained in:
David Jablonski
2021-02-27 15:44:11 +01:00
committed by Mathieu Bresciani
parent 9f6c882d2a
commit 5233737a86
3 changed files with 14 additions and 8 deletions
@@ -449,10 +449,15 @@ void FixedwingAttitudeControl::Run()
*/
float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);
control_input.groundspeed = groundspeed;
control_input.groundspeed_scaler = groundspeed_scaler;
if (groundspeed > gspd_scaling_trim) {
control_input.groundspeed_scaler = gspd_scaling_trim / groundspeed;
} else {
control_input.groundspeed_scaler = 1.0f;
}
}
/* reset body angular rate limits on mode change */
@@ -436,7 +436,7 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
* increase airspeed more aggressively.
*
* @unit m/s
* @min 0.0
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
@@ -451,7 +451,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
* airspeed more aggressively.
*
* @unit m/s
* @min 0.0
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
@@ -467,7 +467,7 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
* no other airspeed setpoint sources are present (e.g. through non-centered RC sticks).
*
* @unit m/s
* @min 0.0
* @min 0.5
* @max 40
* @decimal 1
* @increment 0.5
@@ -39,6 +39,7 @@
#pragma once
#include <lib/mathlib/mathlib.h>
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
@@ -114,9 +115,9 @@ public:
/**
* Set the normalized hover thrust
* @param thrust [0,1] with which the vehicle hovers not acelerating down or up with level orientation
* @param thrust [0.1, 0.9] with which the vehicle hovers not acelerating down or up with level orientation
*/
void setHoverThrust(const float hover_thrust) { _hover_thrust = hover_thrust; }
void setHoverThrust(const float hover_thrust) { _hover_thrust = math::constrain(hover_thrust, 0.1f, 0.9f); }
/**
* Update the hover thrust without immediately affecting the output
@@ -192,7 +193,7 @@ private:
float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not accelerating down or up with level orientation
float _hover_thrust{}; ///< Thrust [0.1, 0.9] with which the vehicle hovers not accelerating down or up with level orientation
// States
matrix::Vector3f _pos; /**< current position */