Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: add reference to AC_PosControl

Browse Source 
Also remove requirement to pass in inertial nav position and velocity to
init_loiter method
This commit is contained in:
Randy Mackay 2014-01-18 11:52:33 +09:00 committed by Andrew Tridgell
parent 49367c5a30
commit 2984e492df
2 changed files with 16 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -71,6 +71,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
_inav(inav),
_ahrs(ahrs),
_pos_control(pos_control),
_pid_pos_lat(pid_pos_lat),
_pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_rate_lat),
@ -108,6 +109,15 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_
/// simple loiter controller
///
/// init_loiter_target - sets initial loiter target based on current position and velocity
void AC_WPNav::init_loiter_target()
{
_pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity());
// initialise pilot input
_pilot_vel_forward_cms = 0;
_pilot_vel_right_cms = 0;
}
/// get_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target) const
{
@ -240,16 +250,10 @@ void AC_WPNav::translate_loiter_target_movements(float nav_dt)
}
}
/// get_distance_to_target - get horizontal distance to loiter target in cm
float AC_WPNav::get_distance_to_target() const
{
return _distance_to_target;
}
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t AC_WPNav::get_bearing_to_target() const
{
return get_bearing_cd(_inav->get_position(), _target);
return get_bearing_cd(_inav->get_position(), _pos_control.get_post_target());
}
/// update_loiter - run the loiter controller - should be called at 10hz

9
libraries/AC_WPNav/AC_WPNav.h
View File

@ -9,6 +9,7 @@
#include <AC_PID.h> // PID library
#include <APM_PI.h> // PID library
#include <AP_InertialNav.h> // Inertial Navigation library
#include <AC_PosControl.h> // Position control library
// loiter maximum velocities and accelerations
#define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller
@ -40,7 +41,7 @@ class AC_WPNav
public:
/// Constructor
AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosControl& pos_control, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
///
/// simple loiter controller
@ -52,8 +53,8 @@ public:
/// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position);
/// init_loiter_target - set initial loiter target based on current position and velocity
void init_loiter_target(const Vector3f& position, const Vector3f& velocity);
/// init_loiter_target - sets initial loiter target based on current position and velocity
void init_loiter_target() { _pos_control.init_pos_target(_inav->get_position(),_inav->get_velocity()); }
/// move_loiter_target - move destination using pilot input
void move_loiter_target(float control_roll, float control_pitch, float dt);
@ -182,6 +183,7 @@ protected:
// references to inertial nav and ahrs libraries
const AP_InertialNav* const _inav;
const AP_AHRS* const _ahrs;
AC_PosControl& _pos_control;
// pointers to pid controllers
APM_PI* const _pid_pos_lat;
@ -224,7 +226,6 @@ protected:
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
float _track_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm
float _distance_to_target; // distance to loiter target
float _wp_leash_xy; // horizontal leash length in cm
float _wp_leash_z; // horizontal leash length in cm
float _limited_speed_xy_cms; // horizontal speed in cm/s used to advance the intermediate target towards the destination. used to limit extreme acceleration after passing a waypoint