Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-30 12:38:33 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: split set_speed_z into up and down

Browse Source 
Also rely on AC_PosControl to store current target up and down speeds so wpnav's defaults are not affected by do-change-speed requests
This commit is contained in:
Randy Mackay 2019-01-24 13:57:11 +09:00
parent b0811c86e7
commit cde2964f05
2 changed files with 22 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -170,12 +170,18 @@ void AC_WPNav::set_speed_xy(float speed_cms)
} }
} }
/// set_speed_z - allows main code to pass target vertical velocity for wp navigation /// set current target climb rate during wp navigation
void AC_WPNav::set_speed_z(float speed_down_cms, float speed_up_cms) void AC_WPNav::set_speed_up(float speed_up_cms)
{ {
_wp_speed_down_cms = speed_down_cms; _pos_control.set_max_speed_z(_pos_control.get_max_speed_down(), speed_up_cms);
_wp_speed_up_cms = speed_up_cms; // flag that wp leash must be recalculated
_pos_control.set_max_speed_z(_wp_speed_down_cms, _wp_speed_up_cms); _flags.recalc_wp_leash = true;
}
/// set current target descent rate during wp navigation
void AC_WPNav::set_speed_down(float speed_down_cms)
{
_pos_control.set_max_speed_z(speed_down_cms, _pos_control.get_max_speed_up());
// flag that wp leash must be recalculated // flag that wp leash must be recalculated
_flags.recalc_wp_leash = true; _flags.recalc_wp_leash = true;
} }
@ -556,10 +562,10 @@ void AC_WPNav::calculate_wp_leash_length()
float speed_z; float speed_z;
float leash_z; float leash_z;
if (_pos_delta_unit.z >= 0.0f) { if (_pos_delta_unit.z >= 0.0f) {
speed_z = _wp_speed_up_cms; speed_z = _pos_control.get_max_speed_up();
leash_z = _pos_control.get_leash_up_z(); leash_z = _pos_control.get_leash_up_z();
}else{ }else{
speed_z = _wp_speed_down_cms; speed_z = fabsf(_pos_control.get_max_speed_down());
leash_z = _pos_control.get_leash_down_z(); leash_z = _pos_control.get_leash_down_z();
} }

17
libraries/AC_WPNav/AC_WPNav.h
View File

@ -78,17 +78,18 @@ public:
/// set current target horizontal speed during wp navigation /// set current target horizontal speed during wp navigation
void set_speed_xy(float speed_cms); void set_speed_xy(float speed_cms);
/// set_speed_z - allows main code to pass target vertical velocity for wp navigation /// set current target climb or descent rate during wp navigation
void set_speed_z(float speed_down_cms, float speed_up_cms); void set_speed_up(float speed_up_cms);
void set_speed_down(float speed_down_cms);
/// get default target horizontal velocity during wp navigation /// get default target horizontal velocity during wp navigation
float get_default_speed_xy() const { return _wp_speed_cms; } float get_default_speed_xy() const { return _wp_speed_cms; }
/// get_speed_up - returns target climb speed in cm/s during missions /// get default target climb speed in cm/s during missions
float get_speed_up() const { return _wp_speed_up_cms; } float get_default_speed_up() const { return _wp_speed_up_cms; }
/// get_speed_down - returns target descent speed in cm/s during missions. Note: always positive /// get default target descent rate in cm/s during missions. Note: always positive
float get_speed_down() const { return _wp_speed_down_cms; } float get_default_speed_down() const { return _wp_speed_down_cms; }
/// get_speed_z - returns target descent speed in cm/s during missions. Note: always positive /// get_speed_z - returns target descent speed in cm/s during missions. Note: always positive
float get_accel_z() const { return _wp_accel_z_cmss; } float get_accel_z() const { return _wp_accel_z_cmss; }
@ -282,8 +283,8 @@ protected:
// parameters // parameters
AP_Float _wp_speed_cms; // default maximum horizontal speed in cm/s during missions AP_Float _wp_speed_cms; // default maximum horizontal speed in cm/s during missions
AP_Float _wp_speed_up_cms; // climb speed target in cm/s AP_Float _wp_speed_up_cms; // default maximum climb rate in cm/s
AP_Float _wp_speed_down_cms; // descent speed target in cm/s AP_Float _wp_speed_down_cms; // default maximum descent rate in cm/s
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions