2010-12-26 20:59:17 -04:00
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
< html xmlns = "http://www.w3.org/1999/xhtml" >
< head >
< meta http-equiv = "Content-Type" content = "text/xhtml;charset=UTF-8" / >
< title > ArduPilot Libraries: /home/jgoppert/Projects/ap/libraries/AP_Navigation/Navigation.cpp Source File< / title >
< link href = "tabs.css" rel = "stylesheet" type = "text/css" / >
< link href = "doxygen.css" rel = "stylesheet" type = "text/css" / >
< / head >
< body >
<!-- Generated by Doxygen 1.7.1 -->
< div class = "navigation" id = "top" >
< div class = "tabs" >
< ul class = "tablist" >
< li > < a href = "main.html" > < span > Main Page< / span > < / a > < / li >
< li > < a href = "namespaces.html" > < span > Namespaces< / span > < / a > < / li >
< li > < a href = "annotated.html" > < span > Classes< / span > < / a > < / li >
< li class = "current" > < a href = "files.html" > < span > Files< / span > < / a > < / li >
< / ul >
< / div >
< div class = "tabs2" >
< ul class = "tablist" >
< li > < a href = "files.html" > < span > File List< / span > < / a > < / li >
< li > < a href = "globals.html" > < span > File Members< / span > < / a > < / li >
< / ul >
< / div >
< div class = "header" >
< div class = "headertitle" >
< h1 > /home/jgoppert/Projects/ap/libraries/AP_Navigation/Navigation.cpp< / h1 > < / div >
< / div >
< div class = "contents" >
< a href = "_navigation_8cpp.html" > Go to the documentation of this file.< / a > < div class = "fragment" > < pre class = "fragment" > < a name = "l00001" > < / a > 00001 < span class = "preprocessor" > #include " < a class = "code" href = "_navigation_8h.html" > Navigation.h< / a > " < / span >
< a name = "l00002" > < / a > 00002
< a name = "l00003" > < / a > < a class = "code" href = "class_navigation.html#a128f871ef138c360fb2c73ffec72729f" > 00003< / a > < a class = "code" href = "class_navigation.html#a128f871ef138c360fb2c73ffec72729f" > Navigation::Navigation< / a > (< a class = "code" href = "class_g_p_s.html" title = "Abstract base class for GPS receiver drivers." > GPS< / a > *withGPS, < a class = "code" href = "class_waypoints.html" > Waypoints< / a > *withWP) :
< a name = "l00004" > < / a > 00004 _gps(withGPS),
< a name = "l00005" > < / a > 00005 _wp(withWP),
< a name = "l00006" > < / a > 00006 _hold_course(-1)
< a name = "l00007" > < / a > 00007 {
< a name = "l00008" > < / a > 00008 }
< a name = "l00009" > < / a > 00009
< a name = "l00010" > < / a > 00010 < span class = "keywordtype" > void< / span >
< a name = "l00011" > < / a > < a class = "code" href = "class_navigation.html#a9d2b3544acc5bf511da32c3e6888acd5" > 00011< / a > < a class = "code" href = "class_navigation.html#a9d2b3544acc5bf511da32c3e6888acd5" > Navigation::update_gps< / a > ()
< a name = "l00012" > < / a > 00012 {
< a name = "l00013" > < / a > 00013 < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > = _gps-> < a class = "code" href = "class_g_p_s.html#aaf73c8b7f3a10e7c37432448ad6582b2" title = "altitude in cm" > altitude< / a > ;
< a name = "l00014" > < / a > 00014 < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > = _gps-> < a class = "code" href = "class_g_p_s.html#ae64cbfd073326bd5ec8a6d0a1bae2746" title = "longitude in degrees * 10,000,000" > longitude< / a > ;
< a name = "l00015" > < / a > 00015 < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > = _gps-> < a class = "code" href = "class_g_p_s.html#aa5df9c7bd452a9689054e55ec90dfef1" title = "latitude in degrees * 10,000,000" > latitude< / a > ;
< a name = "l00016" > < / a > 00016
< a name = "l00017" > < / a > 00017 < span class = "comment" > // target_bearing is where we should be heading < / span >
< a name = "l00018" > < / a > 00018 < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > = < a class = "code" href = "class_navigation.html#acf7075f5eedf8f47ae88b7ae04bccb39" > get_bearing< / a > (& < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > , & < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > );
< a name = "l00019" > < / a > 00019
< a name = "l00020" > < / a > 00020 < span class = "comment" > // waypoint distance from plane< / span >
< a name = "l00021" > < / a > 00021 < a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > = < a class = "code" href = "class_navigation.html#ac05da5065688d7ef70452bbc4b8f8556" > get_distance< / a > (& < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > , & < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > );
< a name = "l00022" > < / a > 00022
< a name = "l00023" > < / a > 00023 calc_bearing_error();
< a name = "l00024" > < / a > 00024 calc_altitude_error();
< a name = "l00025" > < / a > 00025 < a class = "code" href = "class_navigation.html#a5c655bff9637ea00a3dc3650cc1cf4f5" > altitude_above_home< / a > = < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > - < a class = "code" href = "class_navigation.html#acaa6eea8382dda9ba39e0dc3960d5d37" > home< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > ;
< a name = "l00026" > < / a > 00026
< a name = "l00027" > < / a > 00027 < span class = "comment" > // check if we have missed the WP< / span >
< a name = "l00028" > < / a > 00028 _loiter_delta = (< a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > - _old_bearing) / 100;
< a name = "l00029" > < / a > 00029
< a name = "l00030" > < / a > 00030 < span class = "comment" > // reset the old value< / span >
< a name = "l00031" > < / a > 00031 _old_bearing = < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > ;
< a name = "l00032" > < / a > 00032
< a name = "l00033" > < / a > 00033 < span class = "comment" > // wrap values< / span >
< a name = "l00034" > < / a > 00034 < span class = "keywordflow" > if< / span > (_loiter_delta > 170) _loiter_delta -= 360;
< a name = "l00035" > < / a > 00035 < span class = "keywordflow" > if< / span > (_loiter_delta < -170) _loiter_delta += 360;
< a name = "l00036" > < / a > 00036 < a class = "code" href = "class_navigation.html#a2e0bfad230d48a39926571d5d455f139" > loiter_sum< / a > += abs(_loiter_delta);
< a name = "l00037" > < / a > 00037
< a name = "l00038" > < / a > 00038 < span class = "keywordflow" > if< / span > (< a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > < = 0){
< a name = "l00039" > < / a > 00039 < a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > = -1;
< a name = "l00040" > < / a > 00040 < a class = "code" href = "_fast_serial_8h.html#a8e6f49b84135f0158fe250cb7338ddad" > Serial< / a > .print(< span class = "stringliteral" > " MSG wp error " < / span > );
< a name = "l00041" > < / a > 00041 < a class = "code" href = "_fast_serial_8h.html#a8e6f49b84135f0158fe250cb7338ddad" > Serial< / a > .println(< a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > , DEC);
< a name = "l00042" > < / a > 00042 }
< a name = "l00043" > < / a > 00043 }
< a name = "l00044" > < / a > 00044
< a name = "l00045" > < / a > 00045 < span class = "keywordtype" > void< / span >
< a name = "l00046" > < / a > < a class = "code" href = "class_navigation.html#a7d1a2aec8970bd7c4a6f26b4a31f69b1" > 00046< / a > < a class = "code" href = "class_navigation.html#a7d1a2aec8970bd7c4a6f26b4a31f69b1" > Navigation::load_first_wp< / a > (< span class = "keywordtype" > void< / span > )
< a name = "l00047" > < / a > 00047 {
< a name = "l00048" > < / a > 00048 < a class = "code" href = "class_navigation.html#a222cd205c6709681875cbb4efa2657f0" > set_next_wp< / a > (_wp-> < a class = "code" href = "class_waypoints.html#ab942a563846af24d306e3eb19dfb4a2a" > get_waypoint_with_index< / a > (1));
< a name = "l00049" > < / a > 00049 }
< a name = "l00050" > < / a > 00050
< a name = "l00051" > < / a > 00051 < span class = "keywordtype" > void< / span >
< a name = "l00052" > < / a > < a class = "code" href = "class_navigation.html#ac1eb3234566b231e44a2a5fbc6d25ca9" > 00052< / a > < a class = "code" href = "class_navigation.html#ac1eb3234566b231e44a2a5fbc6d25ca9" > Navigation::load_home< / a > (< span class = "keywordtype" > void< / span > )
< a name = "l00053" > < / a > 00053 {
< a name = "l00054" > < / a > 00054 < a class = "code" href = "class_navigation.html#acaa6eea8382dda9ba39e0dc3960d5d37" > home< / a > = _wp-> < a class = "code" href = "class_waypoints.html#ab942a563846af24d306e3eb19dfb4a2a" > get_waypoint_with_index< / a > (0);
< a name = "l00055" > < / a > 00055 }
< a name = "l00056" > < / a > 00056
< a name = "l00057" > < / a > 00057 < span class = "keywordtype" > void< / span >
< a name = "l00058" > < / a > < a class = "code" href = "class_navigation.html#a3cb86f630e37f5d41467a23a7a8cf831" > 00058< / a > < a class = "code" href = "class_navigation.html#a3cb86f630e37f5d41467a23a7a8cf831" > Navigation::return_to_home_with_alt< / a > (uint32_t alt)
< a name = "l00059" > < / a > 00059 {
< a name = "l00060" > < / a > 00060 < a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > loc = _wp-> < a class = "code" href = "class_waypoints.html#ab942a563846af24d306e3eb19dfb4a2a" > get_waypoint_with_index< / a > (0);
< a name = "l00061" > < / a > 00061 loc.< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > += alt;
< a name = "l00062" > < / a > 00062 < a class = "code" href = "class_navigation.html#a222cd205c6709681875cbb4efa2657f0" > set_next_wp< / a > (loc);
< a name = "l00063" > < / a > 00063 }
< a name = "l00064" > < / a > 00064
< a name = "l00065" > < / a > 00065 < span class = "keywordtype" > void< / span >
< a name = "l00066" > < / a > < a class = "code" href = "class_navigation.html#a99a4fd20cd5e7a0bf0fd8cfa5272e79f" > 00066< / a > < a class = "code" href = "class_navigation.html#a99a4fd20cd5e7a0bf0fd8cfa5272e79f" > Navigation::reload_wp< / a > (< span class = "keywordtype" > void< / span > )
< a name = "l00067" > < / a > 00067 {
< a name = "l00068" > < / a > 00068 < a class = "code" href = "class_navigation.html#a222cd205c6709681875cbb4efa2657f0" > set_next_wp< / a > (_wp-> < a class = "code" href = "class_waypoints.html#a0a69eac7c5eadd99d7dcd3df7bb12364" > get_current_waypoint< / a > ());
< a name = "l00069" > < / a > 00069 }
< a name = "l00070" > < / a > 00070
< a name = "l00071" > < / a > 00071 < span class = "keywordtype" > void< / span >
< a name = "l00072" > < / a > < a class = "code" href = "class_navigation.html#a360a3f5097cf11ba28f12df55031d7b0" > 00072< / a > < a class = "code" href = "class_navigation.html#a360a3f5097cf11ba28f12df55031d7b0" > Navigation::load_wp_index< / a > (uint8_t i)
< a name = "l00073" > < / a > 00073 {
< a name = "l00074" > < / a > 00074 _wp-> < a class = "code" href = "class_waypoints.html#a12ee61e5fdd8c7e5943d4beaee20eac0" > set_index< / a > (i);
< a name = "l00075" > < / a > 00075 < a class = "code" href = "class_navigation.html#a222cd205c6709681875cbb4efa2657f0" > set_next_wp< / a > (_wp-> < a class = "code" href = "class_waypoints.html#a0a69eac7c5eadd99d7dcd3df7bb12364" > get_current_waypoint< / a > ());
< a name = "l00076" > < / a > 00076 }
< a name = "l00077" > < / a > 00077
< a name = "l00078" > < / a > 00078 < span class = "keywordtype" > void< / span >
< a name = "l00079" > < / a > < a class = "code" href = "class_navigation.html#ae916ccef7d89b09a972ceda58d5ef9b6" > 00079< / a > < a class = "code" href = "class_navigation.html#ae916ccef7d89b09a972ceda58d5ef9b6" > Navigation::hold_location< / a > ()
< a name = "l00080" > < / a > 00080 {
< a name = "l00081" > < / a > 00081 < span class = "comment" > // set_next_wp() XXX needs to be implemented< / span >
< a name = "l00082" > < / a > 00082 }
< a name = "l00083" > < / a > 00083
< a name = "l00084" > < / a > 00084 < span class = "keywordtype" > void< / span >
< a name = "l00085" > < / a > < a class = "code" href = "class_navigation.html#a55010dc9ffe03ecba91bbfbb4e8f190c" > 00085< / a > < a class = "code" href = "class_navigation.html#a55010dc9ffe03ecba91bbfbb4e8f190c" > Navigation::set_home< / a > (< a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > loc)
< a name = "l00086" > < / a > 00086 {
< a name = "l00087" > < / a > 00087 _wp-> < a class = "code" href = "class_waypoints.html#a78f8f60817720baed33b8081e0ae5a82" > set_waypoint_with_index< / a > (loc, 0);
< a name = "l00088" > < / a > 00088 < a class = "code" href = "class_navigation.html#acaa6eea8382dda9ba39e0dc3960d5d37" > home< / a > = loc;
< a name = "l00089" > < / a > 00089 < span class = "comment" > //location = home;< / span >
< a name = "l00090" > < / a > 00090 }
< a name = "l00091" > < / a > 00091
< a name = "l00092" > < / a > 00092 < span class = "keywordtype" > void< / span >
< a name = "l00093" > < / a > < a class = "code" href = "class_navigation.html#a222cd205c6709681875cbb4efa2657f0" > 00093< / a > < a class = "code" href = "class_navigation.html#a222cd205c6709681875cbb4efa2657f0" > Navigation::set_next_wp< / a > (< a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > loc)
< a name = "l00094" > < / a > 00094 {
< a name = "l00095" > < / a > 00095 < a class = "code" href = "class_navigation.html#aa4cb8fedae77c0028012bf8284bb6217" > prev_wp< / a > = < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > ;
< a name = "l00096" > < / a > 00096 < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > = loc;
< a name = "l00097" > < / a > 00097
< a name = "l00098" > < / a > 00098 < span class = "keywordflow" > if< / span > (_scaleLongDown == 0)
< a name = "l00099" > < / a > 00099 calc_long_scaling(loc.< a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > );
< a name = "l00100" > < / a > 00100
< a name = "l00101" > < / a > 00101 < a class = "code" href = "class_navigation.html#a820f2e9d1a143419eb8e92d76c5702b7" > total_distance< / a > = < a class = "code" href = "class_navigation.html#ac05da5065688d7ef70452bbc4b8f8556" > get_distance< / a > (& < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > , & < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > );
< a name = "l00102" > < / a > 00102 < a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > = < a class = "code" href = "class_navigation.html#a820f2e9d1a143419eb8e92d76c5702b7" > total_distance< / a > ;
< a name = "l00103" > < / a > 00103
< a name = "l00104" > < / a > 00104 < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > = < a class = "code" href = "class_navigation.html#acf7075f5eedf8f47ae88b7ae04bccb39" > get_bearing< / a > (& < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > , & < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > );
< a name = "l00105" > < / a > 00105 _old_bearing = < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > ;
< a name = "l00106" > < / a > 00106
< a name = "l00107" > < / a > 00107 < span class = "comment" > // clear loitering code< / span >
< a name = "l00108" > < / a > 00108 _loiter_delta = 0;
< a name = "l00109" > < / a > 00109 < a class = "code" href = "class_navigation.html#a2e0bfad230d48a39926571d5d455f139" > loiter_sum< / a > = 0;
< a name = "l00110" > < / a > 00110
< a name = "l00111" > < / a > 00111 < span class = "comment" > // set a new crosstrack bearing< / span >
< a name = "l00112" > < / a > 00112 < span class = "comment" > // ----------------------------< / span >
< a name = "l00113" > < / a > 00113 reset_crosstrack();
< a name = "l00114" > < / a > 00114 }
< a name = "l00115" > < / a > 00115
< a name = "l00116" > < / a > 00116 < span class = "keywordtype" > void< / span >
< a name = "l00117" > < / a > 00117 Navigation::calc_long_scaling(int32_t lat)
< a name = "l00118" > < / a > 00118 {
< a name = "l00119" > < / a > 00119 < span class = "comment" > // this is used to offset the shrinking longitude as we go towards the poles < / span >
< a name = "l00120" > < / a > 00120 < span class = "keywordtype" > float< / span > rads = (abs(lat) / T7) * 0.0174532925;
< a name = "l00121" > < / a > 00121 _scaleLongDown = cos(rads);
< a name = "l00122" > < / a > 00122 _scaleLongUp = 1.0f / cos(rads);
< a name = "l00123" > < / a > 00123 }
< a name = "l00124" > < / a > 00124
< a name = "l00125" > < / a > 00125 < span class = "keywordtype" > void< / span >
< a name = "l00126" > < / a > < a class = "code" href = "class_navigation.html#a260152de551277ff2a980e63bbb14fbc" > 00126< / a > < a class = "code" href = "class_navigation.html#a260152de551277ff2a980e63bbb14fbc" > Navigation::set_hold_course< / a > (int16_t b)
< a name = "l00127" > < / a > 00127 {
< a name = "l00128" > < / a > 00128 < span class = "keywordflow" > if< / span > (b)
< a name = "l00129" > < / a > 00129 _hold_course = < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > ;
< a name = "l00130" > < / a > 00130 < span class = "keywordflow" > else< / span >
< a name = "l00131" > < / a > 00131 _hold_course = -1;
< a name = "l00132" > < / a > 00132 }
< a name = "l00133" > < / a > 00133
< a name = "l00134" > < / a > 00134 int16_t
< a name = "l00135" > < / a > < a class = "code" href = "class_navigation.html#a60d4bee672c92c1e4cced76089836ebf" > 00135< / a > < a class = "code" href = "class_navigation.html#a60d4bee672c92c1e4cced76089836ebf" > Navigation::get_hold_course< / a > (< span class = "keywordtype" > void< / span > )
< a name = "l00136" > < / a > 00136 {
< a name = "l00137" > < / a > 00137 < span class = "keywordflow" > return< / span > _hold_course;
< a name = "l00138" > < / a > 00138 }
< a name = "l00139" > < / a > 00139
< a name = "l00140" > < / a > 00140 < span class = "keywordtype" > void< / span >
< a name = "l00141" > < / a > 00141 Navigation::calc_distance_error()
< a name = "l00142" > < / a > 00142 {
< a name = "l00143" > < / a > 00143 < span class = "comment" > //distance_estimate += (float)_gps-> ground_speed * .0002 * cos(radians(bearing_error * .01));< / span >
< a name = "l00144" > < / a > 00144 < span class = "comment" > //distance_estimate -= DST_EST_GAIN * (float)(distance_estimate - GPS_distance);< / span >
< a name = "l00145" > < / a > 00145 < span class = "comment" > //distance = max(distance_estimate,10);< / span >
< a name = "l00146" > < / a > 00146 }
< a name = "l00147" > < / a > 00147
< a name = "l00148" > < / a > 00148 < span class = "keywordtype" > void< / span >
< a name = "l00149" > < / a > 00149 Navigation::calc_bearing_error(< span class = "keywordtype" > void< / span > )
< a name = "l00150" > < / a > 00150 {
< a name = "l00151" > < / a > 00151 < span class = "keywordflow" > if< / span > (_hold_course == -1){
< a name = "l00152" > < / a > 00152 < a class = "code" href = "class_navigation.html#a82c620baabad8049cc58fbe95a34890c" > bearing_error< / a > = < a class = "code" href = "class_navigation.html#abda1088bf8790dbcd31687ad4daf05b1" > wrap_180< / a > (< a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > - _gps-> < a class = "code" href = "class_g_p_s.html#a223b8bf1d912252c15aff621c37d00e3" title = "ground course in 100ths of a degree" > ground_course< / a > );
< a name = "l00153" > < / a > 00153 }< span class = "keywordflow" > else< / span > {
< a name = "l00154" > < / a > 00154 < a class = "code" href = "class_navigation.html#a82c620baabad8049cc58fbe95a34890c" > bearing_error< / a > = _hold_course;
< a name = "l00155" > < / a > 00155 }
< a name = "l00156" > < / a > 00156 }
< a name = "l00157" > < / a > 00157
< a name = "l00158" > < / a > 00158 int32_t
< a name = "l00159" > < / a > < a class = "code" href = "class_navigation.html#abda1088bf8790dbcd31687ad4daf05b1" > 00159< / a > < a class = "code" href = "class_navigation.html#abda1088bf8790dbcd31687ad4daf05b1" > Navigation::wrap_180< / a > (int32_t error)
< a name = "l00160" > < / a > 00160 {
< a name = "l00161" > < / a > 00161 < span class = "keywordflow" > if< / span > (error > 18000) error -= 36000;
< a name = "l00162" > < / a > 00162 < span class = "keywordflow" > if< / span > (error < -18000) error += 36000;
< a name = "l00163" > < / a > 00163 < span class = "keywordflow" > return< / span > error;
< a name = "l00164" > < / a > 00164 }
< a name = "l00165" > < / a > 00165
< a name = "l00166" > < / a > 00166 int32_t
< a name = "l00167" > < / a > < a class = "code" href = "class_navigation.html#a5ae7eb15b8aa81aeb2e2771fec018d5f" > 00167< / a > < a class = "code" href = "class_navigation.html#a5ae7eb15b8aa81aeb2e2771fec018d5f" > Navigation::wrap_360< / a > (int32_t angle)
< a name = "l00168" > < / a > 00168 {
< a name = "l00169" > < / a > 00169 < span class = "keywordflow" > if< / span > (angle > 36000) angle -= 36000;
< a name = "l00170" > < / a > 00170 < span class = "keywordflow" > if< / span > (angle < 0) angle += 36000;
< a name = "l00171" > < / a > 00171 < span class = "keywordflow" > return< / span > angle;
< a name = "l00172" > < / a > 00172 }
< a name = "l00173" > < / a > 00173
< a name = "l00174" > < / a > 00174 < span class = "keywordtype" > void< / span >
< a name = "l00175" > < / a > < a class = "code" href = "class_navigation.html#a3719b4db98a8fdc08599eff7e8e819af" > 00175< / a > < a class = "code" href = "class_navigation.html#a3719b4db98a8fdc08599eff7e8e819af" > Navigation::set_bearing_error< / a > (int32_t error)
< a name = "l00176" > < / a > 00176 {
< a name = "l00177" > < / a > 00177 < a class = "code" href = "class_navigation.html#a82c620baabad8049cc58fbe95a34890c" > bearing_error< / a > = < a class = "code" href = "class_navigation.html#abda1088bf8790dbcd31687ad4daf05b1" > wrap_180< / a > (error);
< a name = "l00178" > < / a > 00178 }
< a name = "l00179" > < / a > 00179
< a name = "l00180" > < / a > 00180
< a name = "l00181" > < / a > 00181 < span class = "comment" > /*****************************************< / span >
< a name = "l00182" > < / a > 00182 < span class = "comment" > * Altitude error with Airspeed correction< / span >
< a name = "l00183" > < / a > 00183 < span class = "comment" > *****************************************/< / span >
< a name = "l00184" > < / a > 00184 < span class = "keywordtype" > void< / span >
< a name = "l00185" > < / a > 00185 Navigation::calc_altitude_error(< span class = "keywordtype" > void< / span > )
< a name = "l00186" > < / a > 00186 {
< a name = "l00187" > < / a > 00187 < span class = "comment" > // limit climb rates< / span >
< a name = "l00188" > < / a > 00188 _target_altitude = < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > - ((float)((< a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > -20) * _offset_altitude) / (< span class = "keywordtype" > float< / span > )(< a class = "code" href = "class_navigation.html#a820f2e9d1a143419eb8e92d76c5702b7" > total_distance< / a > - 20));
< a name = "l00189" > < / a > 00189 < span class = "keywordflow" > if< / span > (< a class = "code" href = "class_navigation.html#aa4cb8fedae77c0028012bf8284bb6217" > prev_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > > < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > ){
< a name = "l00190" > < / a > 00190 _target_altitude = constrain(_target_altitude, < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > , < a class = "code" href = "class_navigation.html#aa4cb8fedae77c0028012bf8284bb6217" > prev_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > );
< a name = "l00191" > < / a > 00191 }< span class = "keywordflow" > else< / span > {
< a name = "l00192" > < / a > 00192 _target_altitude = constrain(_target_altitude, < a class = "code" href = "class_navigation.html#aa4cb8fedae77c0028012bf8284bb6217" > prev_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > , < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > );
< a name = "l00193" > < / a > 00193 }
< a name = "l00194" > < / a > 00194 < a class = "code" href = "class_navigation.html#a63e21494db8c2022f5ff488295f2f1fe" > altitude_error< / a > = _target_altitude - < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > .< a class = "code" href = "struct_waypoints_1_1_w_p.html#aa2b3ddc9b275b4561da9ec0738b1b1bb" > alt< / a > ;
< a name = "l00195" > < / a > 00195 }
< a name = "l00196" > < / a > 00196
< a name = "l00197" > < / a > 00197 < span class = "keywordtype" > void< / span >
< a name = "l00198" > < / a > < a class = "code" href = "class_navigation.html#a9aba08c1a7b668b80f2f3df8795095b6" > 00198< / a > < a class = "code" href = "class_navigation.html#a9aba08c1a7b668b80f2f3df8795095b6" > Navigation::set_loiter_vector< / a > (int16_t v)
< a name = "l00199" > < / a > 00199 {
< a name = "l00200" > < / a > 00200 < span class = "comment" > // _vector = constrain(v, -18000, 18000); XXX needs to be implemented< / span >
< a name = "l00201" > < / a > 00201 }
< a name = "l00202" > < / a > 00202
< a name = "l00203" > < / a > 00203 < span class = "keywordtype" > void< / span >
< a name = "l00204" > < / a > < a class = "code" href = "class_navigation.html#ab1178280258ea8502960ffb5b4fb2815" > 00204< / a > < a class = "code" href = "class_navigation.html#ab1178280258ea8502960ffb5b4fb2815" > Navigation::update_crosstrack< / a > (< span class = "keywordtype" > void< / span > )
< a name = "l00205" > < / a > 00205 {
< a name = "l00206" > < / a > 00206 < span class = "comment" > // Crosstrack Error< / span >
< a name = "l00207" > < / a > 00207 < span class = "comment" > // ----------------< / span >
< a name = "l00208" > < / a > 00208 < span class = "keywordflow" > if< / span > (abs(< a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > - _crosstrack_bearing) < 4500) { < span class = "comment" > // If we are too far off or too close we don' t do track following< / span >
< a name = "l00209" > < / a > 00209 _crosstrack_error = sin(radians((< a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > - _crosstrack_bearing) / 100)) * < a class = "code" href = "class_navigation.html#a5131b103f30de83744a07575680ae8ec" > distance< / a > ; < span class = "comment" > // Meters we are off track line< / span >
< a name = "l00210" > < / a > 00210 < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > += constrain(_crosstrack_error * < a class = "code" href = "_navigation_8h.html#a295b82827f88bc8115c34cfbb4650690" > XTRACK_GAIN< / a > , -< a class = "code" href = "_navigation_8h.html#adfd3d7f59e272b76b8a1038f512862f0" > XTRACK_ENTRY_ANGLE< / a > , < a class = "code" href = "_navigation_8h.html#adfd3d7f59e272b76b8a1038f512862f0" > XTRACK_ENTRY_ANGLE< / a > );
< a name = "l00211" > < / a > 00211 }
< a name = "l00212" > < / a > 00212 }
< a name = "l00213" > < / a > 00213
< a name = "l00214" > < / a > 00214 < span class = "keywordtype" > void< / span >
< a name = "l00215" > < / a > 00215 Navigation::reset_crosstrack(< span class = "keywordtype" > void< / span > )
< a name = "l00216" > < / a > 00216 {
< a name = "l00217" > < / a > 00217 _crosstrack_bearing = < a class = "code" href = "class_navigation.html#acf7075f5eedf8f47ae88b7ae04bccb39" > get_bearing< / a > (& < a class = "code" href = "class_navigation.html#a4f1060bf77cfc499ec5d3169c4494095" > location< / a > , & < a class = "code" href = "class_navigation.html#afd1dec04a84096681bf951df5edf72e0" > next_wp< / a > ); < span class = "comment" > // Used for track following< / span >
< a name = "l00218" > < / a > 00218 }
< a name = "l00219" > < / a > 00219
< a name = "l00220" > < / a > 00220 < span class = "keywordtype" > long< / span >
< a name = "l00221" > < / a > < a class = "code" href = "class_navigation.html#ac05da5065688d7ef70452bbc4b8f8556" > 00221< / a > < a class = "code" href = "class_navigation.html#ac05da5065688d7ef70452bbc4b8f8556" > Navigation::get_distance< / a > (< a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > *loc1, < a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > *loc2)
< a name = "l00222" > < / a > 00222 {
< a name = "l00223" > < / a > 00223 < span class = "keywordflow" > if< / span > (loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > == 0 || loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > == 0)
< a name = "l00224" > < / a > 00224 < span class = "keywordflow" > return< / span > -1;
< a name = "l00225" > < / a > 00225 < span class = "keywordflow" > if< / span > (loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > == 0 || loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > == 0)
< a name = "l00226" > < / a > 00226 < span class = "keywordflow" > return< / span > -1;
< a name = "l00227" > < / a > 00227 < span class = "keywordflow" > if< / span > (_scaleLongDown == 0)
< a name = "l00228" > < / a > 00228 calc_long_scaling(loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > );
< a name = "l00229" > < / a > 00229 < span class = "keywordtype" > float< / span > dlat = (float)(loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > - loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > );
< a name = "l00230" > < / a > 00230 < span class = "keywordtype" > float< / span > dlong = ((float)(loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > - loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > )) * _scaleLongDown;
< a name = "l00231" > < / a > 00231 < span class = "keywordflow" > return< / span > sqrt(sq(dlat) + sq(dlong)) * .01113195;
< a name = "l00232" > < / a > 00232 }
< a name = "l00233" > < / a > 00233
< a name = "l00234" > < / a > 00234 < span class = "keywordtype" > long< / span >
< a name = "l00235" > < / a > < a class = "code" href = "class_navigation.html#acf7075f5eedf8f47ae88b7ae04bccb39" > 00235< / a > < a class = "code" href = "class_navigation.html#acf7075f5eedf8f47ae88b7ae04bccb39" > Navigation::get_bearing< / a > (< a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > *loc1, < a class = "code" href = "struct_waypoints_1_1_w_p.html" > Waypoints::WP< / a > *loc2)
< a name = "l00236" > < / a > 00236 {
< a name = "l00237" > < / a > 00237 < span class = "keywordflow" > if< / span > (loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > == 0 || loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > == 0)
< a name = "l00238" > < / a > 00238 < span class = "keywordflow" > return< / span > -1;
< a name = "l00239" > < / a > 00239 < span class = "keywordflow" > if< / span > (loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > == 0 || loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > == 0)
< a name = "l00240" > < / a > 00240 < span class = "keywordflow" > return< / span > -1;
< a name = "l00241" > < / a > 00241 < span class = "keywordflow" > if< / span > (_scaleLongDown == 0)
< a name = "l00242" > < / a > 00242 calc_long_scaling(loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > );
< a name = "l00243" > < / a > 00243 < span class = "keywordtype" > long< / span > off_x = loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > - loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#aeaeb0aff010ed3057ff7fdfcac737054" > lng< / a > ;
< a name = "l00244" > < / a > 00244 < span class = "keywordtype" > long< / span > off_y = (loc2-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > - loc1-> < a class = "code" href = "struct_waypoints_1_1_w_p.html#a9383b83df64aa44b2c9d7398fac40639" > lat< / a > ) * _scaleLongUp;
< a name = "l00245" > < / a > 00245 < span class = "keywordtype" > long< / span > < a class = "code" href = "class_navigation.html#a327b5eb6bd65c8b1678b50d1002d95cb" > bearing< / a > = 9000 + atan2(-off_y, off_x) * 5729.57795;
< a name = "l00246" > < / a > 00246 < span class = "keywordflow" > if< / span > (bearing < 0)
< a name = "l00247" > < / a > 00247 bearing += 36000;
< a name = "l00248" > < / a > 00248 < span class = "keywordflow" > return< / span > bearing;
< a name = "l00249" > < / a > 00249 }
< / pre > < / div > < / div >
< / div >
2010-12-26 22:59:34 -04:00
< hr class = "footer" / > < address class = "footer" > < small > Generated on Sun Dec 26 2010 21:58:34 for ArduPilot Libraries by
2010-12-26 20:59:17 -04:00
< a href = "http://www.doxygen.org/index.html" >
< img class = "footer" src = "doxygen.png" alt = "doxygen" / > < / a > 1.7.1 < / small > < / address >
< / body >
< / html >
