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

AC_WPNav: Use target yaw instead of current yaw for close waypoints

This commit is contained in:
Jonathan Challinger 2014-10-29 21:58:28 -07:00 committed by Randy Mackay
parent 1e4ec5f6a2
commit 369839c7ca
2 changed files with 7 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -86,10 +86,11 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] PROGMEM = {
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control) :
AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
_inav(inav),
_ahrs(ahrs),
_pos_control(pos_control),
_attitude_control(attitude_control),
_loiter_last_update(0),
_loiter_step(0),
_pilot_accel_fwd_cms(0),
@ -395,7 +396,7 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto
_yaw = get_bearing_cd(_origin, _destination);
} else {
// set target yaw to current heading. Alternatively we could pull this from the attitude controller if we had access to it
_yaw = _ahrs.yaw_sensor;
_yaw = _attitude_control.angle_ef_targets().z;
}
// initialise intermediate point to the origin
@ -785,7 +786,7 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
}
// initialise yaw heading to current heading
_yaw = _ahrs.yaw_sensor;
_yaw = _attitude_control.angle_ef_targets().z;
// store origin and destination locations
_origin = origin;

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -7,6 +7,7 @@
#include <AP_Math.h>
#include <AP_InertialNav.h> // Inertial Navigation library
#include <AC_PosControl.h> // Position control library
#include <AC_AttitudeControl.h> // Attitude control library
// loiter maximum velocities and accelerations
#define WPNAV_ACCELERATION 100.0f // defines the default velocity vs distant curve. maximum acceleration in cm/s/s that position controller asks for from acceleration controller
@ -56,7 +57,7 @@ public:
};
/// Constructor
AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control);
AC_WPNav(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control);
///
/// loiter controller
@ -268,6 +269,7 @@ protected:
const AP_InertialNav& _inav;
const AP_AHRS& _ahrs;
AC_PosControl& _pos_control;
const AC_AttitudeControl& _attitude_control;
// parameters
AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter