AC_WPNav: first implementation

libraries/AC_WPNav/AC_WPNav.cpp

@@ -0,0 +1,271 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#include <AP_HAL.h>
+#include <AC_WPNav.h>
+
+extern const AP_HAL::HAL& hal;
+
+const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
+    // index 0 was used for the old orientation matrix
+
+    // @Param: SPEED
+    // @DisplayName: Speed in cm/s to travel between waypoints
+    // @Description: The desired horizontal speed in cm/s while travelling between waypoints
+    // @Range: 0 1000
+    // @Increment: 50
+    AP_GROUPINFO("SPEED",    0, AC_WPNav, _speed_cms, WP_SPEED),
+
+    AP_GROUPEND
+};
+
+// Default constructor.
+// Note that the Vector/Matrix constructors already implicitly zero
+// their values.
+//
+AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_lat_rate, AC_PID* pid_lon_rate) :
+    _inav(inav),
+    _pid_pos_lat(pid_pos_lat),
+    _pid_pos_lon(pid_pos_lon),
+    _pid_rate_lat(pid_lat_rate),
+    _pid_rate_lon(pid_lon_rate)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+///
+/// simple loiter controller
+///
+
+/// set_loiter_target in cm from home
+void AC_WPNav::set_loiter_target(const Vector3f& position)
+{
+    _state = WPNAV_STATE_LOITER;
+    _target = position;
+}
+
+
+/// move_loiter_target - move loiter target by velocity provided in front/right directions in cm/s
+void AC_WPNav::move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms, float dt)
+{
+    int32_t vel_lat;
+    int32_t vel_lon;
+    int32_t vel_total;
+
+    vel_lat = vel_forward_cms*_cos_yaw - vel_right_cms*_sin_yaw;
+    vel_lon = vel_forward_cms*_sin_yaw + vel_right_cms*_cos_yaw;
+
+    // constrain the velocity vector and scale if necessary
+    vel_total = safe_sqrt(vel_lat*vel_lat + vel_lon*vel_lon);
+    if( vel_total > MAX_LOITER_POS_VEL_VELOCITY ) {
+        vel_lat = MAX_LOITER_POS_VEL_VELOCITY * vel_lat/vel_total;
+        vel_lon = MAX_LOITER_POS_VEL_VELOCITY * vel_lon/vel_total;
+    }
+
+    // update target position
+    _target.x += vel_lat * dt;
+    _target.y += vel_lon * dt;
+}
+
+///
+/// waypoint navigation
+///
+
+/// set_destination - set destination using cm from home
+void AC_WPNav::set_destination(const Vector3f& destination)
+{
+    set_origin_and_destination(_inav->get_position(), destination);
+}
+
+/// set_origin_and_destination - set origin and destination using lat/lon coordinates
+void AC_WPNav::set_origin_and_destination(const Vector3f& origin, const Vector3f& destination)
+{
+    _state = WPNAV_STATE_WPNAV;
+    _origin = origin;
+    _destination = destination;
+    _pos_delta = _destination - _origin;
+    _track_length = _pos_delta.length();
+    _track_desired = 0;
+}
+
+/// advance_target_along_track - move target location along track from origin to destination
+void AC_WPNav::advance_target_along_track(float velocity_cms, float dt)
+{
+    float cross_track_dist;
+    float track_covered;
+    float track_desired_max;
+    float line_a, line_b, line_c, line_m;
+
+    // get current location
+    Vector3f curr = _inav->get_position();
+
+    // check for zero length segment
+    if( _pos_delta.x == 0 && _pos_delta.y == 0) {
+        _target = _destination;
+        return;
+    }
+
+    if( _pos_delta.x == 0 ) {
+        // x is zero
+        cross_track_dist = fabs(curr.x - _destination.x);
+        track_covered = fabs(curr.y - _origin.y);
+    }else if(_pos_delta.y == 0) {
+        // y is zero
+        cross_track_dist = fabs(curr.y - _destination.y);
+        track_covered = fabs(curr.x - _origin.x);
+    }else{
+        // both x and y non zero
+        line_a = _pos_delta.y;
+        line_b = -_pos_delta.x;
+        line_c = _pos_delta.x * _origin.y - _pos_delta.y * _origin.x;
+        line_m = line_a / line_b;
+        cross_track_dist = abs(line_a * curr.x + line_b * curr.y + line_c ) / _track_length;
+
+        line_m = 1/line_m;
+        line_a = line_m;
+        line_b = -1;
+        line_c = curr.y - line_m * curr.x;
+
+        // calculate the distance to the closest point along the track and it's distance from the origin
+        track_covered = abs(line_a*_origin.x + line_b*_origin.y + line_c) / safe_sqrt(line_a*line_a+line_b*line_b);
+    }
+
+    // maximum distance along the track that we will allow (stops target point from getting too far from the current position)
+    track_desired_max = track_covered + safe_sqrt(WPINAV_MAX_POS_ERROR*WPINAV_MAX_POS_ERROR-cross_track_dist*cross_track_dist);
+
+    // advance the current target
+    _track_desired += velocity_cms * dt;
+
+    // constrain the target from moving too far
+    if( _track_desired > track_desired_max ) {
+        _track_desired = track_desired_max;
+    }
+    if( _track_desired > _track_length ) {
+        _track_desired = _track_length;
+    }
+
+    // recalculate the desired position
+    float track_length_pct = _track_desired/_track_length;
+    _target.x = _origin.x + _pos_delta.x * track_length_pct;
+    _target.y = _origin.y + _pos_delta.y * track_length_pct;
+}
+
+///
+/// shared methods
+///
+
+/// update - run the loiter and wpnav controllers - should be called at 10hz
+void AC_WPNav::update()
+{
+    float dt = hal.scheduler->millis() - _last_update;
+
+    // prevent run up in dt value
+    dt = min(dt, 1.0f);
+
+    // advance the target if necessary
+    if( _state == WPNAV_STATE_WPNAV ) {
+        advance_target_along_track(_speed_cms, dt);
+    }
+
+    // run loiter position controller
+    get_loiter_pos_lat_lon(_target.x, _target.y, dt);
+}
+
+// get_loiter_pos_lat_lon - loiter position controller
+//     converts desired position provided as distance from home in lat/lon directions to desired velocity
+void AC_WPNav::get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_home, float dt)
+{
+    float dist_error_lat;
+    int32_t desired_vel_lat;
+
+    float dist_error_lon;
+    int32_t desired_vel_lon;
+
+    int32_t dist_error_total;
+
+    int16_t vel_sqrt;
+    int32_t vel_total;
+
+    int16_t linear_distance;      // the distace we swap between linear and sqrt.
+
+    // calculate distance error
+    dist_error_lat = target_lat_from_home - _inav->get_latitude_diff();
+    dist_error_lon = target_lon_from_home - _inav->get_longitude_diff();
+
+    linear_distance = MAX_LOITER_POS_ACCEL/(2*_pid_pos_lat->kP()*_pid_pos_lat->kP());
+
+    dist_error_total = safe_sqrt(dist_error_lat*dist_error_lat + dist_error_lon*dist_error_lon);
+    if( dist_error_total > 2*linear_distance ) {
+        vel_sqrt = constrain(safe_sqrt(2*MAX_LOITER_POS_ACCEL*(dist_error_total-linear_distance)),0,1000);
+        desired_vel_lat = vel_sqrt * dist_error_lat/dist_error_total;
+        desired_vel_lon = vel_sqrt * dist_error_lon/dist_error_total;
+    }else{
+        desired_vel_lat = _pid_pos_lat->get_p(dist_error_lat);
+        desired_vel_lon = _pid_pos_lon->get_p(dist_error_lon);
+    }
+
+    vel_total = safe_sqrt(desired_vel_lat*desired_vel_lat + desired_vel_lon*desired_vel_lon);
+    if( vel_total > MAX_LOITER_POS_VELOCITY ) {
+        desired_vel_lat = MAX_LOITER_POS_VELOCITY * desired_vel_lat/vel_total;
+        desired_vel_lon = MAX_LOITER_POS_VELOCITY * desired_vel_lon/vel_total;
+    }
+
+    get_loiter_vel_lat_lon(desired_vel_lat, desired_vel_lon, dt);
+}
+
+// get_loiter_vel_lat_lon - loiter velocity controller
+//    converts desired velocities in lat/lon frame to accelerations in lat/lon frame
+void AC_WPNav::get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt)
+{
+    float speed_error_lat = 0;     // The velocity in cm/s.
+    float speed_error_lon = 0;     // The velocity in cm/s.
+
+    float speed_lat = _inav->get_latitude_velocity();
+    float speed_lon = _inav->get_longitude_velocity();
+
+    int32_t accel_lat;
+    int32_t accel_lon;
+    int32_t accel_total;
+
+    int16_t lat_p,lat_i,lat_d;
+    int16_t lon_p,lon_i,lon_d;
+
+    // calculate vel error
+    speed_error_lat = vel_lat - speed_lat;
+    speed_error_lon = vel_lon - speed_lon;
+
+    lat_p   = _pid_rate_lat->get_p(speed_error_lat);
+    lat_i   = _pid_rate_lat->get_i(speed_error_lat, dt);
+    lat_d   = _pid_rate_lat->get_d(speed_error_lat, dt);
+
+    lon_p   = _pid_rate_lon->get_p(speed_error_lon);
+    lon_i   = _pid_rate_lon->get_i(speed_error_lon, dt);
+    lon_d   = _pid_rate_lon->get_d(speed_error_lon, dt);
+
+    accel_lat = (lat_p+lat_i+lat_d);
+    accel_lon = (lon_p+lon_i+lon_d);
+
+    accel_total = safe_sqrt(accel_lat*accel_lat + accel_lon*accel_lon);
+
+    if( accel_total > MAX_LOITER_VEL_ACCEL ) {
+        accel_lat = MAX_LOITER_VEL_ACCEL * accel_lat/accel_total;
+        accel_lon = MAX_LOITER_VEL_ACCEL * accel_lon/accel_total;
+    }
+
+    get_loiter_accel_lat_lon(accel_lat, accel_lon);
+}
+
+// get_loiter_accel_lat_lon - loiter acceration controller
+//    converts desired accelerations provided in lat/lon frame to roll/pitch angles
+void AC_WPNav::get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon)
+{
+    float z_accel_meas = -AP_INTERTIALNAV_GRAVITY * 100;    // gravity in cm/s/s
+    float accel_forward;
+    float accel_right;
+
+    // rotate accelerations into body forward-right frame
+    accel_forward = accel_lat*_cos_yaw + accel_lon*_sin_yaw;
+    accel_right = -accel_lat*_sin_yaw + accel_lon*_cos_yaw;
+
+    // update angle targets that will be passed to stabilize controller
+    _desired_roll = constrain((accel_right/(-z_accel_meas))*(18000/M_PI), -4500, 4500);
+    _desired_pitch = constrain((-accel_forward/(-z_accel_meas*_cos_roll))*(18000/M_PI), -4500, 4500);
+}

