Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4          59 : bool ModeCruise::_enter()
       5             : {
       6          59 :     locked_heading = false;
       7          59 :     lock_timer_ms = 0;
       8             : 
       9             : #if HAL_SOARING_ENABLED
      10             :     // for ArduSoar soaring_controller
      11          59 :     plane.g2.soaring_controller.init_cruising();
      12             : #endif
      13             : 
      14          59 :     plane.set_target_altitude_current();
      15             : 
      16          59 :     return true;
      17             : }
      18             : 
      19      170051 : void ModeCruise::update()
      20             : {
      21             :     /*
      22             :       in CRUISE mode we use the navigation code to control
      23             :       roll when heading is locked. Heading becomes unlocked on
      24             :       any aileron or rudder input
      25             :     */
      26      170051 :     if (plane.channel_roll->get_control_in() != 0 || plane.channel_rudder->get_control_in() != 0) {
      27        6535 :         locked_heading = false;
      28        6535 :         lock_timer_ms = 0;
      29             :     }
      30             : 
      31             : #if AP_SCRIPTING_ENABLED
      32      170051 :     if (plane.nav_scripting_active()) {
      33             :         // while a trick is running unlock heading and zero altitude offset
      34        6657 :         locked_heading = false;
      35        6657 :         lock_timer_ms = 0;
      36        6657 :         plane.set_target_altitude_current();
      37             :     }
      38             : #endif
      39             :     
      40      170051 :     if (!locked_heading) {
      41       32083 :         plane.nav_roll_cd = plane.channel_roll->norm_input() * plane.roll_limit_cd;
      42       32083 :         plane.update_load_factor();
      43             :     } else {
      44      137968 :         plane.calc_nav_roll();
      45             :     }
      46      170051 :     plane.update_fbwb_speed_height();
      47      170051 : }
      48             : 
      49             : /*
      50             :   handle CRUISE mode, locking heading to GPS course when we have
      51             :   sufficient ground speed, and no aileron or rudder input
      52             :  */
      53        9808 : void ModeCruise::navigate()
      54             : {
      55             : #if AP_SCRIPTING_ENABLED
      56        9808 :     if (plane.nav_scripting_active()) {
      57             :         // don't try to navigate while running trick
      58        1331 :         return;
      59             :     }
      60             : #endif
      61             : 
      62             :     // check if we are moving in the direction of the front of the vehicle
      63        8477 :     const int32_t ground_course_cd = plane.gps.ground_course_cd();
      64        8477 :     const bool moving_forwards = fabsf(wrap_PI(cd_to_rad(ground_course_cd) - plane.ahrs.get_yaw_rad())) < M_PI_2;
      65             : 
      66        1715 :     if (!locked_heading &&
      67        2975 :         plane.channel_roll->get_control_in() == 0 &&
      68        2072 :         plane.rudder_input() == 0 &&
      69        1624 :         plane.gps.status() >= AP_GPS_FixType::FIX_2D &&
      70        1477 :         plane.gps.ground_speed() >= GPS_GND_CRS_MIN_SPD &&
      71       10192 :         moving_forwards &&
      72         665 :         lock_timer_ms == 0) {
      73             :         // user wants to lock the heading - start the timer
      74         107 :         lock_timer_ms = millis();
      75             :     }
      76        9220 :     if (lock_timer_ms != 0 &&
      77         743 :         (millis() - lock_timer_ms) > 500) {
      78             :         // lock the heading after 0.5 seconds of zero heading input
      79             :         // from user
      80         107 :         locked_heading = true;
      81         107 :         lock_timer_ms = 0;
      82         107 :         locked_heading_cd = ground_course_cd;
      83         107 :         plane.prev_WP_loc = plane.current_loc;
      84             :     }
      85        8477 :     if (locked_heading) {
      86        6869 :         plane.next_WP_loc = plane.prev_WP_loc;
      87             :         // always look 1km ahead
      88        6869 :         plane.next_WP_loc.offset_bearing(locked_heading_cd*0.01f, plane.prev_WP_loc.get_distance(plane.current_loc) + 1000);
      89        6869 :         plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
      90             :     }
      91             : }
      92             : 
      93           0 : bool ModeCruise::get_target_heading_cd(int32_t &target_heading) const
      94             : {
      95           0 :     target_heading = locked_heading_cd;
      96           0 :     return locked_heading;
      97             : }