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

AC_WPNav: add get_spline_yaw

This commit is contained in:
Randy Mackay 2014-03-23 20:16:02 +09:00
parent 2ce827d2ae
commit b42b12f7be
2 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -86,7 +86,8 @@ AC_WPNav::AC_WPNav(const AP_InertialNav* inav, const AP_AHRS* ahrs, AC_PosContro
_track_leash_length(0.0), _track_leash_length(0.0),
_spline_time(0.0), _spline_time(0.0),
_spline_vel_scaler(0.0), _spline_vel_scaler(0.0),
_spline_slow_down_dist(0.0) _spline_slow_down_dist(0.0),
_spline_yaw(0.0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -593,6 +594,9 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V
update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel); update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
} }
// initialise yaw heading to current heading
_spline_yaw = _ahrs->yaw_sensor;
// To-Do: handle case where this is a straight segment? // To-Do: handle case where this is a straight segment?
// if this is a straight segment, origin velocity is distance vector from origin to destination // if this is a straight segment, origin velocity is distance vector from origin to destination
// To-Do: this handles case of a fast waypoint? // To-Do: this handles case of a fast waypoint?
@ -696,6 +700,9 @@ void AC_WPNav::advance_spline_target_along_track(float dt)
// update target position // update target position
_pos_control.set_pos_target(target_pos); _pos_control.set_pos_target(target_pos);
// update the yaw
_spline_yaw = RadiansToCentiDegrees(atan2(target_vel.y,target_vel.x));
// advance spline time to next step // advance spline time to next step
_spline_time += spline_time_scale*0.1f; _spline_time += spline_time_scale*0.1f;

4
libraries/AC_WPNav/AC_WPNav.h
View File

@ -146,6 +146,9 @@ public:
// straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination // straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination
// spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination // spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination
// get_spline_yaw - returns target yaw in centi-degrees
float get_spline_yaw() { return _spline_yaw; }
/// set_spline_destination waypoint using position vector (distance from home in cm) /// set_spline_destination waypoint using position vector (distance from home in cm)
/// stopped_at_start should be set to true if vehicle is stopped at the origin /// stopped_at_start should be set to true if vehicle is stopped at the origin
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type /// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
@ -258,5 +261,6 @@ protected:
float _spline_vel_scaler; // float _spline_vel_scaler; //
float _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination float _spline_slow_down_dist; // vehicle should begin to slow down once it is within this distance from the destination
// To-Do: this should be used for straight segments as well // To-Do: this should be used for straight segments as well
float _spline_yaw; // heading according to yaw
}; };
#endif // AC_WPNAV_H #endif // AC_WPNAV_H