Line data    Source code

       1             : #include "Rover.h"
       2             : 
       3             : // set ahrs home to current location from inertial-nav location
       4       68172 : bool Rover::set_home_to_current_location(bool lock)
       5             : {
       6       68172 :     Location temp_loc;
       7       68172 :     if (ahrs.have_inertial_nav() && ahrs.get_location(temp_loc)) {
       8         150 :         if (!set_home(temp_loc, lock)) {
       9           0 :             return false;
      10             :         }
      11             :         // we have successfully set AHRS home, set it for SmartRTL
      12         150 :         g2.smart_rtl.set_home(true);
      13         150 :         return true;
      14             :     }
      15       68022 :     return false;
      16             : }
      17             : 
      18             : // sets ahrs home to specified location
      19             : // returns true if home location set successfully
      20         154 : bool Rover::set_home(const Location& loc, bool lock)
      21             : {
      22             :     // set ahrs home
      23         154 :     if (!ahrs.set_home(loc)) {
      24           0 :         return false;
      25             :     }
      26             : 
      27             :     // lock home position
      28         154 :     if (lock) {
      29           8 :         ahrs.lock_home();
      30             :     }
      31             : 
      32             :     // send text of home position to ground stations
      33         154 :     GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm",
      34             :             static_cast<double>(loc.lat * 1.0e-7f),
      35             :             static_cast<double>(loc.lng * 1.0e-7f),
      36             :             static_cast<double>(loc.alt * 0.01f));
      37             : 
      38             :     // return success
      39         154 :     return true;
      40             : }
      41             : 
      42             : // called periodically while disarmed to update our home position to
      43             : // our current location
      44        7527 : void Rover::update_home()
      45             : {
      46        7527 :     if (ahrs.home_is_locked()) {
      47             :         // we've been explicitly told our home location
      48        7220 :         return;
      49             :     }
      50             : 
      51        7498 :     Location loc{};
      52        7498 :     if (!ahrs.get_location(loc)) {
      53        1280 :         return;
      54             :     }
      55             : 
      56        6218 :     barometer.update_calibration();
      57             : 
      58       12434 :     if (ahrs.home_is_set() &&
      59        6216 :         loc.get_distance(ahrs.get_home()) < DISTANCE_HOME_MINCHANGE) {
      60             :         // insufficiently moved from current home - don't change it
      61        5911 :         return;
      62             :     }
      63             : 
      64         307 :     IGNORE_RETURN(ahrs.set_home(loc));
      65             : }