AC_Circle: circle control library

libraries/AC_WPNav/AC_Circle.cpp

@@ -0,0 +1,179 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL.h>
+#include <AC_Circle.h>
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AC_Circle::var_info[] PROGMEM = {
+    // @Param: RADIUS
+    // @DisplayName: Circle Radius
+    // @Description: Defines the radius of the circle the vehicle will fly when in Circle flight mode
+    // @Units: cm
+    // @Range: 0 10000
+    // @Increment: 100
+    // @User: Standard
+    AP_GROUPINFO("RADIUS",  0,  AC_Circle, _radius, AC_CIRCLE_RADIUS_DEFAULT),
+
+    // @Param: RATE
+    // @DisplayName: Circle rate
+    // @Description: Circle mode's turn rate in deg/sec.  Positive to turn clockwise, negative for counter clockwise
+    // @Units: deg/s
+    // @Range: -90 90
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("RATE",    1, AC_Circle, _rate,    AC_CIRCLE_RATE_DEFAULT),
+
+    AP_GROUPEND
+};
+
+// Default constructor.
+// Note that the Vector/Matrix constructors already implicitly zero
+// their values.
+//
+AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control) :
+    _inav(inav),
+    _ahrs(ahrs),
+    _pos_control(pos_control),
+    _last_update(0),
+    _angle(0),
+    _cos_yaw(1.0),
+    _sin_yaw(0.0)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+/// set_circle_center in cm from home
+void AC_Circle::set_center(const Vector3f& position)
+{
+    _center = position;
+
+	// 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());
+
+	// To-Do: set _pos_control speed and accel
+
+    // calculate velocities
+    calc_velocities();
+}
+
+/// init_center in cm from home using stopping point and projecting out based on the copter's heading
+void AC_Circle::init_center()
+{
+    Vector3f stopping_point;
+
+    // get reasonable stopping point
+    _pos_control.get_stopping_point_xy(stopping_point);
+    _pos_control.get_stopping_point_z(stopping_point);
+
+    // set circle center to circle_radius ahead of stopping point
+    _center.x = stopping_point.x + _radius * _cos_yaw;
+    _center.y = stopping_point.y + _radius * _sin_yaw;
+    _center.z = stopping_point.z;
+
+    // update pos_control target to stopping point
+    _pos_control.set_pos_target(stopping_point);
+
+    // calculate velocities
+    calc_velocities();
+}
+
+/// update - update circle controller
+void AC_Circle::update()
+{
+    // calculate dt
+    uint32_t now = hal.scheduler->millis();
+    float dt = (now - _last_update) / 1000.0f;
+
+    // update circle position at 10hz
+    if (dt > 0.095f) {
+
+        // double check dt is reasonable
+        if (dt >= 1.0f) {
+            dt = 0.0;
+        }
+        // capture time since last iteration
+        _last_update = now;
+
+        // ramp up angular velocity to maximum
+        if (_rate >= 0) {
+            if (_angular_vel < _angular_vel_max) {
+                _angular_vel += _angular_accel * dt;
+                _angular_vel = constrain_float(_angular_vel, 0, _angular_vel_max);
+            }
+        }else{
+            if (_angular_vel > _angular_vel_max) {
+                _angular_vel += _angular_accel * dt;
+                _angular_vel = constrain_float(_angular_vel, _angular_vel_max, 0);
+            }
+        }
+
+        // update the target angle and total angle traveled
+        float angle_change = _angular_vel * dt;
+        _angle += angle_change;
+        _angle = wrap_PI(_angle);
+        _angle_total += angle_change;
+
+        // heading is 180 deg from vehicles target position around circle
+        _yaw = wrap_PI(_angle-PI);
+
+        // if the circle_radius is zero we are doing panorama so no need to update loiter target
+        if (_radius != 0.0) {
+            // calculate target position
+            Vector3f target;
+            target.x = _center.x + _radius * cosf(-_angle);
+            target.y = _center.y - _radius * sinf(-_angle);
+            target.z = _pos_control.get_alt_target();
+
+            // update position controller target
+            _pos_control.set_pos_target(target);
+
+            // heading is 180 deg from vehicles target position around circle
+            _yaw = wrap_PI(_angle-PI);
+        }else{
+            // heading is 180 deg from vehicles target position around circle
+            _yaw = wrap_PI(_angle-PI);
+        }
+
+        // trigger position controller on next update
+        _pos_control.trigger_xy();
+    }
+
+    // run loiter's position to velocity step
+    _pos_control.update_pos_controller(false);
+}
+
+// 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
+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{
+        // set starting angle to current heading - 180 degrees
+        _angle = wrap_PI(_ahrs.yaw-PI);
+
+        // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
+        float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));
+
+        // angular_velocity in radians per second
+        _angular_vel_max = velocity_max/_radius;
+        _angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);
+
+        // angular_velocity in radians per second
+        _angular_accel = _pos_control.get_accel_xy()/_radius;
+        if (_rate < 0.0f) {
+            _angular_accel = -_angular_accel;
+        }
+    }
+
+    // debug -- remove me!
+    //hal.console->printf_P(PSTR("\nPan Ang:%4.2f AVM:%4.2f Acc:%4.2f\n"),(float)_angle,(float)_angular_vel_max,(float)_angular_accel);
+
+    // initialise other variables
+    _angle_total = 0;
+    _angular_vel = 0;
+}

libraries/AC_WPNav/AC_Circle.h

@@ -0,0 +1,94 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#ifndef AC_CIRCLE_H
+#define AC_CIRCLE_H
+
+#include <AP_Common.h>
+#include <AP_Param.h>
+#include <AP_Math.h>
+#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 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
+
+class AC_Circle
+{
+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_center in cm from home using stopping point and projecting out based on the copter's heading
+    void init_center();
+
+    /// get_circle_center in cm from home
+    const Vector3f& get_center() const { return _center; }
+
+    /// set_radius - sets circle radius in 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; }
+
+    /// get_angle_total - return total angle in radians that vehicle has circled
+    float get_angle_total() const { return _angle_total; }
+
+    /// update - update circle controller
+    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; };
+
+    /// set_cos_sin_yaw - short-cut to save on calculations to convert from roll-pitch frame to lat-lon frame
+    void set_cos_sin_yaw(float cos_yaw, float sin_yaw) {
+        _cos_yaw = cos_yaw;
+        _sin_yaw = sin_yaw;
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+
+    // 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
+    void calc_velocities();
+
+    // flags structure
+    struct circle_flags {
+        uint8_t panorama    : 1;    // true if we are doing a panorama
+        uint8_t dir         : 1;    // 0 = clockwise, 1 = counter clockwise
+    } _flags;
+
+    // references to inertial nav and ahrs libraries
+    const AP_InertialNav&       _inav;
+    const AP_AHRS&              _ahrs;
+    AC_PosControl&              _pos_control;
+
+    // parameters
+    AP_Float    _radius;        // maximum horizontal speed in cm/s during missions
+    AP_Float    _rate;          // rotation speed in deg/sec
+
+    // internal variables
+    uint32_t    _last_update;   // time of last update_loiter call
+    Vector3f    _center;        // center of circle in cm from home
+    float       _yaw;           // yaw heading (normally towards circle center)
+    float       _angle;         // current angular position around circle in radians (0=directly north of the center of the circle)
+    float       _angle_total;   // total angle travelled in radians
+    float       _angular_vel;   // angular velocity in radians/sec
+    float       _angular_vel_max;   // maximum velocity in radians/sec
+    float       _angular_accel; // angular acceleration in radians/sec/sec
+
+    // helper variables
+    float       _cos_yaw;       // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
+    float       _sin_yaw;       // To-Do: move these to ahrs or attitude control class to save on memory
+};
+#endif	// AC_CIRCLE_H