Line data    Source code

       1             : #include "Sub.h"
       2             : 
       3             : // checks if we should update ahrs/RTL home position from the EKF
       4     2180683 : void Sub::update_home_from_EKF()
       5             : {
       6             :     // exit immediately if home already set
       7     2180683 :     if (ahrs.home_is_set()) {
       8     1811855 :         return;
       9             :     }
      10      368828 :     if (!set_home_to_current_location(false)) {
      11             :         // ignore this failure
      12             :     }
      13             : }
      14             : 
      15             : // set_home_to_current_location - set home to current GPS location
      16      368876 : bool Sub::set_home_to_current_location(bool lock)
      17             : {
      18             :     // get current location from EKF
      19      368876 :     Location temp_loc;
      20      368876 :     if (ahrs.get_location(temp_loc)) {
      21             : 
      22             :         // Make home always at the water's surface.
      23             :         // This allows disarming and arming again at depth.
      24             :         // This also ensures that mission items with relative altitude frame, are always
      25             :         // relative to the water's surface, whether in a high elevation lake, or at sea level.
      26      157628 :         temp_loc.offset_up_m(-barometer.get_altitude());
      27      157628 :         return set_home(temp_loc, lock);
      28             :     }
      29      211248 :     return false;
      30             : }
      31             : 
      32             : // set_home - sets ahrs home (used for RTL) to specified location
      33             : //  returns true if home location set successfully
      34      157630 : bool Sub::set_home(const Location& loc, bool lock)
      35             : {
      36             :     // check if EKF origin has been set
      37      157630 :     Location ekf_origin;
      38      157630 :     if (!ahrs.get_origin(ekf_origin)) {
      39      157546 :         return false;
      40             :     }
      41             : 
      42             :     // set ahrs home (used for RTL)
      43          84 :     if (!ahrs.set_home(loc)) {
      44           0 :         return false;
      45             :     }
      46             : 
      47             :     // lock home position
      48          84 :     if (lock) {
      49           4 :         ahrs.lock_home();
      50             :     }
      51             : 
      52             :     // return success
      53          84 :     return true;
      54             : }