libraries/AC_WPNav/AC_WPNav.h

@@ -0,0 +1,129 @@
+/// -*- 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 <AP_GPS.h>             // ArduPilot GPS library
+#include <AP_Baro.h>            // ArduPilot Mega Barometer Library
+#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         // should be 1.5 ~ 2.0 times the pilot input's max velocity
+#define MAX_LOITER_POS_ACCEL            250
+#define MAX_LOITER_VEL_ACCEL            400         // should be 1.5 times larger than MAX_LOITER_POS_ACCEL
+#define MAX_LOITER_POS_VEL_VELOCITY     1000
+#define WPINAV_MAX_POS_ERROR            2000.0f     // maximum distance (in cm) that the desired track can stray from our current location.
+#define WP_SPEED                        500         // default horizontal speed betwen waypoints in cm/s
+
+// possible states
+#define WPNAV_STATE_INACTIVE    0
+#define WPNAV_STATE_LOITER 	    1
+#define WPNAV_STATE_WPNAV       2
+
+class AC_WPNav
+{
+public:
+
+    /// Constructor
+    AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_lat_rate, AC_PID* pid_lon_rate);
+
+    ///
+    /// simple loiter controller
+    ///
+
+    /// set_loiter_target in cm from home
+    void set_loiter_target(const Vector3f& position);
+
+    /// move_loiter_target - move destination using forward and right velocities in cm/s
+    void move_loiter_target(int16_t vel_forward_cms, int16_t vel_right_cms, float dt);
+
+    ///
+    /// waypoint navigation
+    ///
+
+    /// set_destination with distance from home in cm
+    void set_destination(const Vector3f& destination);
+
+    /// set_origin_and_destination - set origin and destination using lat/lon coordinates
+    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 velocity_cms, float dt);
+
+    ///
+    /// shared methods
+    ///
+
+    /// update - run the loiter and wpnav controllers - should be called at 10hz
+    void update(void);
+
+    /// get desired roll, pitch and altitude which should be fed into stabilize controllers
+    int32_t get_desired_roll() { return _desired_roll; };
+    int32_t get_desired_pitch() { return _desired_pitch; };
+
+    /// desired altitude (in cm) that should be fed into altitude hold controller.  only valid when navigating between waypoints
+    int32_t get_desired_altitude() { return _desired_altitude; };
+
+    /// 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_roll) {
+        _cos_yaw = cos_yaw;
+        _sin_yaw = sin_yaw;
+        _cos_roll = cos_roll;
+    }
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+
+    /// get_loiter_pos_lat_lon - loiter position controller
+    ///     converts desired position provided as distance from home in lat/lon directions to desired velocity
+    void get_loiter_pos_lat_lon(int32_t target_lat_from_home, int32_t target_lon_from_home, float dt);
+
+    /// get_loiter_vel_lat_lon - loiter velocity controller
+    ///    converts desired velocities in lat/lon frame to accelerations in lat/lon frame
+    void get_loiter_vel_lat_lon(int16_t vel_lat, int16_t vel_lon, float dt);
+
+    /// get_loiter_accel_lat_lon - loiter acceration controller
+    ///    converts desired accelerations provided in lat/lon frame to roll/pitch angles
+    void get_loiter_accel_lat_lon(int16_t accel_lat, int16_t accel_lon);
+
+    /// waypoint controller
+    /// get_wpinav_pos - wpinav position controller with desired position held in wpinav_destination
+    void get_wpinav_pos(float dt);
+
+    // 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_cms;         // default horizontal speed in cm/s
+    uint8_t		_state;				// records whether we are loitering or navigating towards a waypoint
+    uint32_t	_last_update;		// time of last update call
+    float       _cos_yaw;           // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
+    float       _sin_yaw;
+    float       _cos_roll;
+
+    // output from controller
+    int32_t     _desired_roll;
+    int32_t     _desired_pitch;
+    int32_t     _desired_altitude;
+
+    Vector3f    _target;   		        // loiter's target location in cm from home
+    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    _wpinav_target;         // the intermediate target location in cm from home
+    Vector3f    _pos_delta;             // position difference between origin and destination
+    float       _track_length;          // distance in cm between origin and destination
+    float       _track_desired;         // our desired distance along the track in cm
+};
+#endif	// AC_WPNAV_H

