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

AC_WPNav: set wp origin to horiz and vert stopping point

Browse Source 
Also bug fix to set pos_control's down speed as a negative number
This commit is contained in:
Randy Mackay 2014-01-25 17:23:27 +09:00 committed by Andrew Tridgell
parent bd88ed8a53
commit fc427967ae
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -247,6 +247,7 @@ void AC_WPNav::set_wp_destination(const Vector3f& destination)
}else{ }else{
// otherwise calculate origin from the current position and velocity // otherwise calculate origin from the current position and velocity
_pos_control.get_stopping_point_xy(_origin); _pos_control.get_stopping_point_xy(_origin);
_pos_control.get_stopping_point_z(_origin);
} }
// set origin and destination // set origin and destination
@ -276,7 +277,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
// initialise position controller speed and acceleration // initialise position controller speed and acceleration
_pos_control.set_speed_xy(_wp_speed_cms); _pos_control.set_speed_xy(_wp_speed_cms);
_pos_control.set_accel_xy(_wp_accel_cms); _pos_control.set_accel_xy(_wp_accel_cms);
_pos_control.set_speed_z(_wp_speed_down_cms, _wp_speed_up_cms); _pos_control.set_speed_z(-_wp_speed_down_cms, _wp_speed_up_cms);
_pos_control.calc_leash_length_xy(); _pos_control.calc_leash_length_xy();
_pos_control.calc_leash_length_z(); _pos_control.calc_leash_length_z();