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

AC_Circle: circle control library

This commit is contained in:
Randy Mackay 2014-01-27 14:08:11 +09:00 committed by Andrew Tridgell
parent e979263c56
commit 864f64b61a
2 changed files with 273 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

179
libraries/AC_WPNav/AC_Circle.cpp Normal file
View File

@ -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;
}

94
libraries/AC_WPNav/AC_Circle.h Normal file
View File

@ -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