Line data    Source code

       1             : #include "mode.h"
       2             : #include "Plane.h"
       3             : 
       4             : #if HAL_QUADPLANE_ENABLED
       5             : 
       6       15142 : bool ModeQLoiter::_enter()
       7             : {
       8             :     // initialise loiter
       9       15142 :     loiter_nav->clear_pilot_desired_acceleration();
      10       15142 :     loiter_nav->init_target();
      11             : 
      12             :     // set vertical speed and acceleration limits
      13             :     // All limits must be positive
      14       15142 :     pos_control->D_set_max_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss);
      15       15142 :     pos_control->D_set_correction_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss);
      16             : 
      17       15142 :     quadplane.init_throttle_wait();
      18             : 
      19             :     // prevent re-init of target position
      20       15142 :     quadplane.last_loiter_ms = AP_HAL::millis();
      21             : 
      22             :     // clear precland timestamp
      23       15142 :     last_target_loc_set_ms = 0;
      24             : 
      25       15142 :     return true;
      26             : }
      27             : 
      28      485907 : void ModeQLoiter::update()
      29             : {
      30      485907 :     plane.mode_qstabilize.update();
      31      485907 : }
      32             : 
      33             : // run quadplane loiter controller
      34      759335 : void ModeQLoiter::run()
      35             : {
      36      759335 :     if (quadplane.assist.check_VTOL_recovery()) {
      37             :         // use QHover to recover from extreme attitudes, this allows
      38             :         // for the fixed wing controller to handle the recovery
      39       16404 :         plane.mode_qhover.run();
      40      122716 :         return;
      41             :     }
      42             : 
      43      742931 :     const uint32_t now = AP_HAL::millis();
      44             : 
      45             : #if AC_PRECLAND_ENABLED
      46      742931 :     const uint32_t precland_timeout_ms = 250;
      47             :     /*
      48             :       see if precision landing or precision loiter is active with
      49             :       an override of the target location.
      50             : 
      51             :     */
      52      742931 :     const uint32_t last_pos_set_ms = last_target_loc_set_ms;
      53      742931 :     const uint32_t last_vel_set_ms = quadplane.poscontrol.last_velocity_match_ms;
      54             : 
      55      742931 :     if (last_pos_set_ms != 0 && now - last_pos_set_ms < precland_timeout_ms) {
      56             :         // we have an active landing target override
      57           0 :         Vector2f rel_origin;
      58           0 :         if (plane.next_WP_loc.get_vector_xy_from_origin_NE_cm(rel_origin)) {
      59           0 :             quadplane.pos_control->set_pos_desired_NE_m(rel_origin.topostype() * 0.01);
      60           0 :             last_target_loc_set_ms = 0;
      61             :         }
      62             :     }
      63             : 
      64             :     // allow for velocity override as well
      65      742931 :     if (last_vel_set_ms != 0 && now - last_vel_set_ms < precland_timeout_ms) {
      66             :         // we have an active landing velocity override
      67           0 :         Vector2f target_accel;
      68           0 :         Vector2f target_speed_xy_ms{quadplane.poscontrol.velocity_match_ms.x, quadplane.poscontrol.velocity_match_ms.y};
      69           0 :         quadplane.pos_control->input_vel_accel_NE_m(target_speed_xy_ms, target_accel);
      70           0 :         quadplane.poscontrol.last_velocity_match_ms = 0;
      71             :     }
      72             : #endif // AC_PRECLAND_ENABLED
      73             : 
      74      742931 :     if (quadplane.tailsitter.in_vtol_transition(now)) {
      75             :         // Tailsitters in FW pull up phase of VTOL transition run FW controllers
      76         427 :         Mode::run();
      77         427 :         return;
      78             :     }
      79             : 
      80      742504 :     if (quadplane.throttle_wait) {
      81      105885 :         quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
      82      105885 :         attitude_control->set_throttle_out(0, true, 0);
      83      105885 :         quadplane.relax_attitude_control();
      84      105885 :         pos_control->D_relax_controller(0);
      85      105885 :         loiter_nav->clear_pilot_desired_acceleration();
      86      105885 :         loiter_nav->init_target();
      87             : 
      88             :         // Stabilize with fixed wing surfaces
      89      105885 :         plane.stabilize_roll();
      90      105885 :         plane.stabilize_pitch();
      91      105885 :         return;
      92             :     }
      93      636619 :     if (!quadplane.motors->armed()) {
      94       15049 :         plane.mode_qloiter._enter();
      95             :     }
      96             : 
      97      636619 :     if (quadplane.should_relax()) {
      98       55270 :         loiter_nav->soften_for_landing();
      99             :     }
     100             : 
     101      636619 :     if (now - quadplane.last_loiter_ms > 500) {
     102          72 :         loiter_nav->clear_pilot_desired_acceleration();
     103          72 :         loiter_nav->init_target();
     104             :     }
     105      636619 :     quadplane.last_loiter_ms = now;
     106             : 
     107             :     // motors use full range
     108      636619 :     quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
     109             : 
     110             :     // set vertical speed and acceleration limits
     111             :     // All limits must be positive
     112      636619 :     pos_control->D_set_max_speed_accel_m(quadplane.get_pilot_velocity_z_max_dn_m(), quadplane.pilot_speed_z_max_up_ms, quadplane.pilot_accel_z_mss);
     113             : 
     114             :     // process pilot's roll and pitch input
     115             :     float target_roll_cd, target_pitch_cd;
     116      636619 :     quadplane.get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd());
     117      636619 :     loiter_nav->set_pilot_desired_acceleration_cd(target_roll_cd, target_pitch_cd);
     118             :     
     119             :     // run loiter controller
     120      636619 :     if (!pos_control->NE_is_active()) {
     121           0 :         pos_control->NE_init_controller();
     122             :     }
     123      636619 :     loiter_nav->update();
     124             : 
     125             :     // nav roll and pitch are controller by loiter controller
     126      636619 :     plane.nav_roll_cd = loiter_nav->get_roll_cd();
     127      636619 :     plane.nav_pitch_cd = loiter_nav->get_pitch_cd();
     128             : 
     129      636619 :     plane.quadplane.assign_tilt_to_fwd_thr();
     130             : 
     131      636619 :     if (quadplane.transition->set_VTOL_roll_pitch_limit(plane.nav_roll_cd, plane.nav_pitch_cd)) {
     132       14305 :         pos_control->NE_set_externally_limited();
     133             :     }
     134             : 
     135             :     // Pilot input, use yaw rate time constant
     136      636619 :     quadplane.set_pilot_yaw_rate_time_constant();
     137             : 
     138      636619 :     Vector3f target { plane.nav_roll_cd*0.01, plane.nav_pitch_cd*0.01, quadplane.get_desired_yaw_rate_cds() * 0.01 };
     139             : 
     140             : #if AP_PLANE_SYSTEMID_ENABLED
     141      636619 :     auto &systemid = plane.g2.systemid;
     142      636619 :     systemid.vtol_update();
     143      636619 :     target += systemid.get_attitude_offset_deg();
     144             : #endif
     145             : 
     146             :     // call attitude controller with conservative smoothing gain of 4.0f
     147      636619 :     attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target.x*100,
     148      636619 :                                                                   target.y*100,
     149      636619 :                                                                   target.z*100);
     150             : 
     151      636619 :     if (plane.control_mode == &plane.mode_qland) {
     152      267747 :         if (poscontrol.get_state() < QuadPlane::QPOS_LAND_FINAL && quadplane.check_land_final()) {
     153          17 :             poscontrol.set_state(QuadPlane::QPOS_LAND_FINAL);
     154          17 :             quadplane.setup_target_position();
     155             : #if AP_ICENGINE_ENABLED
     156             :             // cut IC engine if enabled
     157          17 :             if (quadplane.land_icengine_cut != 0) {
     158          17 :                 plane.g2.ice_control.engine_control(0, 0, 0, false);
     159             :             }
     160             : #endif  // AP_ICENGINE_ENABLED
     161             :         }
     162      267747 :         float height_above_ground = plane.relative_ground_altitude(RangeFinderUse::TAKEOFF_LANDING);
     163      267747 :         float descent_rate_ms = quadplane.landing_descent_rate_ms(height_above_ground);
     164             : 
     165      267747 :         if (poscontrol.get_state() == QuadPlane::QPOS_LAND_FINAL && !quadplane.option_is_set(QuadPlane::Option::DISABLE_GROUND_EFFECT_COMP)) {
     166       84044 :             ahrs.set_touchdown_expected(true);
     167             :         }
     168             : 
     169      267747 :         pos_control->D_set_pos_target_from_climb_rate_ms(-descent_rate_ms, descent_rate_ms>0);
     170      267747 :         quadplane.check_land_complete();
     171      368872 :     } else if (plane.control_mode == &plane.mode_guided && quadplane.guided_takeoff) {
     172           0 :         quadplane.set_climb_rate_ms(0);
     173             :     } else {
     174             :         // update altitude target and call position controller
     175      368872 :         quadplane.set_climb_rate_ms(quadplane.get_pilot_desired_climb_rate_cms() * 0.01);
     176             :     }
     177      636619 :     quadplane.run_z_controller();
     178             : 
     179             :     // Stabilize with fixed wing surfaces
     180      636619 :     plane.stabilize_roll();
     181      636619 :     plane.stabilize_pitch();
     182             : 
     183             :     // Center rudder
     184      636619 :     output_rudder_and_steering(0.0);
     185             : }
     186             : 
     187             : #endif