mirror of https://github.com/ArduPilot/ardupilot
287 lines
31 KiB
HTML
287 lines
31 KiB
HTML
<!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>
|
|
<hr class="footer"/><address class="footer"><small>Generated on Sun Dec 26 2010 21:58:34 for ArduPilot Libraries by
|
|
<a href="http://www.doxygen.org/index.html">
|
|
<img class="footer" src="doxygen.png" alt="doxygen"/></a> 1.7.1 </small></address>
|
|
</body>
|
|
</html>
|