From 99aad419553a1e34b44abc3a7163b8ecb7140a97 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 8 Sep 2017 15:58:59 +1000 Subject: [PATCH] AC_AttitudeControl: added a update_vel_controller_xy() API this allows for just XY control of velocity for quadplanes --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 16 +++++++++++++--- libraries/AC_AttitudeControl/AC_PosControl.h | 6 ++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index b2926a8a35..3b80452b8d 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -723,11 +723,11 @@ void AC_PosControl::init_vel_controller_xyz() init_ekf_z_reset(); } -/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher -/// velocity targets should we set using set_desired_velocity_xyz() method +/// update_velocity_controller_xy - run the velocity controller - should be called at 100hz or higher +/// velocity targets should we set using set_desired_velocity_xy() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller /// throttle targets will be sent directly to the motors -void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) +void AC_PosControl::update_vel_controller_xy(float ekfNavVelGainScaler) { // capture time since last iteration uint32_t now = AP_HAL::millis(); @@ -761,6 +761,16 @@ void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) // update xy update time _last_update_xy_ms = now; } +} + + +/// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher +/// velocity targets should we set using set_desired_velocity_xyz() method +/// callers should use get_roll() and get_pitch() methods and sent to the attitude controller +/// throttle targets will be sent directly to the motors +void AC_PosControl::update_vel_controller_xyz(float ekfNavVelGainScaler) +{ + update_vel_controller_xy(ekfNavVelGainScaler); // update altitude target set_alt_target_from_climb_rate_ff(_vel_desired.z, _dt, false); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 608d6db481..d9c0388a4b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -267,6 +267,12 @@ public: /// init_vel_controller_xyz - initialise the velocity controller - should be called once before the caller attempts to use the controller void init_vel_controller_xyz(); + /// update_velocity_controller_xy - run the XY velocity controller - should be called at 100hz or higher + /// velocity targets should we set using set_desired_velocity_xy() method + /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller + /// throttle targets will be sent directly to the motors + void update_vel_controller_xy(float ekfNavVelGainScaler); + /// update_velocity_controller_xyz - run the velocity controller - should be called at 100hz or higher /// velocity targets should we set using set_desired_velocity_xyz() method /// callers should use get_roll() and get_pitch() methods and sent to the attitude controller