Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: remove various timing hacks

Browse Source
This commit is contained in:
Jonathan Challinger 2014-12-17 13:26:13 -08:00 committed by Randy Mackay
parent 557d339cf1
commit 6eab698e85
3 changed files with 38 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -34,7 +34,6 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosCont
_inav(inav),
_ahrs(ahrs),
_pos_control(pos_control),
_last_update(0),
_yaw(0.0f),
_angle(0.0f),
_angle_total(0.0f),
@ -95,18 +94,15 @@ void AC_Circle::init()
void AC_Circle::update()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _last_update) / 1000.0f;
float dt = _pos_control.time_since_last_xy_update();
// update circle position at 10hz
if (dt > 0.095f) {
// update circle position at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
// double check dt is reasonable
if (dt >= 1.0f) {
if (dt >= 0.2f) {
dt = 0.0;
}
// capture time since last iteration
_last_update = now;
// ramp up angular velocity to maximum
if (_rate >= 0) {
@ -154,12 +150,9 @@ void AC_Circle::update()
_yaw = _angle * AC_CIRCLE_DEGX100;
}
// trigger position controller on next update
_pos_control.trigger_xy();
// update position controller
_pos_control.update_xy_controller(false, 1.0f);
}
// run loiter's position to velocity step
_pos_control.update_xy_controller(false, 1.0f);
}
// get_closest_point_on_circle - returns closest point on the circle

74
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -91,7 +91,6 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosContro
_ahrs(ahrs),
_pos_control(pos_control),
_attitude_control(attitude_control),
_loiter_last_update(0),
_loiter_step(0),
_pilot_accel_fwd_cms(0),
_pilot_accel_rgt_cms(0),
@ -291,27 +290,20 @@ int32_t AC_WPNav::get_loiter_bearing_to_target() const
return get_bearing_cd(_inav.get_position(), _pos_control.get_pos_target());
}
/// update_loiter - run the loiter controller - should be called at 100hz
// update_loiter - run the loiter controller - gets called at 100hz (APM) or 400hz (PX4)
void AC_WPNav::update_loiter(float ekfGndSpdLimit, float ekfNavVelGainScaler)
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _loiter_last_update) / 1000.0f;
float dt = _pos_control.time_since_last_xy_update();
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt >= WPNAV_LOITER_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
// run at poscontrol update rate.
// TODO: run on user input to reduce latency, maybe if (user_input || dt >= _pos_control.get_dt_xy())
if (dt >= _pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// capture time since last iteration
_loiter_last_update = now;
// translate any adjustments from pilot to loiter target
calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
// trigger position controller on next update
_pos_control.trigger_xy();
}else{
// run horizontal position controller
_pos_control.update_xy_controller(true, ekfNavVelGainScaler);
}
}
@ -600,32 +592,30 @@ int32_t AC_WPNav::get_wp_bearing_to_destination() const
void AC_WPNav::update_wpnav()
{
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _wp_last_update) / 1000.0f;
float dt = _pos_control.time_since_last_xy_update();
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt >= WPNAV_WP_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
// update at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// capture time since last iteration
_wp_last_update = now;
// advance the target if necessary
advance_wp_target_along_track(dt);
_pos_control.trigger_xy();
// freeze feedforwards during known discontinuities
// TODO: why always consider Z axis discontinuous?
if (_flags.new_wp_destination) {
_flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
}
_pos_control.freeze_ff_z();
}else{
// run horizontal position controller
_pos_control.update_xy_controller(false, 1.0f);
// check if leash lengths need updating
_pos_control.update_xy_controller(false, 1.0f);
check_wp_leash_length();
_wp_last_update = hal.scheduler->millis();
}
}
@ -864,30 +854,30 @@ void AC_WPNav::update_spline()
return;
}
// calculate dt
uint32_t now = hal.scheduler->millis();
float dt = (now - _wp_last_update) / 1000.0f;
float dt = _pos_control.time_since_last_xy_update();
// reset step back to 0 if 0.1 seconds has passed and we completed the last full cycle
if (dt >= WPNAV_WP_UPDATE_TIME) {
// double check dt is reasonable
if (dt >= 1.0f) {
dt = 0.0;
// run at poscontrol update rate
if (dt >= _pos_control.get_dt_xy()) {
// sanity check dt
if (dt >= 0.2f) {
dt = 0.0f;
}
// capture time since last iteration
_wp_last_update = now;
// advance the target if necessary
advance_spline_target_along_track(dt);
_pos_control.trigger_xy();
// freeze feedforwards during known discontinuities
// TODO: why always consider Z axis discontinuous?
if (_flags.new_wp_destination) {
_flags.new_wp_destination = false;
_pos_control.freeze_ff_xy();
}
_pos_control.freeze_ff_z();
}else{
// run horizontal position controller
_pos_control.update_xy_controller(false, 1.0f);
_wp_last_update = hal.scheduler->millis();
}
}

1
libraries/AC_WPNav/AC_WPNav.h
View File

@ -285,7 +285,6 @@ protected:
AP_Float _wp_accel_z_cms; // vertical acceleration in cm/s/s during missions
// loiter controller internal variables
uint32_t _loiter_last_update; // time of last update_loiter call
uint8_t _loiter_step; // used to decide which portion of loiter controller to run during this iteration
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)