Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: z-axis stopping point up to 3m above vehicle

Browse Source 
Stopping point while descending remains at 2m for safety
This commit is contained in:
Randy Mackay 2017-04-27 17:30:40 +09:00
parent 6e92f74ca0
commit 3a397584a1
2 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -293,7 +293,7 @@ void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
}
}
stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_Z_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_Z_MAX);
stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX);
}
/// init_takeoff - initialises target altitude if we are taking off

3
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -16,9 +16,10 @@
#define POSCONTROL_ACCELERATION_MIN 50.0f // minimum horizontal acceleration in cm/s/s - used for sanity checking acceleration in leash length calculation
#define POSCONTROL_ACCEL_XY 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers
#define POSCONTROL_ACCEL_XY_MAX 980.0f // max horizontal acceleration in cm/s/s that the position velocity controller will ask from the lower accel controller
#define POSCONTROL_STOPPING_DIST_Z_MAX 200.0f // max stopping distance vertically
// should be 1.5 times larger than POSCONTROL_ACCELERATION.
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
#define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing
#define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending
#define POSCONTROL_JERK_LIMIT_CMSSS 1700.0f // default jerk limit on horizontal acceleration (unit: m/s/s/s)
#define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s