yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTask StraightLine: use 2D Vector and remove newline

Browse Source
This commit is contained in:
Matthias Grob
2018-07-23 17:22:26 +02:00
committed by Lorenz Meier
parent ccd8a47015
commit 7e547e6322
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -73,7 +73,7 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
float speed_sp_prev = math::max(velocity_setpoint * u_orig_to_target, 0.0f);
// Calculate accelerating/decelerating distance depending on speed, speed at target and acceleration/deceleration
float acc_dec_distance = fabs(_desired_speed * _desired_speed - _desired_speed_at_target * _desired_speed_at_target) / 2.0f;
float acc_dec_distance = fabs((_desired_speed * _desired_speed) - (_desired_speed_at_target * _desired_speed_at_target)) / 2.0f;
acc_dec_distance /= _desired_speed > _desired_speed_at_target ? _desired_deceleration : _desired_acceleration;
float dist_to_target = (_target - _pos).length(); // distance to target
@@ -95,7 +95,6 @@ void StraightLine::generateSetpoints(matrix::Vector3f &position_setpoint, matrix
// set the position and velocity setpoints
position_setpoint = closest_pt_on_line;
velocity_setpoint = u_orig_to_target * speed_sp;
}
float StraightLine::getMaxAcc()
@@ -109,7 +108,7 @@ float StraightLine::getMaxAcc()
Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
// calculate the maximal horizontal acceleration
float divider = (sqrt(u_orig_to_target(0) * u_orig_to_target(0) + u_orig_to_target(1) * u_orig_to_target(1)));
float divider = Vector2f(u_orig_to_target.data()).length();
float max_acc_hor = MPC_ACC_HOR_MAX.get();
if (divider > FLT_EPSILON) {
@@ -144,7 +143,7 @@ float StraightLine::getMaxVel()
Vector3f u_orig_to_target = (_target - _origin).unit_or_zero();
// calculate the maximal horizontal velocity
float divider = (sqrt(u_orig_to_target(0) * u_orig_to_target(0) + u_orig_to_target(1) * u_orig_to_target(1)));
float divider = Vector2f(u_orig_to_target.data()).length();
float max_vel_hor = MPC_XY_VEL_MAX.get();
if (divider > FLT_EPSILON) {