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

Navigation.cpp

Go to the documentation of this file.
00001 #include "Navigation.h"
00002 
00003 Navigation::Navigation(GPS *withGPS, Waypoints *withWP) : 
00004                 _gps(withGPS),
00005                 _wp(withWP),
00006                 _hold_course(-1)
00007 {
00008 }
00009 
00010 void
00011 Navigation::update_gps()
00012 {
00013         location.alt            = _gps->altitude;
00014         location.lng            = _gps->longitude;
00015         location.lat            = _gps->latitude;
00016         
00017         // target_bearing is where we should be heading 
00018         bearing                         = get_bearing(&location, &next_wp);
00019 
00020         // waypoint distance from plane
00021         distance                        = get_distance(&location, &next_wp);
00022         
00023         calc_bearing_error();
00024         calc_altitude_error();
00025         altitude_above_home = location.alt - home.alt;
00026                         
00027         // check if we have missed the WP
00028         _loiter_delta           = (bearing - _old_bearing) / 100;
00029         
00030         // reset the old value
00031         _old_bearing            = bearing;
00032         
00033         // wrap values
00034         if (_loiter_delta > 170) _loiter_delta -= 360;
00035         if (_loiter_delta < -170) _loiter_delta += 360;
00036         loiter_sum += abs(_loiter_delta);
00037 
00038         if (distance <= 0){
00039                 distance = -1;
00040                 Serial.print("MSG wp error  ");
00041                 Serial.println(distance, DEC);
00042         }
00043 }
00044 
00045 void
00046 Navigation::load_first_wp(void)
00047 {
00048         set_next_wp(_wp->get_waypoint_with_index(1));
00049 }
00050 
00051 void
00052 Navigation::load_home(void)
00053 {
00054         home = _wp->get_waypoint_with_index(0);
00055 }
00056 
00057 void
00058 Navigation::return_to_home_with_alt(uint32_t alt)
00059 {
00060         Waypoints::WP loc = _wp->get_waypoint_with_index(0);
00061         loc.alt += alt;
00062         set_next_wp(loc);
00063 }
00064 
00065 void
00066 Navigation::reload_wp(void)
00067 {
00068         set_next_wp(_wp->get_current_waypoint());
00069 }
00070 
00071 void
00072 Navigation::load_wp_index(uint8_t i)
00073 {
00074         _wp->set_index(i);
00075         set_next_wp(_wp->get_current_waypoint());
00076 }
00077 
00078 void
00079 Navigation::hold_location()
00080 {
00081   //    set_next_wp()   XXX needs to be implemented
00082 }
00083 
00084 void
00085 Navigation::set_home(Waypoints::WP loc)
00086 {
00087         _wp->set_waypoint_with_index(loc, 0);
00088         home            = loc;
00089         //location      = home;
00090 }
00091 
00092 void
00093 Navigation::set_next_wp(Waypoints::WP loc)
00094 {
00095         prev_wp = next_wp;
00096         next_wp = loc;
00097         
00098         if(_scaleLongDown == 0)
00099         calc_long_scaling(loc.lat);
00100 
00101         total_distance          = get_distance(&location, &next_wp);
00102         distance                        = total_distance;
00103         
00104         bearing                         = get_bearing(&location, &next_wp);
00105         _old_bearing            = bearing;
00106 
00107         // clear loitering code
00108         _loiter_delta           = 0;
00109         loiter_sum                      = 0;    
00110 
00111         // set a new crosstrack bearing
00112         // ----------------------------
00113         reset_crosstrack();
00114 }
00115 
00116 void 
00117 Navigation::calc_long_scaling(int32_t lat)
00118 {
00119         // this is used to offset the shrinking longitude as we go towards the poles    
00120         float rads                      = (abs(lat) / T7) * 0.0174532925;
00121         _scaleLongDown          = cos(rads);
00122         _scaleLongUp            = 1.0f / cos(rads);
00123 }
00124 
00125 void
00126 Navigation::set_hold_course(int16_t b)
00127 {
00128         if(b)
00129                 _hold_course = bearing;
00130         else
00131                 _hold_course = -1;
00132 }
00133 
00134 int16_t
00135 Navigation::get_hold_course(void)
00136 {
00137         return _hold_course;
00138 }
00139 
00140 void
00141 Navigation::calc_distance_error()
00142 {       
00143         //distance_estimate     += (float)_gps->ground_speed * .0002 * cos(radians(bearing_error * .01));
00144         //distance_estimate     -= DST_EST_GAIN * (float)(distance_estimate - GPS_distance);
00145         //distance              = max(distance_estimate,10);
00146 }
00147 
00148 void 
00149 Navigation::calc_bearing_error(void)
00150 {
00151         if(_hold_course == -1){
00152                 bearing_error = wrap_180(bearing - _gps->ground_course);
00153         }else{
00154                 bearing_error = _hold_course;
00155         }
00156 }
00157 
00158 int32_t
00159 Navigation::wrap_180(int32_t error)
00160 {
00161         if (error > 18000)      error -= 36000;
00162         if (error < -18000)     error += 36000;
00163         return error;
00164 }
00165 
00166 int32_t
00167 Navigation::wrap_360(int32_t angle)
00168 {
00169         if (angle > 36000)      angle -= 36000;
00170         if (angle < 0)          angle += 36000;
00171         return angle;
00172 }
00173 
00174 void
00175 Navigation::set_bearing_error(int32_t error)
00176 {
00177         bearing_error = wrap_180(error);
00178 }
00179 
00180 
00181 /*****************************************
00182  * Altitude error with Airspeed correction
00183  *****************************************/
00184 void
00185 Navigation::calc_altitude_error(void) 
00186 {
00187         // limit climb rates
00188         _target_altitude = next_wp.alt - ((float)((distance -20) * _offset_altitude) / (float)(total_distance - 20));
00189         if(prev_wp.alt > next_wp.alt){
00190                 _target_altitude = constrain(_target_altitude, next_wp.alt, prev_wp.alt);
00191         }else{
00192                 _target_altitude = constrain(_target_altitude, prev_wp.alt, next_wp.alt);
00193         }
00194         altitude_error  = _target_altitude - location.alt;
00195 }
00196 
00197 void
00198 Navigation::set_loiter_vector(int16_t v)
00199 {
00200   //    _vector = constrain(v, -18000, 18000);  XXX needs to be implemented
00201 }
00202 
00203 void
00204 Navigation::update_crosstrack(void)
00205 {
00206         // Crosstrack Error
00207         // ----------------
00208         if (abs(bearing - _crosstrack_bearing) < 4500) {         // If we are too far off or too close we don't do track following
00209                 _crosstrack_error = sin(radians((bearing - _crosstrack_bearing) / 100)) * distance;      // Meters we are off track line
00210                 bearing += constrain(_crosstrack_error * XTRACK_GAIN, -XTRACK_ENTRY_ANGLE, XTRACK_ENTRY_ANGLE);
00211         }
00212 }
00213 
00214 void
00215 Navigation::reset_crosstrack(void)
00216 {
00217         _crosstrack_bearing     = get_bearing(&location, &next_wp);     // Used for track following
00218 }
00219 
00220 long
00221 Navigation::get_distance(Waypoints::WP *loc1, Waypoints::WP *loc2)
00222 {
00223         if(loc1->lat == 0 || loc1->lng == 0) 
00224                 return -1;
00225         if(loc2->lat == 0 || loc2->lng == 0) 
00226                 return -1;
00227         if(_scaleLongDown == 0)
00228                 calc_long_scaling(loc2->lat);
00229         float dlat      = (float)(loc2->lat - loc1->lat);
00230         float dlong = ((float)(loc2->lng - loc1->lng)) * _scaleLongDown;
00231         return sqrt(sq(dlat) + sq(dlong)) * .01113195;
00232 }
00233 
00234 long 
00235 Navigation::get_bearing(Waypoints::WP *loc1, Waypoints::WP *loc2)
00236 {
00237         if(loc1->lat == 0 || loc1->lng == 0) 
00238                 return -1;
00239         if(loc2->lat == 0 || loc2->lng == 0) 
00240                 return -1;
00241         if(_scaleLongDown == 0)
00242                 calc_long_scaling(loc2->lat);
00243         long off_x                      = loc2->lng - loc1->lng;
00244         long off_y                      = (loc2->lat - loc1->lat) * _scaleLongUp;
00245         long bearing            =       9000 + atan2(-off_y, off_x) * 5729.57795;
00246         if (bearing < 0) 
00247                 bearing                 += 36000;
00248         return bearing;
00249 }

Generated for ArduPilot Libraries by doxygen