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

AC_WPNav: Add EKF ground speed limit to loiter speed control 

This is required to prevent the speed controller saturating the optical flow sensor during low altitude flying.
This commit is contained in:
priseborough 2014-11-16 11:50:35 +11:00 committed by Andrew Tridgell
parent 83775554ea
commit 5fa0c59310
2 changed files with 15 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -191,6 +191,7 @@ void AC_WPNav::loiter_soften_for_landing()
void AC_WPNav::set_loiter_velocity(float velocity_cms)
{
// range check velocity and update position controller
//float maxSpd_cms = _ahrs.getSpeedlimit();
if (velocity_cms >= WPNAV_LOITER_SPEED_MIN) {
_loiter_speed_cms = velocity_cms;
@ -219,7 +220,7 @@ void AC_WPNav::get_loiter_stopping_point_xy(Vector3f& stopping_point) const
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
void AC_WPNav::calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit)
{
// range check nav_dt
if( nav_dt < 0 ) {
@ -276,6 +277,15 @@ void AC_WPNav::calc_loiter_desired_velocity(float nav_dt)
}
}
// limit EKF speed limit and convert to cm/s
ekfGndSpdLimit = 100.0f * max(ekfGndSpdLimit,0.1f);
// Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing)
float horizSpdDem = sqrtf(sq(desired_vel.x) + sq(desired_vel.y));
if (horizSpdDem > ekfGndSpdLimit) {
desired_vel.x = desired_vel.x * ekfGndSpdLimit / horizSpdDem;
desired_vel.y = desired_vel.y * ekfGndSpdLimit / horizSpdDem;
}
// send adjusted feed forward velocity back to position controller
_pos_control.set_desired_velocity_xy(desired_vel.x,desired_vel.y);
}
@ -287,7 +297,7 @@ int32_t AC_WPNav::get_loiter_bearing_to_target() const
}
/// update_loiter - run the loiter controller - should be called at 100hz
void AC_WPNav::update_loiter()
void AC_WPNav::update_loiter(float ekfGndSpdLimit)
{
// calculate dt
uint32_t now = hal.scheduler->millis();
@ -302,7 +312,7 @@ void AC_WPNav::update_loiter()
// capture time since last iteration
_loiter_last_update = now;
// translate any adjustments from pilot to loiter target
calc_loiter_desired_velocity(dt);
calc_loiter_desired_velocity(dt,ekfGndSpdLimit);
// trigger position controller on next update
_pos_control.trigger_xy();
}else{

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -95,7 +95,7 @@ public:
int32_t get_loiter_bearing_to_target() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
void update_loiter(float ekfGndSpdLimit);
///
/// waypoint controller
@ -245,7 +245,7 @@ protected:
/// calc_loiter_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance
/// updated velocity sent directly to position controller
void calc_loiter_desired_velocity(float nav_dt);
void calc_loiter_desired_velocity(float nav_dt, float ekfGndSpdLimit);
/// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;