Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: add ACCEL and JERK params and accessors

Browse Source 
allows users to specify a different acceleration and jerk for Auto, Guided, RTL, etc compared with manual modes (Acro, etc)
This commit is contained in:
Randy Mackay 2021-12-24 11:27:01 +09:00
parent 47ab961f42
commit 461c5196e7
2 changed files with 47 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -68,6 +68,24 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = {
// @Path: AR_PivotTurn.cpp
AP_SUBGROUPINFO(_pivot, "PIVOT_", 8, AR_WPNav, AR_PivotTurn),
// @Param: ACCEL
// @DisplayName: Waypoint acceleration
// @Description: Waypoint acceleration. If zero then ATC_ACCEL_MAX is used
// @Units: m/s/s
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("ACCEL", 9, AR_WPNav, _accel_max, 0),
// @Param: JERK
// @DisplayName: Waypoint jerk
// @Description: Waypoint jerk (change in acceleration). If zero then jerk is same as acceleration
// @Units: m/s/s/s
// @Range: 0 100
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("JERK", 10, AR_WPNav, _jerk_max, 0),
AP_GROUPEND
};
@ -91,18 +109,18 @@ void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float
speed_max = _speed_max;
}
if (!is_positive(accel_max)) {
accel_max = _atc.get_accel_max();
accel_max = get_accel_max();
}
if (!is_positive(lat_accel_max)) {
lat_accel_max = _atc.get_turn_lat_accel_max();
}
if (!is_positive(jerk_max)) {
jerk_max = _atc.get_accel_max();
jerk_max = get_jerk_max();
}
_scurve_jerk = jerk_max;
// initialise position controller
_pos_control.set_limits(speed_max, accel_max, lat_accel_max);
_pos_control.set_limits(speed_max, accel_max, lat_accel_max, jerk_max);
_scurve_prev_leg.init();
_scurve_this_leg.init();
@ -284,6 +302,24 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vecto
return set_desired_location(dest_loc, next_dest_loc);
}
// get max acceleration in m/s/s
float AR_WPNav::get_accel_max() const
{
if (is_positive(_accel_max)) {
return _accel_max;
}
return _atc.get_accel_max();
}
// get max jerk in m/s/s/s
float AR_WPNav::get_jerk_max() const
{
if (is_positive(_jerk_max)) {
return _jerk_max;
}
return get_accel_max();
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
bool AR_WPNav::get_stopping_location(Location& stopping_loc)
{

8
libraries/AR_WPNav/AR_WPNav.h
View File

@ -88,6 +88,12 @@ public:
float get_radius() const { return _radius; }
float get_pivot_rate() const { return _pivot.get_rate_max(); }
// get max acceleration in m/s/s
float get_accel_max() const;
// get max jerk in m/s/s/s
float get_jerk_max() const;
// calculate stopping location using current position and attitude controller provided maximum deceleration
// returns true on success, false on failure
bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED;
@ -125,6 +131,8 @@ protected:
AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners
AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached
AR_PivotTurn _pivot; // pivot turn controller
AP_Float _accel_max; // max acceleration. If zero then attitude controller's specified max accel is used
AP_Float _jerk_max; // max jerk (change in acceleration). If zero then value is same as accel_max
// references
AR_AttitudeControl& _atc; // rover attitude control library