Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4             : #if HAL_SOARING_ENABLED
       5             : 
       6           2 : bool ModeThermal::_enter()
       7             : {
       8           2 :     if (!plane.g2.soaring_controller.is_active()) {
       9           1 :         return false;
      10             :     }
      11             : 
      12           1 :     plane.do_loiter_at_location();
      13           1 :     plane.loiter_angle_reset();
      14             : 
      15           1 :     plane.g2.soaring_controller.init_thermalling();
      16           1 :     plane.g2.soaring_controller.get_target(plane.next_WP_loc); // ahead on flight path
      17             : 
      18           1 :     return true;
      19             : }
      20             : 
      21       13618 : void ModeThermal::update()
      22             : {
      23       13618 :     plane.calc_nav_roll();
      24       13618 :     plane.calc_nav_pitch();
      25       13618 :     plane.calc_throttle();
      26       13618 : }
      27             : 
      28       13618 : void ModeThermal::update_soaring()
      29             : {
      30             :     // Update the thermal estimation and switching logic.
      31             :     // This is called from soaring.cpp at fixed 50Hz to avoid
      32             :     // potential issues with the main loop rate setting.
      33             : 
      34             :     // Update thermal estimate and check for switch back to AUTO
      35       13618 :     plane.g2.soaring_controller.update_thermalling();  // Update estimate
      36             : 
      37             :     // Thermalling is done in a home-relative coordinate system, so we need home to be set.
      38       13618 :     Vector3f position;
      39       13618 :     if (!AP::ahrs().get_relative_position_NED_home(position)) {
      40       13617 :         return;
      41             :     }
      42             : 
      43             :     // Check distance to home against MAX_RADIUS.
      44       13618 :     if (plane.g2.soaring_controller.max_radius >= 0 &&
      45       13618 :         sq(position.x)+sq(position.y) > sq(plane.g2.soaring_controller.max_radius) &&
      46           0 :         plane.previous_mode->mode_number()!=Mode::Number::AUTO) {
      47             :         // Some other loiter status, and outside of maximum soaring radius, and previous mode wasn't AUTO
      48           0 :         gcs().send_text(MAV_SEVERITY_INFO, "Soaring: Outside SOAR_MAX_RADIUS, RTL");
      49           0 :         plane.set_mode(plane.mode_rtl, ModeReason::SOARING_DRIFT_EXCEEDED);
      50           0 :         return;
      51             :     }
      52             : 
      53             :     // If previous mode was AUTO and there was a previous NAV command&, we can use previous and next wps for drift calculation
      54             :     // with respect to the desired direction of travel. If these vectors are zero, drift will be calculated from thermal start
      55             :     // position only, without taking account of the desired direction of travel.
      56       13618 :     Vector2f prev_wp, next_wp;
      57             : 
      58       13618 :     if (plane.previous_mode == &plane.mode_auto) {
      59       13618 :         AP_Mission::Mission_Command current_nav_cmd = plane.mission.get_current_nav_cmd();
      60       13618 :         AP_Mission::Mission_Command prev_nav_cmd;
      61             : 
      62       27236 :         if (!(plane.mission.get_next_nav_cmd(plane.mission.get_prev_nav_cmd_with_wp_index(), prev_nav_cmd) &&
      63       13618 :             prev_nav_cmd.content.location.get_vector_xy_from_origin_NE_cm(prev_wp) &&
      64       13618 :             current_nav_cmd.content.location.get_vector_xy_from_origin_NE_cm(next_wp))) {
      65           0 :             prev_wp.zero();
      66           0 :             next_wp.zero();
      67             :         }
      68             :     }
      69             : 
      70             :     // Get the status of the soaring controller cruise checks.
      71       13618 :     const SoaringController::LoiterStatus loiterStatus = plane.g2.soaring_controller.check_cruise_criteria(prev_wp/100, next_wp/100);
      72             : 
      73       13618 :     if (loiterStatus == SoaringController::LoiterStatus::GOOD_TO_KEEP_LOITERING) {
      74             :         // Reset loiter angle, so that the loiter exit heading criteria
      75             :         // only starts expanding when we're ready to exit.
      76       13617 :         plane.loiter.sum_cd = 0;
      77       13617 :         plane.soaring_mode_timer_ms = AP_HAL::millis();
      78             : 
      79             :         //update the wp location
      80       13617 :         plane.g2.soaring_controller.get_target(plane.next_WP_loc);
      81             : 
      82       13617 :         return;
      83             :     }
      84             : 
      85             :     // Some other loiter status, we need to think about exiting loiter.
      86           1 :     const uint32_t time_in_loiter_ms = AP_HAL::millis() - plane.soaring_mode_timer_ms;
      87           1 :     const uint32_t timeout = MIN(1000*plane.g2.soaring_controller.get_circling_time(), 20000);
      88             : 
      89           1 :     if (!exit_heading_aligned() && loiterStatus != SoaringController::LoiterStatus::ALT_TOO_LOW && time_in_loiter_ms < timeout) {
      90             :         // Heading not lined up, and not timed out or in a condition requiring immediate exit.
      91           0 :         return;
      92             :     }
      93             : 
      94             :     // Heading lined up and loiter status not good to continue. Need to restore previous mode.
      95           1 :     switch (loiterStatus) {
      96           1 :     case SoaringController::LoiterStatus::ALT_TOO_HIGH:
      97           1 :         restore_mode("Reached SOAR_ALT_MAX", ModeReason::SOARING_ALT_TOO_HIGH);
      98           1 :         break;
      99           0 :     case SoaringController::LoiterStatus::ALT_TOO_LOW:
     100           0 :         restore_mode("Reached SOAR_ALT_MIN", ModeReason::SOARING_ALT_TOO_LOW);
     101           0 :         break;
     102           0 :     default:
     103             :     case SoaringController::LoiterStatus::THERMAL_WEAK:
     104           0 :         restore_mode("Climb below SOAR_VSPEED", ModeReason::SOARING_THERMAL_ESTIMATE_DETERIORATED);
     105           0 :         break;
     106           0 :     case SoaringController::LoiterStatus::DRIFT_EXCEEDED:
     107           0 :         restore_mode("Reached SOAR_MAX_DRIFT", ModeReason::SOARING_DRIFT_EXCEEDED);
     108           0 :         break;
     109           0 :     case SoaringController::LoiterStatus::EXIT_COMMANDED:
     110           0 :         restore_mode("Exit via RC switch", ModeReason::RC_COMMAND);
     111           0 :         break;
     112             :     } // switch loiterStatus
     113             : }
     114             : 
     115        2724 : void ModeThermal::navigate()
     116             : {
     117             :     // Soaring library calculates radius from SOAR_THML_BANK.
     118        2724 :     const float radius = plane.g2.soaring_controller.get_thermalling_radius();
     119             : 
     120        2724 :     plane.update_loiter(radius);
     121        2724 : }
     122             : 
     123           1 : bool ModeThermal::exit_heading_aligned() const
     124             : {
     125             :     // Return true if the current heading is aligned with the next objective.
     126             :     // If home is not set, or heading not locked, return true to avoid delaying mode change.
     127           1 :     switch (plane.previous_mode->mode_number()) {
     128           1 :     case Mode::Number::AUTO: {
     129             :         //Get the lat/lon of next Nav waypoint after this one:
     130           1 :         AP_Mission::Mission_Command current_nav_cmd = plane.mission.get_current_nav_cmd();
     131           1 :         return plane.mode_loiter.isHeadingLinedUp(plane.next_WP_loc, current_nav_cmd.content.location);
     132             :     }
     133           0 :     case Mode::Number::FLY_BY_WIRE_B:
     134           0 :         return (!AP::ahrs().home_is_set() || plane.mode_loiter.isHeadingLinedUp(plane.next_WP_loc, AP::ahrs().get_home()));
     135           0 :     case Mode::Number::CRUISE:
     136             :         int32_t target_heading_cd;
     137           0 :         return (!plane.mode_cruise.get_target_heading_cd(target_heading_cd) || plane.mode_loiter.isHeadingLinedUp_cd(target_heading_cd));
     138           0 :     default:
     139           0 :         break;
     140             :     }
     141           0 :     return true;
     142             : }
     143             : 
     144           1 : void ModeThermal::restore_mode(const char *reason, ModeReason modereason)
     145             : {
     146           1 :     gcs().send_text(MAV_SEVERITY_INFO, "Soaring: %s, restoring %s", reason, plane.previous_mode->name());
     147           1 :     plane.set_mode(*plane.previous_mode, modereason);
     148           1 : }
     149             : 
     150             : #endif