Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : #if MODE_FOLLOW_ENABLED
       4             : 
       5             : /*
       6             :  * mode_follow.cpp - follow another mavlink-enabled vehicle by system id
       7             :  *
       8             :  * TODO: stick control to move around on sphere
       9             :  * TODO: stick control to change sphere diameter
      10             :  * TODO: "channel 7 option" to lock onto "pointed at" target
      11             :  * TODO: do better in terms of loitering around the moving point; may need a PID?  Maybe use loiter controller somehow?
      12             :  * TODO: extrapolate target vehicle position using its velocity and acceleration
      13             :  * TODO: ensure AP_AVOIDANCE_ENABLED is true because we rely on it velocity limiting functions
      14             :  */
      15             : 
      16             : // Return true if this mode is enabled, used by MAVLink available modes
      17       75230 : bool ModeFollow::enabled() const
      18             : {
      19       75230 :     return g2.follow.enabled();
      20             : }
      21             : 
      22             : // initialise follow mode
      23           7 : bool ModeFollow::init(const bool ignore_checks)
      24             : {
      25           7 :     if (!enabled()) {
      26           0 :         gcs().send_text(MAV_SEVERITY_WARNING, "Set FOLL_ENABLE = 1");
      27           0 :         return false;
      28             :     }
      29             : 
      30             : #if HAL_MOUNT_ENABLED
      31           7 :     AP_Mount *mount = AP_Mount::get_singleton();
      32             :     // follow the lead vehicle using sysid
      33           7 :     if (g2.follow.option_is_enabled(AP_Follow::Option::MOUNT_FOLLOW_ON_ENTER) && mount != nullptr) {
      34           0 :         mount->set_target_sysid(g2.follow.get_target_sysid());
      35             :     }
      36             : #endif
      37             : 
      38             :     // initialise horizontal speed, acceleration
      39           7 :     pos_control->NE_set_max_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      40           7 :     pos_control->NE_set_correction_speed_accel_m(wp_nav->get_default_speed_NE_ms(), wp_nav->get_wp_acceleration_mss());
      41             : 
      42             :     // initialize vertical speeds and acceleration
      43           7 :     pos_control->D_set_max_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
      44           7 :     pos_control->D_set_correction_speed_accel_m(wp_nav->get_default_speed_down_ms(), wp_nav->get_default_speed_up_ms(), wp_nav->get_accel_D_mss());
      45             : 
      46             :     // initialise velocity controller
      47           7 :     pos_control->D_init_controller();
      48           7 :     pos_control->NE_init_controller();
      49             : 
      50             :     // initialise yaw
      51           7 :     auto_yaw.set_mode_to_default(false);
      52             : 
      53           7 :     return true;
      54             : }
      55             : 
      56             : // perform cleanup required when leaving follow mode
      57           7 : void ModeFollow::exit()
      58             : {
      59           7 :     g2.follow.clear_offsets_if_required();
      60           7 : }
      61             : 
      62       16085 : void ModeFollow::run()
      63             : {
      64             :     // if not armed set throttle to zero and exit immediately
      65       16085 :     if (is_disarmed_or_landed()) {
      66         648 :         make_safe_ground_handling();
      67         648 :         return;
      68             :     }
      69             : 
      70             :     // Initialize follow offset if not yet set.
      71             :     // Prevents vehicle from starting directly on top of the lead vehicle.
      72       15437 :     g2.follow.init_offsets_if_required();
      73             : 
      74             :     // set motors to full range
      75       15437 :     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
      76             : 
      77       15437 :     float yaw_rad = attitude_control->get_att_target_euler_rad().z;
      78       15437 :     float yaw_rate_rads = 0.0f;
      79             : 
      80       15437 :     Vector3p pos_ofs_ned_m;  // vector to lead vehicle + offset
      81       15437 :     Vector3f vel_ofs_ned_ms;  // velocity of lead vehicle + offset
      82       15437 :     Vector3f accel_ofs_ned_mss;  // accel of lead vehicle + offset
      83       15437 :     if (g2.follow.get_ofs_pos_vel_accel_NED_m(pos_ofs_ned_m, vel_ofs_ned_ms, accel_ofs_ned_mss)) {
      84             : 
      85       15272 :         float target_heading_deg = 0.0f;
      86       15272 :         float target_heading_rate_degs = 0.0f;
      87       15272 :         g2.follow.get_target_heading_deg(target_heading_deg);
      88       15272 :         g2.follow.get_target_heading_rate_degs(target_heading_rate_degs);
      89             : 
      90       15272 :         pos_control->input_pos_vel_accel_NE_m(pos_ofs_ned_m.xy(), vel_ofs_ned_ms.xy(), accel_ofs_ned_mss.xy(), false);
      91             : 
      92       15272 :         float pos_ofs_d_m = pos_ofs_ned_m.z;
      93       15272 :         pos_control->input_pos_vel_accel_D_m(pos_ofs_d_m, vel_ofs_ned_ms.z, accel_ofs_ned_mss.z, false);
      94             : 
      95             :         // Determine desired yaw behavior based on configured follow mode
      96       15272 :         switch (g2.follow.get_yaw_behave()) {
      97       15272 :             case AP_Follow::YAW_BEHAVE_FACE_LEAD_VEHICLE: {
      98             :                 // Face the target directly
      99       15272 :                 Vector3p pos_ned_m;  // vector to lead vehicle
     100       15272 :                 Vector3f vel_ned_ms;  // velocity of lead vehicle
     101       15272 :                 Vector3f accel_ned_mss;  // accel of lead vehicle
     102       15272 :                 if (g2.follow.get_target_pos_vel_accel_NED_m(pos_ned_m, vel_ned_ms, accel_ned_mss)) {
     103       15272 :                     const Vector2p vec_to_lead_ne_m = pos_ned_m.xy() - pos_control->get_pos_target_NED_m().xy();
     104       15272 :                     if (vec_to_lead_ne_m.length_squared() > 1.0) {
     105       15241 :                         yaw_rad = vec_to_lead_ne_m.angle();
     106             :                     }
     107             :                 }
     108       15272 :                 break;
     109             :             }
     110             : 
     111           0 :             case AP_Follow::YAW_BEHAVE_SAME_AS_LEAD_VEHICLE: {
     112             :                 // Match the heading of the lead vehicle
     113           0 :                 yaw_rad = radians(target_heading_deg);
     114           0 :                 yaw_rate_rads = radians(target_heading_rate_degs);
     115           0 :                 break;
     116             :             }
     117             : 
     118           0 :             case AP_Follow::YAW_BEHAVE_DIR_OF_FLIGHT: {
     119             :                 // Face the direction of travel
     120           0 :                 if (vel_ofs_ned_ms.xy().length_squared() > 1.0) {
     121           0 :                     yaw_rad = vel_ofs_ned_ms.xy().angle();
     122             :                 }
     123           0 :                 break;
     124             :             }
     125             : 
     126           0 :             case AP_Follow::YAW_BEHAVE_NONE:
     127             :             default:
     128             :                 // do nothing
     129           0 :                break;
     130             : 
     131             :         }
     132             :     } else {
     133             :         // Target data is invalid; hold position using zero velocity and acceleration inputs
     134         165 :         Vector2f vel_ne_zero;
     135         165 :         Vector2f accel_ne_zero;
     136         165 :         pos_control->input_vel_accel_NE_m(vel_ne_zero, accel_ne_zero, false);
     137         165 :         float vel_d_zero = 0.0;
     138         165 :         pos_control->input_vel_accel_D_m(vel_d_zero, 0.0, false);
     139         165 :         yaw_rate_rads = 0.0f;
     140             :     }
     141             : 
     142             :     // update the position controller
     143       15437 :     pos_control->NE_update_controller();
     144       15437 :     pos_control->D_update_controller();
     145             : 
     146             :     // call attitude controller
     147       15437 :     attitude_control->input_thrust_vector_heading_rad(pos_control->get_thrust_vector(), yaw_rad, yaw_rate_rads);
     148             : }
     149             : 
     150         243 : float ModeFollow::wp_distance_m() const
     151             : {
     152         243 :     return g2.follow.get_distance_to_target_m();
     153             : }
     154             : 
     155         243 : float ModeFollow::wp_bearing_deg() const
     156             : {
     157         243 :     return g2.follow.get_bearing_to_target_deg();
     158             : }
     159             : 
     160             : // Returns target location with offset applied, for MAVLink reporting
     161         201 : bool ModeFollow::get_wp(Location &loc) const
     162             : {
     163         201 :     Vector3f vel_ned_ms;
     164         201 :     return g2.follow.get_target_location_and_velocity_ofs(loc, vel_ned_ms);
     165             : }
     166             : 
     167             : #endif // MODE_FOLLOW_ENABLED