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

FlightTask StraighLine: check values before dividing

This commit is contained in:
ChristophTobler 2018-08-10 15:59:34 +02:00 committed by Daniel Agar
parent d3d549b8e6
commit 56ea4fa6fa
1 changed files with 9 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/FlightTasks/tasks/Utility/StraightLine.cpp
View File

@ -83,7 +83,11 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
float speed_sp = dist_to_target > acc_dec_distance ? _desired_speed : _desired_speed_at_target;
float max_acc_dec = speed_sp > speed_sp_prev ? _desired_acceleration : -_desired_deceleration;
float acc_track = (speed_sp - speed_sp_prev) / _deltatime;
float acc_track = 0.0f;
if (_deltatime > FLT_EPSILON) {
acc_track = (speed_sp - speed_sp_prev) / _deltatime;
}
if (fabs(acc_track) > fabs(max_acc_dec)) {
// accelerate/decelerate with desired acceleration/deceleration towards target
@ -192,7 +196,7 @@ void StraightLine::setSpeed(const float &speed)
{
float vel_max = getMaxVel();
if (speed > 0 && speed < vel_max) {
if (speed > FLT_EPSILON && speed < vel_max) {
_desired_speed = speed;
} else if (speed > vel_max) {
@ -204,7 +208,7 @@ void StraightLine::setSpeedAtTarget(const float &speed_at_target)
{
float vel_max = getMaxVel();
if (speed_at_target > 0 && speed_at_target < vel_max) {
if (speed_at_target > FLT_EPSILON && speed_at_target < vel_max) {
_desired_speed_at_target = speed_at_target;
} else if (speed_at_target > vel_max) {
@ -216,7 +220,7 @@ void StraightLine::setAcceleration(const float &acc)
{
float acc_max = getMaxVel();
if (acc > 0 && acc < acc_max) {
if (acc > FLT_EPSILON && acc < acc_max) {
_desired_acceleration = acc;
} else if (acc > acc_max) {
@ -226,7 +230,7 @@ void StraightLine::setAcceleration(const float &acc)
void StraightLine::setDeceleration(const float &dec)
{
if (dec > 0 && dec < DECELERATION_MAX) {
if (dec > FLT_EPSILON && dec < DECELERATION_MAX) {
_desired_deceleration = dec;
} else if (dec > DECELERATION_MAX) {