libraries/AC_WPNav/examples/AC_WPNav_test/AC_WPNav_test.pde

@@ -0,0 +1,69 @@
+/*
+ *       Example of AC_WPNav library .
+ *       DIYDrones.com
+ */
+
+#include <AP_Common.h>
+#include <AP_Progmem.h>
+#include <AP_Math.h>            // ArduPilot Mega Vector/Matrix math Library
+#include <AP_Param.h>
+#include <AP_HAL.h>
+#include <AP_HAL_AVR.h>
+
+#include <AP_GPS.h>             // ArduPilot GPS library
+#include <AP_ADC.h>             // ArduPilot Mega Analog to Digital Converter Library
+#include <AP_Baro.h>            // ArduPilot Mega Barometer Library
+#include <Filter.h>
+#include <AP_Compass.h>         // ArduPilot Mega Magnetometer Library
+#include <AP_Declination.h>
+#include <AP_InertialSensor.h>  // ArduPilot Mega Inertial Sensor (accel & gyro) Library
+#include <AP_AHRS.h>
+#include <AP_Airspeed.h>
+#include <AC_PID.h>             // PID library
+#include <APM_PI.h>             // PID library
+#include <AP_Buffer.h>          // ArduPilot general purpose FIFO buffer
+#include <AP_InertialNav.h>     // Inertial Navigation library
+#include <AC_WPNav.h>           // Waypoint Navigation library
+
+const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
+
+// INS and Baro declaration
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
+
+#define A_LED_PIN 27
+#define C_LED_PIN 25
+AP_InertialSensor_MPU6000 ins;
+AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
+
+#else
+
+#define A_LED_PIN 37
+#define C_LED_PIN 35
+AP_ADC_ADS7844 adc;
+AP_InertialSensor_Oilpan ins(&adc);
+AP_Baro_BMP085 baro;
+#endif
+
+// GPS declaration
+GPS *gps;
+AP_GPS_Auto auto_gps(&gps);
+
+AP_Compass_HMC5843 compass;
+AP_AHRS_DCM ahrs(&ins, gps);
+
+// Inertial Nav declaration
+AP_InertialNav inertialnav(&ahrs, &ins, &baro, &gps);
+
+void setup()
+{
+    hal.console->println("WPNav library test");
+}
+
+void loop()
+{
+    // call update function
+    hal.console->printf_P(PSTR("hello"));
+    hal.scheduler->delay(1);
+}
+
+AP_HAL_MAIN();

libraries/AC_WPNav/examples/AC_WPNav_test/Makefile

@@ -0,0 +1 @@
+include ../../../../mk/apm.mk

libraries/AC_WPNav/keywords.txt

@@ -0,0 +1,19 @@
+Compass	KEYWORD1
+AP_Compass	KEYWORD1
+APM_Compass	KEYWORD1
+init	KEYWORD2
+read	KEYWORD2
+calculate	KEYWORD2
+set_orientation	KEYWORD2
+set_offsets	KEYWORD2
+set_declination	KEYWORD2
+heading	KEYWORD2
+heading_x	KEYWORD2
+heading_y	KEYWORD2
+mag_x	KEYWORD2
+mag_y	KEYWORD2
+mag_z	KEYWORD2
+last_update	KEYWORD2
+
+
+