00001 00002 #ifndef AP_NAVIGATION_h 00003 #define AP_NAVIGATION_h 00004 00005 #define XTRACK_GAIN 10 // Amount to compensate for crosstrack (degrees/100 per meter) 00006 #define XTRACK_ENTRY_ANGLE 3000 // Max angle used to correct for track following degrees*100 00007 #include <GPS.h> // ArduPilot GPS Library 00008 #include "Waypoints.h" // ArduPilot Waypoints Library 00009 #include "WProgram.h" 00010 00011 #define T7 10000000 00012 00013 class Navigation { 00014 public: 00015 Navigation(GPS *withGPS, Waypoints *withWP); 00016 00017 void update_gps(void); // called 50 Hz 00018 void set_home(Waypoints::WP loc); 00019 void set_next_wp(Waypoints::WP loc); 00020 void load_first_wp(void); 00021 void load_wp_with_index(uint8_t i); 00022 void load_home(void); 00023 void return_to_home_with_alt(uint32_t alt); 00024 00025 void reload_wp(void); 00026 void load_wp_index(uint8_t i); 00027 void hold_location(); 00028 void set_wp(Waypoints::WP loc); 00029 00030 void set_hold_course(int16_t b); // -1 deisables, 0-36000 enables 00031 int16_t get_hold_course(void); 00032 00033 int32_t get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2); 00034 int32_t get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2); 00035 void set_bearing_error(int32_t error); 00036 00037 void set_loiter_vector(int16_t v); 00038 void update_crosstrack(void); 00039 00040 int32_t wrap_180(int32_t error); // utility 00041 int32_t wrap_360(int32_t angle); // utility 00042 00043 int32_t bearing; // deg * 100 : 0 to 360 current desired bearing to navigate 00044 int32_t distance; // meters : distance plane to next waypoint 00045 int32_t altitude_above_home; // meters * 100 relative to home position 00046 int32_t total_distance; // meters : distance between waypoints 00047 int32_t bearing_error; // deg * 100 : 18000 to -18000 00048 int32_t altitude_error; // deg * 100 : 18000 to -18000 00049 00050 int16_t loiter_sum; 00051 Waypoints::WP home, location, prev_wp, next_wp; 00052 00053 private: 00054 void calc_int32_t_scaling(int32_t lat); 00055 void calc_bearing_error(void); 00056 void calc_altitude_error(void); 00057 void calc_distance_error(void); 00058 void calc_long_scaling(int32_t lat); 00059 void reset_crosstrack(void); 00060 00061 int16_t _old_bearing; // used to track delta on the bearing 00062 GPS *_gps; 00063 Waypoints *_wp; 00064 int32_t _crosstrack_bearing; // deg * 100 : 0 to 360 desired angle of plane to target 00065 float _crosstrack_error; // deg * 100 : 18000 to -18000 meters we are off trackline 00066 int16_t _hold_course; // deg * 100 dir of plane 00067 int32_t _target_altitude; // used for 00068 int32_t _offset_altitude; // used for 00069 float _altitude_estimate; 00070 float _scaleLongUp; // used to reverse int32_ttitude scaling 00071 float _scaleLongDown; // used to reverse int32_ttitude scaling 00072 int16_t _loiter_delta; 00073 }; 00074 00075 00076 #endif // AP_NAVIGATION_h
1.7.1