Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   handle checking mission points for terrain data
      17             :  */
      18             : 
      19             : #include "AP_Terrain.h"
      20             : 
      21             : #if AP_TERRAIN_AVAILABLE
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : #include <AP_Common/AP_Common.h>
      25             : #include <AP_Math/AP_Math.h>
      26             : #include <AP_Mission/AP_Mission.h>
      27             : #include <AP_Rally/AP_Rally.h>
      28             : #include <AP_GPS/AP_GPS.h>
      29             : 
      30             : extern const AP_HAL::HAL& hal;
      31             : 
      32             : /*
      33             :   check that we have fetched all mission terrain data
      34             :  */
      35     1293011 : void AP_Terrain::update_mission_data(void)
      36             : {
      37             : #if AP_MISSION_ENABLED
      38     1293011 :     const AP_Mission &mission = AP::mission();
      39             : 
      40     2582885 :     if (last_mission_change_ms != mission.last_change_time_ms() ||
      41     1289874 :         last_mission_spacing != grid_spacing) {
      42             :         // the mission has changed - start again
      43        3141 :         next_mission_index = 1;
      44        3141 :         next_mission_pos = 0;
      45        3141 :         last_mission_change_ms = mission.last_change_time_ms();
      46        3141 :         last_mission_spacing = grid_spacing;
      47             :     }
      48     1293011 :     if (next_mission_index == 0) {
      49             :         // nothing to do
      50     1292300 :         return;
      51             :     }
      52             : 
      53             :     uint16_t pending, loaded;
      54       76290 :     get_statistics(pending, loaded);
      55       76290 :     if (pending && AP::gps().status() >= AP_GPS_FixType::FIX_3D) {
      56             :         // wait till we have fully filled the current set of grids
      57       72525 :         return;
      58             :     }
      59             : 
      60             :     // don't do more than 20 waypoints at a time, to prevent too much
      61             :     // CPU usage
      62       22509 :     for (uint8_t i=0; i<20; i++) {
      63             :         // get next mission command
      64       21798 :         AP_Mission::Mission_Command cmd;
      65       21798 :         if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
      66             :             // nothing more to do
      67        2173 :             next_mission_index = 0;
      68        3054 :             return;
      69             :         }
      70             : 
      71             :         // we only want nav waypoint commands. That should be enough to
      72             :         // prefill the terrain data and makes many things much simpler
      73       19625 :         while ((cmd.id != MAV_CMD_NAV_WAYPOINT &&
      74       20879 :                 cmd.id != MAV_CMD_NAV_SPLINE_WAYPOINT) ||
      75       19251 :                (cmd.content.location.lat == 0 && cmd.content.location.lng == 0)) {
      76        1663 :             next_mission_index++;
      77        1663 :             if (!mission.read_cmd_from_storage(next_mission_index, cmd)) {
      78             :                 // nothing more to do
      79         409 :                 next_mission_index = 0;
      80         409 :                 next_mission_pos = 0;
      81         409 :                 return;
      82             :             }
      83             :         }
      84             : 
      85             :         // we will fetch 5 points around the waypoint. Four at 10 grid
      86             :         // spacings away at 45, 135, 225 and 315 degrees, and the
      87             :         // point itself
      88       19216 :         if (next_mission_pos != 4) {
      89       15465 :             cmd.content.location.offset_bearing(45+90*next_mission_pos, grid_spacing.get() * 10);
      90             :         }
      91             : 
      92             :         // we have a mission command to check
      93             :         float height;
      94       19216 :         if (!height_amsl(cmd.content.location, height)) {
      95             :             // if we can't get data for a mission item then return and
      96             :             // check again next time
      97         472 :             return;
      98             :         }
      99             : 
     100       18744 :         next_mission_pos++;
     101       18744 :         if (next_mission_pos == 5) {
     102             : #if TERRAIN_DEBUG
     103             :             hal.console->printf("checked waypoint %u\n", (unsigned)next_mission_index);
     104             : #endif
     105             : 
     106             :             // move to next waypoint
     107        3748 :             next_mission_index++;
     108        3748 :             next_mission_pos = 0;
     109             :         }
     110             :     }
     111             : #endif  // AP_MISSION_ENABLED
     112             : }
     113             : 
     114             : #if HAL_RALLY_ENABLED
     115             : /*
     116             :   check that we have fetched all rally terrain data
     117             :  */
     118     1293011 : void AP_Terrain::update_rally_data(void)
     119             : {
     120     1293011 :     const AP_Rally *rally = AP::rally();
     121     1293011 :     if (rally == nullptr) {
     122           0 :         return;
     123             :     }
     124             : 
     125     2584331 :     if (last_rally_change_ms != rally->last_change_time_ms() ||
     126     1291320 :         last_rally_spacing != grid_spacing) {
     127             :         // a rally point has changed - start again
     128        1695 :         next_rally_index = 1;
     129        1695 :         last_rally_change_ms = rally->last_change_time_ms();
     130        1695 :         last_rally_spacing = grid_spacing;
     131             :     }
     132     1293011 :     if (next_rally_index == 0) {
     133             :         // nothing to do
     134     1291082 :         return;
     135             :     }
     136             : 
     137             :     uint16_t pending, loaded;
     138        1929 :     get_statistics(pending, loaded);
     139        1929 :     if (pending && AP::gps().status() >= AP_GPS_FixType::FIX_3D) {
     140             :         // wait till we have fully filled the current set of grids
     141         236 :         return;
     142             :     }
     143             : 
     144             :     while (true) {
     145             :         // get next rally point
     146             :         struct RallyLocation rp;
     147        1693 :         if (!rally->get_rally_point_with_index(next_rally_index, rp)) {
     148             :             // nothing more to do
     149        1693 :             next_rally_index = 0;
     150        1693 :             return;
     151             :         }
     152             : 
     153           0 :         Location loc;
     154           0 :         loc.lat = rp.lat;
     155           0 :         loc.lng = rp.lng;
     156             :         float height;
     157           0 :         if (!height_amsl(loc, height)) {
     158             :             // if we can't get data for a rally item then return and
     159             :             // check again next time
     160           0 :             return;
     161             :         }
     162             : 
     163             : #if TERRAIN_DEBUG
     164             :         hal.console->printf("checked rally point %u\n", (unsigned)next_rally_index);
     165             : #endif
     166             : 
     167             :         // move to next rally point
     168           0 :         next_rally_index++;
     169           0 :     }
     170             : }
     171             : #endif
     172             : 
     173             : #endif // AP_TERRAIN_AVAILABLE