Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
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 // Note that the Vector/Matrix constructors already implicitly zero
// their values. // 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), _inav(inav),
_ahrs(ahrs), _ahrs(ahrs),
_pos_control(pos_control), _pos_control(pos_control),
_attitude_control(attitude_control),
_loiter_last_update(0), _loiter_last_update(0),
_loiter_step(0), _loiter_step(0),
_pilot_accel_fwd_cms(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); _yaw = get_bearing_cd(_origin, _destination);
} else { } else {
// set target yaw to current heading. Alternatively we could pull this from the attitude controller if we had access to it // 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 // 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 // initialise yaw heading to current heading
_yaw = _ahrs.yaw_sensor; _yaw = _attitude_control.angle_ef_targets().z;
// store origin and destination locations // store origin and destination locations
_origin = origin; _origin = origin;

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -7,6 +7,7 @@
#include <AP_Math.h> #include <AP_Math.h>
#include <AP_InertialNav.h> // Inertial Navigation library #include <AP_InertialNav.h> // Inertial Navigation library
#include <AC_PosControl.h> // Position control library #include <AC_PosControl.h> // Position control library
#include <AC_AttitudeControl.h> // Attitude control library
// loiter maximum velocities and accelerations // 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 #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 /// 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 /// loiter controller
@ -268,6 +269,7 @@ protected:
const AP_InertialNav& _inav; const AP_InertialNav& _inav;
const AP_AHRS& _ahrs; const AP_AHRS& _ahrs;
AC_PosControl& _pos_control; AC_PosControl& _pos_control;
const AC_AttitudeControl& _attitude_control;
// parameters // parameters
AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter AP_Float _loiter_speed_cms; // maximum horizontal speed in cm/s while in loiter