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

AC_Circle: add Circle options

This commit is contained in:
Iampete1 2020-09-15 15:50:33 +01:00 committed by Andrew Tridgell
parent ac8dc26d41
commit 4e064ef812
2 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -24,12 +24,12 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("RATE", 1, AC_Circle, _rate, AC_CIRCLE_RATE_DEFAULT), AP_GROUPINFO("RATE", 1, AC_Circle, _rate, AC_CIRCLE_RATE_DEFAULT),
// @Param: CONTROL // @Param: OPTIONS
// @DisplayName: Circle control // @DisplayName: Circle options
// @Description: Enable or disable using the pitch/roll stick control circle mode's radius and rate // @Description: 0:Enable or disable using the pitch/roll stick control circle mode's radius and rate
// @Values: 0:Disable,1:Enable // @Bitmask: 0:manual control, 1:face direction of travel, 2:Start at center rather than on perimeter
// @User: Standard // @User: Standard
AP_GROUPINFO("CONTROL", 2, AC_Circle, _control, 1), AP_GROUPINFO("OPTIONS", 2, AC_Circle, _options, 1),
AP_GROUPEND AP_GROUPEND
}; };
@ -96,9 +96,11 @@ void AC_Circle::init()
const Vector3f& stopping_point = _pos_control.get_pos_target(); const Vector3f& stopping_point = _pos_control.get_pos_target();
// set circle center to circle_radius ahead of stopping point // set circle center to circle_radius ahead of stopping point
_center.x = stopping_point.x + _radius * _ahrs.cos_yaw(); _center = stopping_point;
_center.y = stopping_point.y + _radius * _ahrs.sin_yaw(); if ((_options.get() & CircleOptions::INIT_AT_CENTER) == 0) {
_center.z = stopping_point.z; _center.x += _radius * _ahrs.cos_yaw();
_center.y += _radius * _ahrs.sin_yaw();
}
_terrain_alt = false; _terrain_alt = false;
// calculate velocities // calculate velocities
@ -209,6 +211,12 @@ bool AC_Circle::update()
// heading is from vehicle to center of circle // heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position(), _center); _yaw = get_bearing_cd(_inav.get_position(), _center);
if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
_yaw += is_positive(_rate)?-9000.0f:9000.0f;
_yaw = wrap_360_cd(_yaw);
}
} else { } else {
// set target position to center // set target position to center
Vector3f target; Vector3f target;

10
libraries/AC_WPNav/AC_Circle.h
View File

@ -86,7 +86,7 @@ public:
int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target(); } 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 /// true if pilot control of radius and turn rate is enabled
bool pilot_control_enabled() const { return _control > 0; } bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; }
/// provide rangefinder altitude /// provide rangefinder altitude
void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; } void set_rangefinder_alt(bool use, bool healthy, float alt_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_alt_cm = alt_cm; }
@ -127,10 +127,16 @@ private:
const AP_AHRS_View& _ahrs; const AP_AHRS_View& _ahrs;
AC_PosControl& _pos_control; AC_PosControl& _pos_control;
enum CircleOptions {
MANUAL_CONTROL = 1U << 0,
FACE_DIRECTION_OF_TRAVEL = 1U << 1,
INIT_AT_CENTER = 1U << 2, // true then the circle center will be the current location, false and the center will be 1 radius ahead
};
// parameters // parameters
AP_Float _radius; // maximum horizontal speed in cm/s during missions AP_Float _radius; // maximum horizontal speed in cm/s during missions
AP_Float _rate; // rotation speed in deg/sec AP_Float _rate; // rotation speed in deg/sec
AP_Int16 _control; // stick control enable/disable AP_Int16 _options; // stick control enable/disable
// internal variables // internal variables
Vector3f _center; // center of circle in cm from home Vector3f _center; // center of circle in cm from home