Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 17:38:32 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: bug fix to dt calculation

Browse Source
This commit is contained in:
Randy Mackay 2013-03-21 18:28:10 +09:00
parent 49828eba7d
commit 0d70ba1030
2 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -21,12 +21,12 @@ 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(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_lat_rate, AC_PID* pid_lon_rate) : AC_WPNav::AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon) :
_inav(inav), _inav(inav),
_pid_pos_lat(pid_pos_lat), _pid_pos_lat(pid_pos_lat),
_pid_pos_lon(pid_pos_lon), _pid_pos_lon(pid_pos_lon),
_pid_rate_lat(pid_lat_rate), _pid_rate_lat(pid_rate_lat),
_pid_rate_lon(pid_lon_rate) _pid_rate_lon(pid_rate_lon)
{ {
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
@ -73,7 +73,7 @@ int32_t AC_WPNav::get_bearing_to_target()
void AC_WPNav::update_loiter() void AC_WPNav::update_loiter()
{ {
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
float dt = now - _last_update; float dt = (now - _last_update) / 1000.0f;
_last_update = now; _last_update = now;
// catch if we've just been started // catch if we've just been started
@ -186,7 +186,7 @@ int32_t AC_WPNav::get_bearing_to_destination()
void AC_WPNav::update_wpnav() void AC_WPNav::update_wpnav()
{ {
uint32_t now = hal.scheduler->millis(); uint32_t now = hal.scheduler->millis();
float dt = now - _last_update; float dt = (now - _last_update) / 1000.0f;
_last_update = now; _last_update = now;
// catch if we've just been started // catch if we've just been started

2
libraries/AC_WPNav/AC_WPNav.h
View File

@ -25,7 +25,7 @@ class AC_WPNav
public: public:
/// Constructor /// Constructor
AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_lat_rate, AC_PID* pid_lon_rate); AC_WPNav(AP_InertialNav* inav, APM_PI* pid_pos_lat, APM_PI* pid_pos_lon, AC_PID* pid_rate_lat, AC_PID* pid_rate_lon);
/// ///
/// simple loiter controller /// simple loiter controller