Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : // checks if we should update ahrs/RTL home position from the EKF
       4    30412448 : void Copter::update_home_from_EKF()
       5             : {
       6             :     // exit immediately if home already set
       7    30412448 :     if (ahrs.home_is_set()) {
       8    26564941 :         return;
       9             :     }
      10             : 
      11             :     // special logic if home is set in-flight
      12     3847507 :     if (motors->armed()) {
      13       74802 :         set_home_to_current_location_inflight();
      14             :     } else {
      15             :         // move home to current ekf location (this will set home_state to HOME_SET)
      16     3772705 :         if (!set_home_to_current_location(false)) {
      17             :             // ignore failure
      18             :         }
      19             :     }
      20             : }
      21             : 
      22             : // set_home_to_current_location_inflight - set home to current GPS location (horizontally) and EKF origin vertically
      23       74802 : void Copter::set_home_to_current_location_inflight() {
      24             :     // get current location from EKF
      25       74802 :     Location temp_loc;
      26       74802 :     Location ekf_origin;
      27       74802 :     if (ahrs.get_location(temp_loc) && ahrs.get_origin(ekf_origin)) {
      28           1 :         temp_loc.copy_alt_from(ekf_origin);
      29           1 :         if (!set_home(temp_loc, false)) {
      30           0 :             return;
      31             :         }
      32             :         // we have successfully set AHRS home, set it for SmartRTL
      33             : #if MODE_SMARTRTL_ENABLED
      34           1 :         g2.smart_rtl.set_home(true);
      35             : #endif
      36             :     }
      37             : }
      38             : 
      39             : // set_home_to_current_location - set home to current GPS location
      40     3773231 : bool Copter::set_home_to_current_location(bool lock) {
      41             :     // get current location from EKF
      42     3773231 :     Location temp_loc;
      43     3773231 :     if (ahrs.get_location(temp_loc)) {
      44     1588335 :         if (!set_home(temp_loc, lock)) {
      45     1587360 :             return false;
      46             :         }
      47             :         // we have successfully set AHRS home, set it for SmartRTL
      48             : #if MODE_SMARTRTL_ENABLED
      49         975 :         g2.smart_rtl.set_home(true);
      50             : #endif
      51         975 :         return true;
      52             :     }
      53     2184896 :     return false;
      54             : }
      55             : 
      56             : // set_home - sets ahrs home (used for RTL) to specified location
      57             : //  returns true if home location set successfully
      58     1588363 : bool Copter::set_home(const Location& loc, bool lock)
      59             : {
      60             :     // check EKF origin has been set
      61     1588363 :     Location ekf_origin;
      62     1588363 :     if (!ahrs.get_origin(ekf_origin)) {
      63     1587360 :         return false;
      64             :     }
      65             : 
      66             :     // set ahrs home (used for RTL)
      67        1003 :     if (!ahrs.set_home(loc)) {
      68           0 :         return false;
      69             :     }
      70             : 
      71             :     // lock home position
      72        1003 :     if (lock) {
      73          32 :         ahrs.lock_home();
      74             :     }
      75             : 
      76             :     // return success
      77        1003 :     return true;
      78             : }