Line data    Source code

       1             : /*
       2             :  *  logic for dealing with the current command in the mission and home location
       3             :  */
       4             : 
       5             : #include "Plane.h"
       6             : 
       7             : /*
       8             :  *  set_next_WP - sets the target location the vehicle should fly to
       9             :  */
      10        3869 : void Plane::set_next_WP(const Location &loc)
      11             : {
      12        3869 :     if (auto_state.next_wp_crosstrack) {
      13             :         // copy the current WP into the OldWP slot
      14        3382 :         prev_WP_loc = next_WP_loc;
      15        3382 :         auto_state.crosstrack = true;
      16             :     } else {
      17             :         // we should not try to cross-track for this waypoint
      18         487 :         prev_WP_loc = current_loc;
      19             :         // use cross-track for the next waypoint
      20         487 :         auto_state.next_wp_crosstrack = true;
      21         487 :         auto_state.crosstrack = false;
      22             :     }
      23             : 
      24             :     // Load the next_WP slot
      25             :     // ---------------------
      26        3869 :     next_WP_loc = loc;
      27             : 
      28             :     // if lat and lon is zero, then use current lat/lon
      29             :     // this allows a mission to contain a "loiter on the spot"
      30             :     // command
      31        3869 :     if (next_WP_loc.lat == 0 && next_WP_loc.lng == 0) {
      32        2644 :         next_WP_loc.lat = current_loc.lat;
      33        2644 :         next_WP_loc.lng = current_loc.lng;
      34             :         // additionally treat zero altitude as current altitude
      35        2644 :         if (next_WP_loc.alt_is_zero()) {
      36           6 :             next_WP_loc.copy_alt_from(current_loc);
      37             :         }
      38             :     }
      39             : 
      40        3869 :     fix_terrain_WP(next_WP_loc, __AP_LINE__);
      41             : 
      42             :     // convert relative alt to absolute alt
      43        3869 :     if (!next_WP_loc.terrain_alt) {
      44        3754 :         next_WP_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
      45             :     }
      46             : 
      47             :     // are we already past the waypoint? This happens when we jump
      48             :     // waypoints, and it can cause us to skip a waypoint. If we are
      49             :     // past the waypoint when we start on a leg, then use the current
      50             :     // location as the previous waypoint, to prevent immediately
      51             :     // considering the waypoint complete
      52        3869 :     if (current_loc.past_interval_finish_line(prev_WP_loc, next_WP_loc)) {
      53        2700 :         prev_WP_loc = current_loc;
      54             :     }
      55             : 
      56             :     // zero out our loiter vals to watch for missed waypoints
      57        3869 :     loiter_angle_reset();
      58             : 
      59        3869 :     setup_alt_slope();
      60        3869 :     setup_turn_angle();
      61             : 
      62             :     // update plane.target_altitude straight away, or if we are too
      63             :     // close to out loiter point we may decide we are at the correct
      64             :     // altitude before updating it (this is based on scheduler table
      65             :     // ordering, where we navigate() before we
      66             :     // adjust_altitude_target(), and navigate() uses values updated in
      67             :     // adjust_altitude_target()
      68        3869 :     adjust_altitude_target();
      69        3869 : }
      70             : 
      71         209 : void Plane::set_guided_WP(const Location &loc)
      72             : {
      73         209 :     if (aparm.loiter_radius < 0 || loc.loiter_ccw) {
      74           5 :         loiter.direction = -1;
      75             :     } else {
      76         204 :         loiter.direction = 1;
      77             :     }
      78             : 
      79             :     // copy the current location into the OldWP slot
      80             :     // ---------------------------------------
      81         209 :     prev_WP_loc = current_loc;
      82             : 
      83             :     // Load the next_WP slot
      84             :     // ---------------------
      85         209 :     next_WP_loc = loc;
      86             : 
      87         209 :     fix_terrain_WP(next_WP_loc, __AP_LINE__);
      88             : 
      89             :     // used to control FBW and limit the rate of climb
      90             :     // -----------------------------------------------
      91         209 :     set_target_altitude_current();
      92             : 
      93         209 :     setup_alt_slope();
      94         209 :     setup_turn_angle();
      95             : 
      96             :     // disable crosstrack, head directly to the point
      97         209 :     auto_state.crosstrack = false;
      98             : 
      99             :     // reset loiter start time.
     100         209 :     loiter.start_time_ms = 0;
     101             : 
     102             :     // start in non-VTOL mode
     103         209 :     auto_state.vtol_loiter = false;
     104             :     
     105         209 :     loiter_angle_reset();
     106             : 
     107             : #if HAL_QUADPLANE_ENABLED
     108             :     // cancel pending takeoff
     109         209 :     quadplane.guided_takeoff = false;
     110             : #endif
     111         209 : }
     112             : 
     113             : /*
     114             :   update home location from GPS
     115             :   this is called as long as we have 3D lock and the arming switch is
     116             :   not pushed
     117             : 
     118             :   returns true if home is changed
     119             : */
     120        2368 : bool Plane::update_home()
     121             : {
     122        2368 :     if (hal.util->was_watchdog_armed()) {
     123           0 :         return false;
     124             :     }
     125        4558 :     if ((g2.home_reset_threshold == -1) ||
     126        2190 :         ((g2.home_reset_threshold > 0) &&
     127           0 :          (fabsf(barometer.get_altitude()) > g2.home_reset_threshold))) {
     128             :         // don't auto-update if we have changed barometer altitude
     129             :         // significantly. This allows us to cope with slow baro drift
     130             :         // but not re-do home and the baro if we have changed height
     131             :         // significantly
     132         178 :         return false;
     133             :     }
     134        2190 :     bool ret = false;
     135        2190 :     if (ahrs.home_is_set() && !ahrs.home_is_locked() && gps.status() >= AP_GPS_FixType::FIX_3D) {
     136        1853 :         Location loc;
     137        1853 :         if (ahrs.get_location(loc)) {
     138             :             // we take the altitude directly from the GPS as we are
     139             :             // about to reset the baro calibration. We can't use AHRS
     140             :             // altitude or we can end up perpetuating a bias in
     141             :             // altitude, as AHRS alt depends on home alt, which means
     142             :             // we would have a circular dependency
     143        1853 :             loc.set_alt_cm(gps.location().alt,
     144             :                            Location::AltFrame::ABSOLUTE);
     145        1853 :             ret = AP::ahrs().set_home(loc);
     146             :         }
     147             :     }
     148             : 
     149             :     // even if home is not updated we do a baro reset to stop baro
     150             :     // drift errors while disarmed
     151        2190 :     barometer.update_calibration();
     152        2190 :     ahrs.resetHeightDatum();
     153             : 
     154        2190 :     update_current_loc();
     155             : 
     156        2190 :     return ret;
     157             : }
     158             : 
     159         355 : bool Plane::set_home_persistently(const Location &loc)
     160             : {
     161         355 :     if (hal.util->was_watchdog_armed()) {
     162           0 :         return false;
     163             :     }
     164         355 :     if (!AP::ahrs().set_home(loc)) {
     165           0 :         return false;
     166             :     }
     167             : 
     168         355 :     return true;
     169             : }