aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_Circle: add get_closest_point_on_circle

Browse Source 
init_start_angle method added to use current heading or position to
decide on initial start angle
This commit is contained in:
Randy Mackay 2014-04-16 16:19:41 +09:00
parent ff9838ea27
commit 94d38ee294
2 changed files with 103 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -40,10 +40,10 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosCont
AP_Param::setup_object_defaults(this, var_info);
}
/// set_circle_center in cm from home
void AC_Circle::set_center(const Vector3f& position)
/// init - initialise circle controller setting center specifically
void AC_Circle::init(const Vector3f& center)
{
_center = position;
_center = center;
// To-Do: set target position, angle, etc so that copter begins circle from closest point to stopping point
_pos_control.set_pos_target(_inav.get_position());
@ -52,10 +52,13 @@ void AC_Circle::set_center(const Vector3f& position)
// calculate velocities
calc_velocities();
// set start angle from position
init_start_angle(false);
}
/// init_center in cm from home using stopping point and projecting out based on the copter's heading
void AC_Circle::init_center()
/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
void AC_Circle::init()
{
Vector3f stopping_point;
@ -73,6 +76,9 @@ void AC_Circle::init_center()
// calculate velocities
calc_velocities();
// set starting angle from vehicle heading
init_start_angle(true);
}
/// update - update circle controller
@ -146,6 +152,42 @@ void AC_Circle::update()
_pos_control.update_pos_controller(false);
}
// get_closest_point_on_circle - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void AC_Circle::get_closest_point_on_circle(Vector3f &result)
{
// return center if radius is zero
if (_radius <= 0) {
result = _center;
return;
}
// get current position
const Vector3f &curr_pos = _inav.get_position();
// calc vector from current location to circle center
Vector2f vec; // vector from circle center to current location
vec.x = (curr_pos.x - _center.x);
vec.y = (curr_pos.y - _center.y);
float dist = pythagorous2(vec.x, vec.y);
// if current location is exactly at the center of the circle return edge directly behind vehicle
if (dist == 0) {
result.x = _center.x - _radius * _ahrs.cos_yaw();
result.y = _center.y - _radius * _ahrs.sin_yaw();
result.z = _center.z;
return;
}
// calculate closest point on edge of circle
result.x = _center.x + vec.x / dist * _radius;
result.y = _center.y + vec.y / dist * _radius;
result.z = _center.z;
}
// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
// this should be called whenever the radius or rate are changed
// initialises the yaw and current position around the circle
@ -153,7 +195,6 @@ void AC_Circle::calc_velocities()
{
// if we are doing a panorama set the circle_angle to the current heading
if (_radius <= 0) {
_angle = _ahrs.yaw;
_angular_vel_max = ToRad(_rate);
_angular_accel = _angular_vel_max; // reach maximum yaw velocity in 1 second
}else{
@ -174,7 +215,36 @@ void AC_Circle::calc_velocities()
}
}
// initialise other variables
_angle_total = 0;
// initialise angular velocity
_angular_vel = 0;
}
// init_start_angle - sets the starting angle around the circle and initialises the angle_total
// if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
// if use_heading is false the vehicle's position from the center will be used to initialise the angle
void AC_Circle::init_start_angle(bool use_heading)
{
// initialise angle total
_angle_total = 0;
// if the radius is zero we are doing panorama so init angle to the current heading
if (_radius <= 0) {
_angle = _ahrs.yaw;
return;
}
// if use_heading is true
if (use_heading) {
_angle = wrap_PI(_ahrs.yaw-PI);
} else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f &curr_pos = _inav.get_position();
if (curr_pos.x == _center.x && curr_pos.y == _center.y) {
_angle = wrap_PI(_ahrs.yaw-PI);
} else {
// get bearing from circle center to vehicle in radians
float bearing_rad = ToRad(90) + atan2f(-(curr_pos.x-_center.x), curr_pos.y-_center.y);
_angle = wrap_PI(bearing_rad);
}
}
}

33
libraries/AC_WPNav/AC_Circle.h
View File

@ -21,17 +21,22 @@ public:
/// Constructor
AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control);
/// set_circle_center in cm from home
void set_center(const Vector3f& position);
/// init - initialise circle controller setting center specifically
void init(const Vector3f& center);
/// init_center in cm from home using stopping point and projecting out based on the copter's heading
void init_center();
/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
void init();
/// set_circle_center in cm from home
void set_center(const Vector3f& center) { _center = center; }
/// get_circle_center in cm from home
const Vector3f& get_center() const { return _center; }
/// get_radius - returns radius of circle in cm
float get_radius() { return _radius; }
/// set_radius - sets circle radius in cm
void set_radius(float radius_cm) { _radius = radius_cm; };
void set_radius(float radius_cm) { _radius = radius_cm; }
/// set_circle_rate - set circle rate in degrees per second
void set_rate(float deg_per_sec) { _rate = deg_per_sec; }
@ -43,9 +48,16 @@ public:
void update();
/// get desired roll, pitch which should be fed into stabilize controllers
int32_t get_roll() const { return _pos_control.get_roll(); };
int32_t get_pitch() const { return _pos_control.get_pitch(); };
int32_t get_yaw() const { return _yaw; };
int32_t get_roll() const { return _pos_control.get_roll(); }
int32_t get_pitch() const { return _pos_control.get_pitch(); }
int32_t get_yaw() const { return _yaw; }
// get_closest_point_on_circle - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void get_closest_point_on_circle(Vector3f &result);
static const struct AP_Param::GroupInfo var_info[];
@ -56,6 +68,11 @@ private:
// initialises the yaw and current position around the circle
void calc_velocities();
// init_start_angle - sets the starting angle around the circle and initialises the angle_total
// if use_heading is true the vehicle's heading will be used to init the angle causing minimum yaw movement
// if use_heading is false the vehicle's position from the center will be used to initialise the angle
void init_start_angle(bool use_heading);
// flags structure
struct circle_flags {
uint8_t panorama : 1; // true if we are doing a panorama