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
void AC_WPNav::set_speed_z(float speed_down_cms, float speed_up_cms)
/// set current target climb rate during wp navigation
void AC_WPNav::set_speed_up(float speed_up_cms)
{
_wp_speed_down_cms = speed_down_cms;
_wp_speed_up_cms = speed_up_cms;
_pos_control.set_max_speed_z(_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.set_max_speed_z(_pos_control.get_max_speed_down(), speed_up_cms);
// flag that wp leash must be recalculated
_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
_flags.recalc_wp_leash = true;
}
@ -556,10 +562,10 @@ void AC_WPNav::calculate_wp_leash_length()
float speed_z;
float leash_z;
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();
}else{
speed_z = _wp_speed_down_cms;
speed_z = fabsf(_pos_control.get_max_speed_down());
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
void set_speed_xy(float speed_cms);
/// set_speed_z - allows main code to pass target vertical velocity for wp navigation
void set_speed_z(float speed_down_cms, float speed_up_cms);
/// set current target climb or descent rate during wp navigation
void set_speed_up(float speed_up_cms);
void set_speed_down(float speed_down_cms);
/// get default target horizontal velocity during wp navigation
float get_default_speed_xy() const { return _wp_speed_cms; }
/// get_speed_up - returns target climb speed in cm/s during missions
float get_speed_up() const { return _wp_speed_up_cms; }
/// get default target climb speed in cm/s during missions
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
float get_speed_down() const { return _wp_speed_down_cms; }
/// get default target descent rate in cm/s during missions. Note: always positive
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
float get_accel_z() const { return _wp_accel_z_cmss; }
@ -282,8 +283,8 @@ protected:
// parameters
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_down_cms; // descent speed target in cm/s
AP_Float _wp_speed_up_cms; // default maximum climb rate 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_accel_cmss; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions