ardupilot/libraries/AC_WPNav/AC_WPNav.h

232 lines
12 KiB
C
Raw Normal View History

2013-03-20 03:27:26 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AC_WPNAV_H
#define AC_WPNAV_H
#include <inttypes.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
// loiter maximum velocities and accelerations
#define MAX_LOITER_POS_VELOCITY 750.0f // maximum velocity that our position controller will request. should be 1.5 ~ 2.0 times the pilot input's max velocity. To-Do: make consistent with maximum velocity requested by pilot input to loiter
#define MAX_LOITER_POS_ACCEL 500.0f // defines the velocity vs distant curve. maximum acceleration in cm/s/s that loiter position controller asks for from acceleration controller
#define MAX_LOITER_VEL_ACCEL 800.0f // max acceleration in cm/s/s that the loiter velocity controller will ask from the lower accel controller.
// should be 1.5 times larger than MAX_LOITER_POS_ACCEL.
// max acceleration = max lean angle * 980 * pi / 180. i.e. 23deg * 980 * 3.141 / 180 = 393 cm/s/s
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle
2013-03-20 03:27:26 -03:00
#define MAX_LOITER_OVERSHOOT 812.5f // maximum distance (in cm) that we will allow the target loiter point to be from the current location when switching into loiter
// D0 = MAX_LOITER_POS_ACCEL/(2*Pid_P^2);
// if MAX_LOITER_POS_VELOCITY > 2*D0*Pid_P
// MAX_LOITER_OVERSHOOT = D0 + MAX_LOITER_POS_VELOCITY.^2 ./ (2*MAX_LOITER_POS_ACCEL);
// else
// MAX_LOITER_OVERSHOOT = min(200, MAX_LOITER_POS_VELOCITY/Pid_P); // to stop it being over sensitive to error
// end
#define WPNAV_WP_SPEED 500.0f // default horizontal speed betwen waypoints in cm/s
#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm
#define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity
#define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity
#define WPNAV_ALT_HOLD_P 2.0f // hard coded estimate of throttle controller's altitude hold's P gain. To-Do: retrieve gain from throttle controller
#define WPNAV_ALT_HOLD_ACCEL_MAX 250.0f // hard coded estimate of throttle controller's maximum acceleration in cm/s. To-Do: retrieve from throttle controller
#define WPNAV_WP_ACCELERATION 200.0f // acceleration in cm/s/s used to increase the speed of the intermediate point up to it's maximum speed held in _speed_xy_cms
2013-03-20 03:27:26 -03:00
class AC_WPNav
{
public:
/// Constructor
2013-03-21 06:28:10 -03:00
AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
2013-03-20 03:27:26 -03:00
///
/// simple loiter controller
///
/// get_loiter_target - get loiter target as position vector (from home in cm)
const Vector3f &get_loiter_target() const { return _target; }
2013-03-20 03:27:26 -03:00
/// set_loiter_target in cm from home
void set_loiter_target(const Vector3f& position) { _target = position; }
2013-03-20 03:27:26 -03:00
/// set_loiter_target - set initial loiter target based on current position and velocity
void set_loiter_target(const Vector3f& position, const Vector3f& velocity);
/// move_loiter_target - move destination using pilot input
void move_loiter_target(float control_roll, float control_pitch, float dt);
2013-03-20 03:27:26 -03:00
/// get_distance_to_target - get horizontal distance to loiter target in cm
float get_distance_to_target() const;
/// get_bearing_to_target - get bearing to loiter target in centi-degrees
int32_t get_bearing_to_target() const;
/// update_loiter - run the loiter controller - should be called at 10hz
void update_loiter();
/// set_angle_limit - limits maximum angle in centi-degrees the copter will lean
void set_angle_limit(int32_t lean_angle) { _lean_angle_max = lean_angle; }
/// clear_angle_limit - reset angle limits back to defaults
void clear_angle_limit() { _lean_angle_max = MAX_LEAN_ANGLE; }
/// get_angle_limit - retrieve maximum angle in centi-degrees the copter will lean
int32_t get_angle_limit() const { return _lean_angle_max; }
2013-03-20 03:27:26 -03:00
///
/// waypoint controller
2013-03-20 03:27:26 -03:00
///
/// get_destination waypoint using position vector (distance from home in cm)
const Vector3f &get_destination() const { return _destination; }
/// set_destination waypoint using position vector (distance from home in cm)
2013-03-20 03:27:26 -03:00
void set_destination(const Vector3f& destination);
/// set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
2013-03-20 03:27:26 -03:00
void set_origin_and_destination(const Vector3f& origin, const Vector3f& destination);
/// advance_target_along_track - move target location along track from origin to destination
void advance_target_along_track(float dt);
2013-03-20 03:27:26 -03:00
/// get_distance_to_destination - get horizontal distance to destination in cm
float get_distance_to_destination();
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t get_bearing_to_destination();
/// reached_destination - true when we have come within RADIUS cm of the waypoint
bool reached_destination() const { return _reached_destination; }
/// update_wp - update waypoint controller
void update_wpnav();
2013-03-20 03:27:26 -03:00
///
/// shared methods
///
/// get desired roll, pitch which should be fed into stabilize controllers
int32_t get_desired_roll() const { return _desired_roll; };
int32_t get_desired_pitch() const { return _desired_pitch; };
2013-03-20 03:27:26 -03:00
/// get_desired_alt - get desired altitude (in cm above home) from loiter or wp controller which should be fed into throttle controller
float get_desired_alt() const { return _target.z; }
/// set_desired_alt - set desired altitude (in cm above home)
2013-04-08 23:58:18 -03:00
void set_desired_alt(float desired_alt) { _target.z = desired_alt; }
2013-03-20 03:27:26 -03:00
/// 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, float cos_pitch) {
2013-03-20 03:27:26 -03:00
_cos_yaw = cos_yaw;
_sin_yaw = sin_yaw;
_cos_pitch = cos_pitch;
2013-03-20 03:27:26 -03:00
}
/// set_horizontal_velocity - allows main code to pass target horizontal velocity for wp navigation
void set_horizontal_velocity(float velocity_cms) { _speed_xy_cms = velocity_cms; };
/// get_climb_velocity - returns target climb speed in cm/s during missions
float get_climb_velocity() const { return _speed_up_cms; };
/// get_descent_velocity - returns target descent speed in cm/s during missions. Note: always positive
float get_descent_velocity() const { return _speed_down_cms; };
/// get_waypoint_radius - access for waypoint radius in cm
float get_waypoint_radius() const { return _wp_radius_cm; }
2013-03-20 03:27:26 -03:00
static const struct AP_Param::GroupInfo var_info[];
protected:
/// project_stopping_point - returns vector to stopping point based on a horizontal position and velocity
void project_stopping_point(const Vector3f& position, const Vector3f& velocity, Vector3f &target);
/// translate_loiter_target_movements - consumes adjustments created by move_loiter_target
void translate_loiter_target_movements(float nav_dt);
2013-04-15 09:54:29 -03:00
/// get_loiter_position_to_velocity - loiter position controller
/// converts desired position held in _target vector to desired velocity
void get_loiter_position_to_velocity(float dt);
2013-03-20 03:27:26 -03:00
2013-04-15 09:54:29 -03:00
/// get_loiter_velocity_to_acceleration - loiter velocity controller
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
void get_loiter_velocity_to_acceleration(float vel_lat_cms, float vel_lon_cms, float dt);
2013-03-20 03:27:26 -03:00
2013-04-15 09:54:29 -03:00
/// get_loiter_acceleration_to_lean_angles - loiter acceleration controller
2013-03-20 03:27:26 -03:00
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
2013-04-15 09:54:29 -03:00
void get_loiter_acceleration_to_lean_angles(float accel_lat_cmss, float accel_lon_cmss);
2013-03-20 03:27:26 -03:00
/// get_bearing_cd - return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const;
/// reset_I - clears I terms from loiter PID controller
void reset_I();
2013-03-20 03:27:26 -03:00
/// calculate_leash_length - calculates horizontal and vertical leash lengths for waypoint controller
/// set climb param to true if track climbs vertically, false if descending
void calculate_leash_length(bool climb);
2013-03-20 03:27:26 -03:00
// pointers to inertial nav library
AP_InertialNav* _inav;
// pointers to pid controllers
APM_PI* _pid_pos_lat;
APM_PI* _pid_pos_lon;
AC_PID* _pid_rate_lat;
AC_PID* _pid_rate_lon;
// parameters
AP_Float _speed_xy_cms; // horizontal speed target in cm/s
AP_Float _speed_up_cms; // climb speed target in cm/s
AP_Float _speed_down_cms; // descent speed target in cm/s
AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
uint32_t _loiter_last_update; // time of last update_loiter call
uint32_t _wpnav_last_update; // time of last update_wpnav call
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
2013-03-20 03:27:26 -03:00
float _sin_yaw;
float _cos_pitch;
2013-03-20 03:27:26 -03:00
// output from controller
int32_t _desired_roll; // fed to stabilize controllers at 50hz
int32_t _desired_pitch; // fed to stabilize controllers at 50hz
2013-03-20 03:27:26 -03:00
// loiter controller internal variables
2013-03-20 03:27:26 -03:00
Vector3f _target; // loiter's target location in cm from home
int16_t _pilot_vel_forward_cms; // pilot's desired velocity forward (body-frame)
int16_t _pilot_vel_right_cms; // pilot's desired velocity right (body-frame)
Vector3f _target_vel; // pilot's latest desired velocity in earth-frame
Vector3f _vel_last; // previous iterations velocity in cm/s
int32_t _lean_angle_max; // maximum lean angle. can be set from main code so that throttle controller can stop leans that cause copter to lose altitude
// waypoint controller internal variables
2013-03-20 03:27:26 -03:00
Vector3f _origin; // starting point of trip to next waypoint in cm from home (equivalent to next_WP)
Vector3f _destination; // target destination in cm from home (equivalent to next_WP)
Vector3f _pos_delta_unit; // each axis's percentage of the total track from origin to destination
2013-03-20 03:27:26 -03:00
float _track_length; // distance in cm between origin and destination
float _track_desired; // our desired distance along the track in cm
float _distance_to_target; // distance to loiter target
bool _reached_destination; // true if we have reached the destination
float _vert_track_scale; // vertical scaling to give altitude equal weighting to horizontal position
float _leash_xy; // horizontal leash length in cm
float _limited_speed_xy_cms; // horizontal speed in cm/s used to advance the intermediate target towards the destination. used to limit extreme acceleration after passing a waypoint
2013-04-15 09:54:29 -03:00
public:
// for logging purposes
Vector2f dist_error; // distance error calculated by loiter controller
Vector2f desired_vel; // loiter controller desired velocity
Vector2f desired_accel; // the resulting desired acceleration
// To-Do: add split of fast (100hz for accel->angle) and slow (10hz for navigation)
/// update - run the loiter and wpnav controllers - should be called at 100hz
//void update_100hz(void);
/// update - run the loiter and wpnav controllers - should be called at 10hz
//void update_10hz(void);
2013-03-20 03:27:26 -03:00
};
#endif // AC_WPNAV_H