You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
50 lines
2.0 KiB
50 lines
2.0 KiB
#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 |
|
#include <AP_GPS.h> // ArduPilot GPS Library |
|
#include "Waypoints.h" // ArduPilot Waypoints Library |
|
#include "WProgram.h" |
|
|
|
#define T7 10000000 |
|
|
|
class Navigation { |
|
public: |
|
Navigation(AP_GPS *withGPS); |
|
|
|
void update_gps(void); // called 50 Hz |
|
void set_home(Waypoints::WP loc); |
|
void set_next_wp(Waypoints::WP loc); |
|
void hold_course(int8_t b); // 1 = hold a current course, 0 disables course hold |
|
long get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2); |
|
long get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2); |
|
|
|
long bearing; // deg * 100 : 0 to 360 current desired bearing to navigate |
|
long distance; // meters : distance plane to next waypoint |
|
long altitude_above_home; // meters * 100 relative to home position |
|
long total_distance; // meters : distance between waypoints |
|
long bearing_error; // deg * 100 : 18000 to -18000 |
|
long altitude_error; // deg * 100 : 18000 to -18000 |
|
|
|
int16_t loiter_sum; |
|
Waypoints::WP home, location, prev_wp, next_wp; |
|
|
|
private: |
|
void calc_long_scaling(int32_t lat); |
|
void calc_bearing_error(void); |
|
void calc_altitude_error(void); |
|
void calc_distance_error(void); |
|
void update_crosstrack(void); |
|
void reset_crosstrack(void); |
|
int32_t wrap_360(int32_t error); // utility |
|
|
|
int16_t _old_bearing; // used to track delta on the bearing |
|
AP_GPS *_gps; |
|
long _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target |
|
float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline |
|
long _hold_course; // deg * 100 dir of plane |
|
long _target_altitude; // used for |
|
long _offset_altitude; // used for |
|
float _altitude_estimate; |
|
float _scaleLongUp; // used to reverse longtitude scaling |
|
float _scaleLongDown; // used to reverse longtitude scaling |
|
int16_t _loiter_delta; |
|
};
|
|
|