Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: remove ekf position reset handler 

This has been migrated to the position control library
This commit is contained in:
Randy Mackay 2016-11-22 14:36:52 +09:00
parent 1012333eef
commit a32068a973
2 changed files with 0 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -117,7 +117,6 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosContro
_attitude_control(attitude_control),
_pilot_accel_fwd_cms(0),
_pilot_accel_rgt_cms(0),
_loiter_ekf_pos_reset_ms(0),
_wp_last_update(0),
_wp_step(0),
_track_length(0.0f),
@ -179,9 +178,6 @@ void AC_WPNav::init_loiter_target()
const Vector3f& curr_pos = _inav.get_position();
const Vector3f& curr_vel = _inav.get_velocity();
// initialise ekf position reset check
init_ekf_position_reset();
// initialise position controller
_pos_control.init_xy_controller();
@ -329,8 +325,6 @@ void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
if (dt >= 0.2f) {
dt = 0.0f;
}
// initialise ekf position reset check
check_for_ekf_position_reset();
// initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
@ -348,9 +342,6 @@ void AC_WPNav::init_brake_target(float accel_cmss)
const Vector3f& curr_vel = _inav.get_velocity();
Vector3f stopping_point;
// initialise ekf position reset check
init_ekf_position_reset();
// initialise position controller
_pos_control.init_xy_controller();
@ -1258,22 +1249,3 @@ float AC_WPNav::get_slow_down_speed(float dist_from_dest_cm, float accel_cmss)
return target_speed;
}
}
/// initialise ekf position reset check
void AC_WPNav::init_ekf_position_reset()
{
Vector2f pos_shift;
_loiter_ekf_pos_reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
}
/// check for ekf position reset and adjust loiter or brake target position
void AC_WPNav::check_for_ekf_position_reset()
{
// check for position shift
Vector2f pos_shift;
uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
if (reset_ms != _loiter_ekf_pos_reset_ms) {
_pos_control.shift_pos_xy_target(pos_shift.x * 100.0f, pos_shift.y * 100.0f);
_loiter_ekf_pos_reset_ms = reset_ms;
}
}

5
libraries/AC_WPNav/AC_WPNav.h
View File

@ -286,10 +286,6 @@ protected:
/// get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm)
float get_slow_down_speed(float dist_from_dest_cm, float accel_cmss);
/// initialise and check for ekf position reset and adjust loiter or brake target position
void init_ekf_position_reset();
void check_for_ekf_position_reset();
/// spline protected functions
/// update_spline_solution - recalculates hermite_spline_solution grid
@ -334,7 +330,6 @@ protected:
int16_t _pilot_accel_fwd_cms; // pilot's desired acceleration forward (body-frame)
int16_t _pilot_accel_rgt_cms; // pilot's desired acceleration right (body-frame)
Vector2f _loiter_desired_accel; // slewed pilot's desired acceleration in lat/lon frame
uint32_t _loiter_ekf_pos_reset_ms; // system time of last recorded ekf position reset
// waypoint controller internal variables
uint32_t _wp_last_update; // time of last update_wpnav call