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

AP_WPNav: read turn G acceleration from Atitude control

This commit is contained in:
Iampete1 2021-05-02 17:37:03 +01:00 committed by Randy Mackay
parent 980c41e273
commit c5503b9aed
2 changed files with 5 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -216,7 +216,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, float ne
} else { } else {
// calculate maximum speed that keeps overshoot within bounds // calculate maximum speed that keeps overshoot within bounds
const float radius_m = fabsf(_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f)); const float radius_m = fabsf(_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(_turn_max_mss * radius_m)); _desired_speed_final = MIN(_desired_speed, safe_sqrt(_atc.get_turn_lat_accel_max() * radius_m));
// ensure speed does not fall below minimum // ensure speed does not fall below minimum
apply_speed_min(_desired_speed_final); apply_speed_min(_desired_speed_final);
} }
@ -374,7 +374,7 @@ void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
_nav_controller.update_waypoint(_reached_destination ? current_loc : _oa_origin, _oa_destination, _radius); _nav_controller.update_waypoint(_reached_destination ? current_loc : _oa_origin, _oa_destination, _radius);
// retrieve lateral acceleration, heading back towards line and crosstrack error // retrieve lateral acceleration, heading back towards line and crosstrack error
_desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_turn_max_mss, _turn_max_mss); _desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_atc.get_turn_lat_accel_max(), _atc.get_turn_lat_accel_max());
_desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd()); _desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd());
if (_reversed) { if (_reversed) {
_desired_lat_accel *= -1.0f; _desired_lat_accel *= -1.0f;
@ -422,7 +422,7 @@ void AR_WPNav::update_desired_speed(float dt)
// calculate and limit speed to allow vehicle to stay on circle // calculate and limit speed to allow vehicle to stay on circle
// ensure limit does not force speed below minimum // ensure limit does not force speed below minimum
float overshoot_speed_max = safe_sqrt(_turn_max_mss * MAX(_turn_radius, radius_m)); float overshoot_speed_max = safe_sqrt(_atc.get_turn_lat_accel_max() * MAX(_turn_radius, radius_m));
apply_speed_min(overshoot_speed_max); apply_speed_min(overshoot_speed_max);
des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max); des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max);
@ -436,9 +436,8 @@ void AR_WPNav::update_desired_speed(float dt)
} }
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
void AR_WPNav::set_turn_params(float turn_max_g, float turn_radius, bool pivot_possible) void AR_WPNav::set_turn_params(float turn_radius, bool pivot_possible)
{ {
_turn_max_mss = turn_max_g * GRAVITY_MSS;
_turn_radius = turn_radius; _turn_radius = turn_radius;
_pivot_possible = pivot_possible; _pivot_possible = pivot_possible;
} }

3
libraries/AR_WPNav/AR_WPNav.h
View File

@ -70,7 +70,7 @@ public:
float oa_wp_bearing_cd() const { return _oa_wp_bearing_cd; } float oa_wp_bearing_cd() const { return _oa_wp_bearing_cd; }
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
void set_turn_params(float turn_max_g, float turn_radius, bool pivot_possible); void set_turn_params(float turn_radius, bool pivot_possible);
// accessors for parameter values // accessors for parameter values
float get_default_speed() const { return _speed_max; } float get_default_speed() const { return _speed_max; }
@ -129,7 +129,6 @@ private:
AP_Navigation& _nav_controller; // navigation controller (aka L1 controller) AP_Navigation& _nav_controller; // navigation controller (aka L1 controller)
// variables held in vehicle code (for now) // variables held in vehicle code (for now)
float _turn_max_mss; // lateral acceleration maximum in m/s/s
float _turn_radius; // vehicle turn radius in meters float _turn_radius; // vehicle turn radius in meters
bool _pivot_possible; // true if vehicle can pivot bool _pivot_possible; // true if vehicle can pivot
bool _pivot_active; // true if vehicle is currently pivoting bool _pivot_active; // true if vehicle is currently pivoting