Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Copter: Pilot RC stick control of Circle mode radius, rate, & direction #12829

Open
wants to merge 2 commits into
base: master
from
Open
Changes from all commits
Commits
File filter...
Filter file types
Jump to…
Jump to file or symbol
Failed to load files and symbols.

Always

Just for now

@@ -39,6 +39,56 @@ void ModeCircle::run()
pilot_yaw_override = true;
}

// pilot changes to circle rate and radius
// skip if in radio failsafe
if (!copter.failsafe.radio && !is_zero(copter.circle_nav->get_control())) {
// update the circle controller's radius target based on pilot pitch stick inputs
float radius_current = copter.circle_nav->get_radius(); // circle controller's radius target, which begins as the circle_radius parameter
float pitch_stick = channel_pitch->norm_input_dz(); // pitch stick normalized -1 to 1
float nav_speed = copter.wp_nav->get_default_speed_xy(); // copter loiter speed
float radius_pilot_change = (pitch_stick * nav_speed) * G_Dt; // rate of change
float radius_new = MAX(radius_current + radius_pilot_change,0); // new radius target

if (!is_equal(radius_current,radius_new)) {
copter.circle_nav->set_radius(radius_new);
}

// update the orbicular rate target based on pilot roll stick inputs
// skip if using CH6 tuning knob for circle rate
if (g.radio_tuning != TUNING_CIRCLE_RATE) {
float rate = copter.circle_nav->get_rate(); // circle controller's rate target, which begins as the circle_rate parameter
float rate_current = copter.circle_nav->get_rate_current(); // actual rate, which is probably different from the target rate
float roll_stick = channel_roll->norm_input_dz(); // roll stick normalized -1 to 1
float rate_pilot_change = (roll_stick * G_Dt); // rate of change
float rate_new = rate_current; // new rate target
static bool speed_changing = false; // true when the roll stick is being held to facilitate stopping at 0 rate

if (is_zero(roll_stick)) {
// no speed change, so reset speed changing flag
if (speed_changing) {
speed_changing = false; }

} else {
if (is_positive(rate)) {
// currently moving clockwise, constrain 0 to 90
rate_new = constrain_float(rate_current + rate_pilot_change,0,90);
speed_changing = true;

} else if (is_negative(rate)) {
// currently moving counterclockwise, constrain -90 to 0
rate_new = constrain_float(rate_current + rate_pilot_change,-90,0);
speed_changing = true;

} else if (is_zero(rate) && !speed_changing) {
// Stopped, pilot has released the roll stick, and pilot now wants to begin moving with the roll stick
speed_changing = true;
rate_new = rate_pilot_change;
}
copter.circle_nav->set_rate(rate_new);
}
}
}

// get pilot desired climb rate (or zero if in radio failsafe)
float target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// adjust climb rate using rangefinder
@@ -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, 0),

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()
{
@@ -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
{
@@ -34,10 +35,17 @@ class AC_Circle

/// 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);

/// get_rate - returns rate in deg/sec
float get_rate() { return _rate; }

/// set_circle_rate - set circle rate in degrees per second
/// get_rate_current- returns actual current rate in deg/sec, which may be less than _rate)
float get_rate_current() {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
@@ -64,6 +72,9 @@ class AC_Circle
/// get bearing to target in centi-degrees
int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target(); }

/// get control parameter
float get_control() const { return _control;}

static const struct AP_Param::GroupInfo var_info[];

private:
@@ -92,6 +103,7 @@ class AC_Circle
// parameters
AP_Float _radius; // maximum horizontal speed in cm/s during missions
AP_Float _rate; // rotation speed in deg/sec
AP_Float _control; // stick control enable/disable

// internal variables
Vector3f _center; // center of circle in cm from home
ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.