Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4          61 : bool ModeGuided::_enter()
       5             : {
       6          61 :     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          61 :     Location loc{plane.current_loc};
      12             : 
      13             : #if HAL_QUADPLANE_ENABLED
      14          61 :     if (plane.quadplane.guided_mode_enabled()) {
      15             :         /*
      16             :           if using Q_GUIDED_MODE then project forward by the stopping distance
      17             :         */
      18           3 :         loc.offset_bearing(degrees(ahrs.groundspeed_vector().angle()),
      19           3 :                            plane.quadplane.stopping_distance_m());
      20             :     }
      21             : #endif
      22             : 
      23             :     // set guided radius to WP_LOITER_RAD on mode change.
      24          61 :     active_radius_m = 0;
      25             : 
      26          61 :     plane.set_guided_WP(loc);
      27          61 :     return true;
      28             : }
      29             : 
      30      735053 : void ModeGuided::update()
      31             : {
      32             : #if HAL_QUADPLANE_ENABLED
      33      735053 :     if (plane.auto_state.vtol_loiter && plane.quadplane.available()) {
      34      310640 :         plane.quadplane.guided_update();
      35      310640 :         return;
      36             :     }
      37             : #endif
      38             : 
      39             :     // Received an external msg that guides roll within g2.guided_timeout?
      40      429654 :     if (plane.guided_state.last_forced_rpy_ms.x > 0 &&
      41        5241 :             millis() - plane.guided_state.last_forced_rpy_ms.x < plane.g2.guided_timeout*1000.0f) {
      42        5241 :         plane.nav_roll_cd = constrain_int32(plane.guided_state.forced_rpy_cd.x, -plane.roll_limit_cd, plane.roll_limit_cd);
      43        5241 :         plane.update_load_factor();
      44             : 
      45             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_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      419172 :     } else if ((plane.control_mode == &plane.mode_guided) && (plane.guided_state.target_heading_type != GUIDED_HEADING_NONE) ) {
      49           0 :         uint32_t tnow = AP_HAL::millis();
      50           0 :         float delta = (tnow - plane.guided_state.target_heading_time_ms) * 1e-3f;
      51           0 :         plane.guided_state.target_heading_time_ms = tnow;
      52             : 
      53           0 :         float error = 0.0f;
      54           0 :         if (plane.guided_state.target_heading_type == GUIDED_HEADING_HEADING) {
      55           0 :             error = wrap_PI(plane.guided_state.target_heading - AP::ahrs().get_yaw_rad());
      56             :         } else {
      57           0 :             Vector2f groundspeed = AP::ahrs().groundspeed_vector();
      58           0 :             error = wrap_PI(plane.guided_state.target_heading - atan2f(-groundspeed.y, -groundspeed.x) + M_PI);
      59             :         }
      60             : 
      61           0 :         float bank_limit = degrees(atanf(plane.guided_state.target_heading_accel_limit/GRAVITY_MSS)) * 1e2f;
      62           0 :         bank_limit = MIN(bank_limit, plane.roll_limit_cd);
      63             : 
      64             :         // push error into AC_PID
      65           0 :         const float desired = plane.g2.guidedHeading.update_error(error, delta, plane.guided_state.target_heading_limit);
      66             : 
      67             :         // Check for output saturation
      68           0 :         plane.guided_state.target_heading_limit = fabsf(desired) >= bank_limit;
      69             : 
      70           0 :         plane.nav_roll_cd = constrain_int32(desired, -bank_limit, bank_limit);
      71           0 :         plane.update_load_factor();
      72             : 
      73             : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
      74           0 :     } else {
      75      419172 :         plane.calc_nav_roll();
      76             :     }
      77             : 
      78      429654 :     if (plane.guided_state.last_forced_rpy_ms.y > 0 &&
      79        5241 :             millis() - plane.guided_state.last_forced_rpy_ms.y < plane.g2.guided_timeout*1000.0f) {
      80        5241 :         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      419172 :         plane.calc_nav_pitch();
      83             :     }
      84             : 
      85             :     // Throttle output
      86      424413 :     if (plane.guided_throttle_passthru) {
      87             :         // manual passthrough of throttle in fence breach
      88          16 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.get_throttle_input(true));
      89             : 
      90      424397 :     }  else if (plane.aparm.throttle_cruise > 1 &&
      91      426993 :             plane.guided_state.last_forced_throttle_ms > 0 &&
      92        2596 :             millis() - plane.guided_state.last_forced_throttle_ms < plane.g2.guided_timeout*1000.0f) {
      93             :         // Received an external msg that guides throttle within g2.guided_timeout?
      94        1574 :         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, plane.guided_state.forced_throttle);
      95             : 
      96             :     } else {
      97             :         // TECS control
      98      422823 :         plane.calc_throttle();
      99             : 
     100             :     }
     101             : 
     102             : }
     103             : 
     104       40616 : void ModeGuided::navigate()
     105             : {
     106       40616 :     plane.update_loiter(active_radius_m);
     107       40616 : }
     108             : 
     109           1 : bool ModeGuided::handle_guided_request(Location target_loc)
     110             : {
     111           1 :     plane.fix_terrain_WP(target_loc, __AP_LINE__);
     112             :     // add home alt if needed
     113           1 :     if (!target_loc.terrain_alt) {
     114           1 :         target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
     115             :     }
     116             : 
     117           1 :     plane.set_guided_WP(target_loc);
     118             : 
     119           1 :     return true;
     120             : }
     121             : 
     122             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     123           4 : bool ModeGuided::handle_change_airspeed(const float airspeed, const float acceleration)
     124             : {
     125             :     // reject airspeeds that are outside of the tuning envelope
     126           4 :     if (airspeed > plane.aparm.airspeed_max || airspeed < plane.aparm.airspeed_min) {
     127           0 :         return false;
     128             :     }
     129             : 
     130             :     // no need to process any new packet/s with the
     131             :     // same airspeed any further, if we are already doing it.
     132           4 :     float new_target_airspeed_cm = airspeed * 100;
     133           4 :     if (is_equal(new_target_airspeed_cm,plane.guided_state.target_airspeed_cm)) { 
     134           0 :         return true;
     135             :     }
     136           4 :     plane.guided_state.target_airspeed_cm = new_target_airspeed_cm;
     137           4 :     plane.guided_state.target_airspeed_time_ms = AP_HAL::millis();
     138             : 
     139           4 :     if (is_zero(acceleration)) {
     140             :         // the user wanted /maximum acceleration, pick a large value as close enough
     141           3 :         plane.guided_state.target_airspeed_accel = 1000.0f;
     142             :     } else {
     143           1 :         plane.guided_state.target_airspeed_accel = fabsf(acceleration);
     144             :     }
     145             : 
     146             :     // assign an acceleration direction
     147           4 :     if (plane.guided_state.target_airspeed_cm < plane.target_airspeed_cm) {
     148           1 :         plane.guided_state.target_airspeed_accel *= -1.0f;
     149             :     }
     150           4 :     return true; 
     151             : }
     152             : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     153             : 
     154           6 : void ModeGuided::set_radius_and_direction(const float radius, const bool direction_is_ccw)
     155             : {
     156             :     // constrain to (uint16_t) range for update_loiter()
     157           6 :     active_radius_m = constrain_int32(fabsf(radius), 0, UINT16_MAX);
     158           6 :     plane.loiter.direction = direction_is_ccw ? -1 : 1;
     159           6 : }
     160             : 
     161             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     162      139768 : bool Plane::GuidedState::target_location_alt_is_minus_one() const
     163             : {
     164      139768 :     int32_t target_location_alt = 0;
     165      139768 :     UNUSED_RESULT(target_location.get_alt_cm(target_location.get_alt_frame(), target_location_alt));
     166      139768 :     return target_location_alt == -1;
     167             : }
     168             : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     169             : 
     170       40665 : void ModeGuided::update_target_altitude()
     171             : {
     172             : #if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     173             :     // target altitude can be negative (e.g. flying below home altitude from the top of a mountain)
     174       78804 :     if ((plane.guided_state.target_alt_time_ms != 0) ||
     175       38139 :         !plane.guided_state.target_location_alt_is_minus_one()) { // target_alt now defaults to -1, and _time_ms defaults to zero.
     176             :         // offboard altitude demanded
     177        2526 :         uint32_t now = AP_HAL::millis();
     178        2526 :         float delta = 1e-3f * (now - plane.guided_state.target_alt_time_ms);
     179        2526 :         plane.guided_state.target_alt_time_ms = now;
     180             :         // determine delta accurately as a float
     181        2526 :         float delta_amt_f = delta * plane.guided_state.target_alt_rate;
     182             :         // then scale x100 to match last_target_alt and convert to a signed int32_t as it may be negative
     183        2526 :         int32_t delta_amt_i = (int32_t)(100.0 * delta_amt_f); 
     184             :         // To calculate the required velocity (up or down), we need to target and current altitudes in the target frame
     185        2526 :         const Location::AltFrame target_frame = plane.guided_state.target_location.get_alt_frame();
     186             :         int32_t target_alt_previous_cm;
     187        5052 :         if (plane.current_loc.initialised() && plane.guided_state.target_location.initialised() && 
     188        2526 :             plane.current_loc.get_alt_cm(target_frame, target_alt_previous_cm)) {
     189             :             // create a new interim target location that that takes current_location and moves delta_amt_i in the right direction
     190        2526 :             int32_t temp_alt_cm = constrain_int32(plane.guided_state.target_location.alt, target_alt_previous_cm - delta_amt_i,  target_alt_previous_cm + delta_amt_i);
     191        2526 :             Location temp_location = plane.guided_state.target_location;
     192        2526 :             temp_location.set_alt_cm(temp_alt_cm, target_frame);
     193             : 
     194             :             // incrementally step the altitude towards the target            
     195        2526 :             plane.set_target_altitude_location(temp_location);
     196             :         }
     197             :     } else 
     198             : #endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
     199             :         {
     200       38139 :         Mode::update_target_altitude();
     201             :     }
     202       40665 : }