ArduPlane/mode_guided.cpp

   1 #include "mode.h"
   2 #include "Plane.h"
   3
   4 bool ModeGuided::_enter()
   5 {
   6     plane.guided_throttle_passthru = false;
   7     /*
   8       when entering guided mode we set the target as the current
   9       location. This matches the behaviour of the copter code
  10     */
  11     Location loc{plane.current_loc};
  12
  13 #if HAL_QUADPLANE_ENABLED
  14     if (plane.quadplane.guided_mode_enabled()) {
  15         /*
  16           if using Q_GUIDED_MODE then project forward by the stopping distance
  17         */
  18         loc.offset_bearing(degrees(ahrs.groundspeed_vector().angle()),
  19                            plane.quadplane.stopping_distance());
  20     }
  21 #endif
  22
  23     // set guided radius to WP_LOITER_RAD on mode change.
  24     active_radius_m = 0;
  25
  26     plane.set_guided_WP(loc);
  27     return true;
  28 }
  29
  30 void ModeGuided::update()
  31 {
  32 #if HAL_QUADPLANE_ENABLED
  33     if (plane.auto_state.vtol_loiter && plane.quadplane.available()) {
  34         plane.quadplane.guided_update();
  35         return;
  36     }
  37 #endif
  38
  39     // Received an external msg that guides roll in the last 3 seconds?
  40     if (plane.guided_state.last_forced_rpy_ms.x > 0 &&
  41             millis() - plane.guided_state.last_forced_rpy_ms.x < 3000) {
  42         plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
  43         plane.update_load_factor();
  44
  45 #if OFFBOARD_GUIDED == ENABLED
  46     // guided_state.target_heading is radians at this point between -pi and pi ( defaults to -4 )
  47     // This function is used in Guided and AvoidADSB, check for guided
  48     } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
  49         uint32_t tnow = AP_HAL::millis();
  50         float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f;
  51         plane.guided_state.target_heading_time_ms = tnow;
  52
  53         float error = 0.0f;
  54         if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) {
  55             error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().get_yaw());
  56         } else {
  57             Vector2f groundspeed = AP::ahrs().groundspeed_vector();
  58             error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI);
  59         }
  60
  61         float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
  62         bank_limit = MIN(bank_limit, plane.roll_limit_cd);
  63
  64         // push error into AC_PID
  65         const float desired = plane.g2.guidedHeading.update_error(error, delta, plane.guided_state.target_heading_limit);
  66
  67         // Check for output saturation
  68         plane.guided_state.target_heading_limit = fabsf(desired) >= bank_limit;
  69
  70         plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
  71         plane.update_load_factor();
  72
  73 #endif // OFFBOARD_GUIDED == ENABLED
  74     } else {
  75         plane.calc_nav_roll();
  76     }
  77
  78     if (plane.guided_state.last_forced_rpy_ms.y > 0 &&
  79             millis() - plane.guided_state.last_forced_rpy_ms.y < 3000) {
  80         plane.nav_pitch_cd = constrain_int32(plane.guided_state.forced_rpy_cd.y, plane.pitch_limit_min*100, plane.aparm.pitch_limit_max.get()*100);
  81     } else {
  82         plane.calc_nav_pitch();
  83     }
  84
  85     // Throttle output
  86     if (plane.guided_throttle_passthru) {
  87         // manual passthrough of throttle in fence breach
  88         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
  89
  90     }  else if (plane.aparm.throttle_cruise > 1 &&
  91             plane.guided_state.last_forced_throttle_ms > 0 &&
  92             millis() - plane.guided_state.last_forced_throttle_ms < 3000) {
  93         // Received an external msg that guides throttle in the last 3 seconds?
  94         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);
  95
  96     } else {
  97         // TECS control
  98         plane.calc_throttle();
  99
 100     }
 101
 102 }
 103
 104 void ModeGuided::navigate()
 105 {
 106     plane.update_loiter(active_radius_m);
 107 }
 108
 109 bool ModeGuided::handle_guided_request(Location target_loc)
 110 {
 111     // add home alt if needed
 112     if (target_loc.relative_alt) {
 113         target_loc.alt += plane.home.alt;
 114         target_loc.relative_alt = 0;
 115     }
 116
 117     plane.set_guided_WP(target_loc);
 118
 119     return true;
 120 }
 121
 122 void ModeGuided::set_radius_and_direction(const float radius, const bool direction_is_ccw)
 123 {
 124     // constrain to (uint16_t) range for update_loiter()
 125     active_radius_m = constrain_int32(fabsf(radius), 0, UINT16_MAX);
 126     plane.loiter.direction = direction_is_ccw ? -1 : 1;
 127 }
 128
 129 void ModeGuided::update_target_altitude()
 130 {
 131 #if OFFBOARD_GUIDED == ENABLED
 132     if (((plane.guided_state.target_alt_time_ms != 0) || plane.guided_state.target_alt > -0.001 )) { // target_alt now defaults to -1, and _time_ms defaults to zero.
 133         // offboard altitude demanded
 134         uint32_t now = AP_HAL::millis();
 135         float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms);
 136         plane.guided_state.target_alt_time_ms = now;
 137         // determine delta accurately as a float
 138         float delta_amt_f = delta * plane.guided_state.target_alt_accel;
 139         // then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
 140         int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f);
 141         Location temp {};
 142         temp.alt = plane.guided_state.last_target_alt + delta_amt_i; // ...to avoid floats here,
 143         if (is_positive(plane.guided_state.target_alt_accel)) {
 144             temp.alt = MIN(plane.guided_state.target_alt, temp.alt);
 145         } else {
 146             temp.alt = MAX(plane.guided_state.target_alt, temp.alt);
 147         }
 148         plane.guided_state.last_target_alt = temp.alt;
 149         plane.set_target_altitude_location(temp);
 150     } else
 151 #endif // OFFBOARD_GUIDED == ENABLED
 152         {
 153         Mode::update_target_altitude();
 154     }
 155 }