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

AC_WPNav: Use DEGX100 define instead of hardcoded value (NFC)

Browse Source
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-12-03 20:45:47 +01:00 committed by Randy Mackay
parent ec1790dad9
commit 07c8d208bc
3 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -144,7 +144,7 @@ void AC_Circle::update()
_pos_control.set_xy_target(target.x, target.y); _pos_control.set_xy_target(target.x, target.y);
// heading is 180 deg from vehicles target position around circle // heading is 180 deg from vehicles target position around circle
_yaw = wrap_PI(_angle-M_PI) * AC_CIRCLE_DEGX100; _yaw = wrap_PI(_angle-M_PI) * DEGX100;
}else{ }else{
// set target position to center // set target position to center
Vector3f target; Vector3f target;
@ -156,7 +156,7 @@ void AC_Circle::update()
_pos_control.set_xy_target(target.x, target.y); _pos_control.set_xy_target(target.x, target.y);
// heading is same as _angle but converted to centi-degrees // heading is same as _angle but converted to centi-degrees
_yaw = _angle * AC_CIRCLE_DEGX100; _yaw = _angle * DEGX100;
} }
// update position controller // update position controller

2
libraries/AC_WPNav/AC_Circle.h
View File

@ -11,8 +11,6 @@
#define AC_CIRCLE_RATE_DEFAULT 20.0f // turn rate in deg/sec. Positive to turn clockwise, negative for counter clockwise #define AC_CIRCLE_RATE_DEFAULT 20.0f // turn rate in deg/sec. Positive to turn clockwise, negative for counter clockwise
#define AC_CIRCLE_ANGULAR_ACCEL_MIN 2.0f // angular acceleration should never be less than 2deg/sec #define AC_CIRCLE_ANGULAR_ACCEL_MIN 2.0f // angular acceleration should never be less than 2deg/sec
#define AC_CIRCLE_DEGX100 5729.57795f // constant to convert from radians to centi-degrees
class AC_Circle class AC_Circle
{ {
public: public:

3
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -1,4 +1,5 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <AP_Math/definitions.h>
#include "AC_WPNav.h" #include "AC_WPNav.h"
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
@ -1276,7 +1277,7 @@ bool AC_WPNav::get_vector_NEU(const Location_Class &loc, Vector3f &vec, bool &te
// To-Do: move this to math library // To-Do: move this to math library
float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const float AC_WPNav::get_bearing_cd(const Vector3f &origin, const Vector3f &destination) const
{ {
float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * 5729.57795f; float bearing = 9000 + atan2f(-(destination.x-origin.x), destination.y-origin.y) * DEGX100;
if (bearing < 0) { if (bearing < 0) {
bearing += 36000; bearing += 36000;
} }