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

AC_WPNav: Circle mode pilot control of rate & radius

Browse Source
This commit is contained in:
Matt Lawrence 2020-01-06 17:35:17 -05:00 committed by Randy Mackay
parent db48386463
commit 0d2b42e4f4
2 changed files with 33 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -9,7 +9,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
// @DisplayName: Circle Radius
// @Description: Defines the radius of the circle the vehicle will fly when in Circle flight mode
// @Units: cm
// @Range: 0 10000
// @Range: 0 200000
// @Increment: 100
// @User: Standard
AP_GROUPINFO("RADIUS", 0, AC_Circle, _radius, AC_CIRCLE_RADIUS_DEFAULT),
@ -23,6 +23,13 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
// @User: Standard
AP_GROUPINFO("RATE", 1, AC_Circle, _rate, AC_CIRCLE_RATE_DEFAULT),
// @Param: CONTROL
// @DisplayName: Circle control
// @Description: Enable or disable using the pitch/roll stick control circle mode's radius and rate
// @Values: 0:Disable,1:Enable
// @User: Standard
AP_GROUPINFO("CONTROL", 2, AC_Circle, _control, 1),
AP_GROUPEND
};
@ -106,6 +113,13 @@ void AC_Circle::set_rate(float deg_per_sec)
}
}
/// set_circle_rate - set circle rate in degrees per second
void AC_Circle::set_radius(float radius_cm)
{
_radius = constrain_float(radius_cm, 0, AC_CIRCLE_RADIUS_MAX);
calc_velocities(false);
}
/// update - update circle controller
void AC_Circle::update()
{
@ -167,7 +181,7 @@ void AC_Circle::update()
// 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)
void AC_Circle::get_closest_point_on_circle(Vector3f &result) const
{
// return center if radius is zero
if (_radius <= 0) {

22
libraries/AC_WPNav/AC_Circle.h
View File

@ -10,6 +10,7 @@
#define AC_CIRCLE_RADIUS_DEFAULT 1000.0f // radius of the circle in cm that the vehicle will fly
#define AC_CIRCLE_RATE_DEFAULT 20.0f // turn rate in deg/sec. Positive to turn clockwise, negative for counter clockwise
#define AC_CIRCLE_ANGULAR_ACCEL_MIN 2.0f // angular acceleration should never be less than 2deg/sec
#define AC_CIRCLE_RADIUS_MAX 200000.0f // maximum allowed circle radius of 2km
class AC_Circle
{
@ -33,11 +34,18 @@ public:
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; }
float get_radius() const { return _radius; }
/// set_circle_rate - set circle rate in degrees per second
/// set_radius - sets circle radius in cm
void set_radius(float radius_cm);
/// get_rate - returns target rate in deg/sec held in RATE parameter
float get_rate() const { return _rate; }
/// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate
float get_rate_current() const { return ToDeg(_angular_vel); }
/// set_rate - set circle rate in degrees per second
void set_rate(float deg_per_sec);
/// get_angle_total - return total angle in radians that vehicle has circled
@ -56,7 +64,7 @@ public:
// 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);
void get_closest_point_on_circle(Vector3f &result) const;
/// get horizontal distance to loiter target in cm
float get_distance_to_target() const { return _pos_control.get_distance_to_target(); }
@ -64,6 +72,9 @@ public:
/// get bearing to target in centi-degrees
int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target(); }
/// true if pilot control of radius and turn rate is enabled
bool pilot_control_enabled() const { return _control > 0; }
static const struct AP_Param::GroupInfo var_info[];
private:
@ -92,6 +103,7 @@ private:
// parameters
AP_Float _radius; // maximum horizontal speed in cm/s during missions
AP_Float _rate; // rotation speed in deg/sec
AP_Int16 _control; // stick control enable/disable
// internal variables
Vector3f _center; // center of circle in cm from home