Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Circle: use trig values from ahrs

Browse Source
This commit is contained in:
Randy Mackay 2014-02-09 12:34:50 +09:00 committed by Andrew Tridgell
parent 64cfcb2308
commit 598a1b1f43
2 changed files with 3 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -35,9 +35,7 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS& ahrs, AC_PosCont
_ahrs(ahrs), _ahrs(ahrs),
_pos_control(pos_control), _pos_control(pos_control),
_last_update(0), _last_update(0),
_angle(0), _angle(0)
_cos_yaw(1.0),
_sin_yaw(0.0)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -66,8 +64,8 @@ void AC_Circle::init_center()
_pos_control.get_stopping_point_z(stopping_point); _pos_control.get_stopping_point_z(stopping_point);
// set circle center to circle_radius ahead of stopping point // set circle center to circle_radius ahead of stopping point
_center.x = stopping_point.x + _radius * _cos_yaw; _center.x = stopping_point.x + _radius * _ahrs.cos_yaw();
_center.y = stopping_point.y + _radius * _sin_yaw; _center.y = stopping_point.y + _radius * _ahrs.sin_yaw();
_center.z = stopping_point.z; _center.z = stopping_point.z;
// update pos_control target to stopping point // update pos_control target to stopping point

10
libraries/AC_WPNav/AC_Circle.h
View File

@ -47,12 +47,6 @@ public:
int32_t get_pitch() const { return _pos_control.get_pitch(); }; int32_t get_pitch() const { return _pos_control.get_pitch(); };
int32_t get_yaw() const { return _yaw; }; int32_t get_yaw() const { return _yaw; };
/// 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) {
_cos_yaw = cos_yaw;
_sin_yaw = sin_yaw;
}
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private:
@ -86,9 +80,5 @@ private:
float _angular_vel; // angular velocity in radians/sec float _angular_vel; // angular velocity in radians/sec
float _angular_vel_max; // maximum velocity in radians/sec float _angular_vel_max; // maximum velocity in radians/sec
float _angular_accel; // angular acceleration in radians/sec/sec float _angular_accel; // angular acceleration in radians/sec/sec
// helper variables
float _cos_yaw; // short-cut to save on calcs required to convert roll-pitch frame to lat-lon frame
float _sin_yaw; // To-Do: move these to ahrs or attitude control class to save on memory
}; };
#endif // AC_CIRCLE_H #endif // AC_CIRCLE_H