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

still unstable

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@528 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-09-18 19:18:55 +00:00
parent d342f7582d
commit 6fa92cc299
5 changed files with 21 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Navigation/Navigation.cpp
View File

@ -164,11 +164,11 @@ Navigation::wrap_180(int32_t error)
} }
int32_t int32_t
Navigation::wrap_360(int32_t error) Navigation::wrap_360(int32_t angle)
{ {
if (error > 36000) error -= 36000; if (angle > 36000) angle -= 36000;
if (error < 0) error += 36000; if (angle < 0) angle += 36000;
return error; return angle;
} }
void void

7
libraries/AP_Navigation/Navigation.h
View File

@ -5,7 +5,7 @@
#define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter) #define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter)
#define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100 #define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100
#include <GPS.h> // ArduPilot GPS Library #include <GPS.h> // ArduPilot GPS Library
#include "Waypoints.h" // ArduPilot Waypoints Library #include "Waypoints.h" // ArduPilot Waypoints Library
#include "WProgram.h" #include "WProgram.h"
#define T7 10000000 #define T7 10000000
@ -36,6 +36,9 @@ public:
void set_loiter_vector(int16_t v); void set_loiter_vector(int16_t v);
void update_crosstrack(void); void update_crosstrack(void);
int32_t wrap_180(int32_t error); // utility
int32_t wrap_360(int32_t angle); // utility
int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate
int32_t distance; // meters : distance plane to next waypoint int32_t distance; // meters : distance plane to next waypoint
@ -54,8 +57,6 @@ private:
void calc_distance_error(void); void calc_distance_error(void);
void calc_long_scaling(int32_t lat); void calc_long_scaling(int32_t lat);
void reset_crosstrack(void); void reset_crosstrack(void);
int32_t wrap_180(int32_t error); // utility
int32_t wrap_360(int32_t error); // utility
int16_t _old_bearing; // used to track delta on the bearing int16_t _old_bearing; // used to track delta on the bearing
GPS *_gps; GPS *_gps;

2
libraries/AP_Navigation/examples/Navigation/Navigation.pde
View File

@ -11,7 +11,7 @@ This test assumes you are at the LOWl demo Airport
AP_GPS_IMU gps; AP_GPS_IMU gps;
Navigation nav((GPS *) & gps); Navigation nav((GPS *) &gps);
AP_RC rc; AP_RC rc;
#define CH_ROLL 0 #define CH_ROLL 0

17
libraries/Waypoints/Waypoints.cpp
View File

@ -70,15 +70,6 @@ Waypoints::get_waypoint_with_index(uint8_t i)
} }
Waypoints::WP
Waypoints::get_next_waypoint(void)
{
_index++;
if (_index >= _total)
_index == 0;
return get_waypoint_with_index(_index);
}
Waypoints::WP Waypoints::WP
Waypoints::get_current_waypoint(void) Waypoints::get_current_waypoint(void)
{ {
@ -91,6 +82,14 @@ Waypoints::get_index(void)
return _index; return _index;
} }
void
Waypoints::next_index(void)
{
_index++;
if (_index >= _total)
_index == 0;
}
void void
Waypoints::set_index(uint8_t i) Waypoints::set_index(uint8_t i)
{ {

8
libraries/Waypoints/Waypoints.h
View File

@ -18,15 +18,15 @@ class Waypoints
int32_t lng; // Longitude * 10**7 int32_t lng; // Longitude * 10**7
}; };
WP get_waypoint_with_index(uint8_t i); WP get_waypoint_with_index(uint8_t i);
WP get_current_waypoint(void); WP get_current_waypoint(void);
WP get_next_waypoint(void);
void set_waypoint_with_index(Waypoints::WP wp, uint8_t i); void set_waypoint_with_index(Waypoints::WP wp, uint8_t i);
void set_start_byte(uint16_t start_byte); void set_start_byte(uint16_t start_byte);
void set_wp_size(uint8_t wp_size); void set_wp_size(uint8_t wp_size);
void next_index(void);
uint8_t get_index(void); uint8_t get_index(void);
void set_index(uint8_t i); void set_index(uint8_t i);