Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: check acceleration is non zero

This commit is contained in:
Randy Mackay 2014-03-30 11:47:58 +09:00
parent f0f3688172
commit c0458b786a
1 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -56,7 +56,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// @DisplayName: Waypoint Acceleration
// @Description: Defines the horizontal acceleration in cm/s/s used during missions
// @Units: cm/s/s
// @Range: 0 980
// @Range: 50 500
// @Increment: 10
// @User: Standard
AP_GROUPINFO("ACCEL", 5, AC_WPNav, _wp_accel_cms, WPNAV_ACCELERATION),
@ -281,6 +281,11 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_pos_delta_unit = pos_delta/_track_length;
}
// check _wp_accel_cms is reasonable
if (_wp_accel_cms <= 0) {
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
// initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms);
@ -526,6 +531,11 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
// mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint
bool prev_segment_exists = (_flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000));
// check _wp_accel_cms is reasonable to avoid divide by zero
if (_wp_accel_cms <= 0) {
_wp_accel_cms.set_and_save(WPNAV_ACCELERATION);
}
// segment start types
// stop - vehicle is not moving at origin
// straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp