|
|
|
@ -309,8 +309,8 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
@@ -309,8 +309,8 @@ 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) |
|
|
|
|
/// init_brake_target - initializes stop position from current position and velocity
|
|
|
|
|
void AC_WPNav::init_brake_target(float accel_cmss) |
|
|
|
|
{ |
|
|
|
|
const Vector3f& curr_vel = _inav.get_velocity(); |
|
|
|
|
Vector3f stopping_point; |
|
|
|
@ -329,8 +329,8 @@ void AC_WPNav::init_stop_target(float accel_cmss)
@@ -329,8 +329,8 @@ void AC_WPNav::init_stop_target(float accel_cmss)
|
|
|
|
|
init_loiter_target(stopping_point); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// update_stop - run the stop controller - gets called at 400hz
|
|
|
|
|
void AC_WPNav::update_stop(float ekfGndSpdLimit, float ekfNavVelGainScaler) |
|
|
|
|
// update_brake - run the stop controller - gets called at 400hz
|
|
|
|
|
void AC_WPNav::update_brake(float ekfGndSpdLimit, float ekfNavVelGainScaler) |
|
|
|
|
{ |
|
|
|
|
// calculate dt
|
|
|
|
|
float dt = _pos_control.time_since_last_xy_update(); |
|
|
|
|