• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

Navigation.h

Go to the documentation of this file.
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

Generated for ArduPilot Libraries by doxygen