Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: move stop below all loiter methods

Browse Source 
No functional change
This commit is contained in:
Randy Mackay 2015-04-26 16:26:53 +09:00
parent 1aa696bc10
commit 35874292a0
2 changed files with 43 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -212,41 +212,6 @@ void AC_WPNav::set_loiter_velocity(float velocity_cms)
}
}
/// init_stop_target - initializes stop position from current position and velocity
void AC_WPNav::init_stop_target(float accel_cmss)
{
const Vector3f& curr_vel = _inav.get_velocity();
Vector3f stopping_point;
// initialise position controller
_pos_control.init_xy_controller();
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(curr_vel.length());
_pos_control.set_accel_xy(accel_cmss);
_pos_control.calc_leash_length_xy();
_pos_control.get_stopping_point_xy(stopping_point);
// set target position
init_loiter_target(stopping_point);
}
// update_stop - run the stop controller - gets called at 400hz
void AC_WPNav::update_stop(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// run at poscontrol update rate.
if (dt >= _pos_control.get_dt_xy()) {
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity_xy(0,0);
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler);
}
}
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void AC_WPNav::set_pilot_desired_acceleration(float control_roll, float control_pitch)
{
@ -357,6 +322,40 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
}
}
/// init_stop_target - initializes stop position from current position and velocity
void AC_WPNav::init_stop_target(float accel_cmss)
{
const Vector3f& curr_vel = _inav.get_velocity();
Vector3f stopping_point;
// initialise position controller
_pos_control.init_xy_controller();
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(curr_vel.length());
_pos_control.set_accel_xy(accel_cmss);
_pos_control.calc_leash_length_xy();
_pos_control.get_stopping_point_xy(stopping_point);
// set target position
init_loiter_target(stopping_point);
}
// update_stop - run the stop controller - gets called at 400hz
void AC_WPNav::update_stop(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
// calculate dt
float dt = _pos_control.time_since_last_xy_update();
// run at poscontrol update rate.
if (dt >= _pos_control.get_dt_xy()) {
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity_xy(0,0);
_pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_LIMITED_AND_VEL_FF, ekfNavVelGainScaler);
}
}
///
/// waypoint navigation

18
libraries/AC_WPNav/AC_WPNav.h
View File

@ -79,15 +79,6 @@ public:
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
void calculate_loiter_leash_length();
///
/// stop controller
///
/// init_stop_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void init_stop_target(float accel_cmss);
///
/// update_stop - run the stop controller - should be called at 400hz
void update_stop(float ekfGndSpdLimit, float ekfNavVelGainScaler);
/// set_pilot_desired_acceleration - sets pilot desired acceleration from roll and pitch stick input
void set_pilot_desired_acceleration(float control_roll, float control_pitch);
@ -106,6 +97,15 @@ public:
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler);
///
/// stop controller
///
/// init_stop_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void init_stop_target(float accel_cmss);
///
/// update_stop - run the stop controller - should be called at 400hz
void update_stop(float ekfGndSpdLimit, float ekfNavVelGainScaler);
///
/// waypoint controller
///