Line data    Source code

       1             : #include "Copter.h"
       2             : 
       3             : Mode::AutoYaw Mode::auto_yaw;
       4             : 
       5             : // roi_yaw_rad - returns heading towards location held in roi_ned_m
       6       14717 : float Mode::AutoYaw::roi_yaw_rad() const
       7             : {
       8       14717 :     Vector2f pos_ne_m;
       9       14717 :     if (AP::ahrs().get_relative_position_NE_origin_float(pos_ne_m)){
      10       14717 :         return get_bearing_rad(pos_ne_m, roi_ned_m.xy());
      11             :     }
      12           0 :     return copter.attitude_control->get_att_target_euler_rad().z;
      13             : }
      14             : 
      15             : // Returns the yaw angle (in radians) representing the direction of horizontal motion.
      16      394024 : float Mode::AutoYaw::look_ahead_yaw_rad()
      17             : {
      18             :     // Commanded Yaw to automatically look ahead.
      19      394024 :     Vector3f vel_ned_ms;
      20      394024 :     if (copter.position_ok() && AP::ahrs().get_velocity_NED(vel_ned_ms)) {
      21      394024 :         const float speed_ms_sq = vel_ned_ms.xy().length_squared();
      22      394024 :         if (speed_ms_sq > (YAW_LOOK_AHEAD_MIN_SPEED_MS * YAW_LOOK_AHEAD_MIN_SPEED_MS)) {
      23      334536 :             _look_ahead_yaw_rad = atan2f(vel_ned_ms.y,vel_ned_ms.x);
      24             :         }
      25             :     }
      26      394024 :     return _look_ahead_yaw_rad;
      27             : }
      28             : 
      29        2055 : void Mode::AutoYaw::set_mode_to_default(bool rtl)
      30             : {
      31        2055 :     set_mode(default_mode(rtl));
      32        2055 : }
      33             : 
      34             : // default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
      35             : // set rtl parameter to true if this is during an RTL
      36        2328 : Mode::AutoYaw::Mode Mode::AutoYaw::default_mode(bool rtl) const
      37             : {
      38        2328 :     switch ((Copter::WPYawBehavior)copter.g.wp_yaw_behavior) {
      39             : 
      40          56 :     case Copter::WPYawBehavior::NONE:
      41          56 :         return Mode::HOLD;
      42             : 
      43        2175 :     case Copter::WPYawBehavior::LOOK_AT_NEXT_WP_EXCEPT_RTL:
      44        2175 :         if (rtl) {
      45         524 :             return Mode::HOLD;
      46             :         } else {
      47        1651 :             return Mode::LOOK_AT_NEXT_WP;
      48             :         }
      49             : 
      50          79 :     case Copter::WPYawBehavior::LOOK_AHEAD:
      51          79 :         return Mode::LOOK_AHEAD;
      52             : 
      53          18 :     case Copter::WPYawBehavior::LOOK_AT_NEXT_WP:
      54             :     default:
      55          18 :         return Mode::LOOK_AT_NEXT_WP;
      56             :     }
      57             : }
      58             : 
      59             : // set_mode - sets the yaw mode for auto
      60       54423 : void Mode::AutoYaw::set_mode(Mode yaw_mode)
      61             : {
      62             :     // return immediately if no change
      63       54423 :     if (_mode == yaw_mode) {
      64       53572 :         return;
      65             :     }
      66         851 :     _last_mode = _mode;
      67         851 :     _mode = yaw_mode;
      68             : 
      69             :     // perform initialisation
      70         851 :     switch (_mode) {
      71             : 
      72         509 :     case Mode::HOLD:
      73         509 :         break;
      74             : 
      75         196 :     case Mode::LOOK_AT_NEXT_WP:
      76             :         // wpnav will initialise heading when wpnav's set_destination method is called
      77         196 :         break;
      78             : 
      79          10 :     case Mode::ROI:
      80             :         // look ahead until we know otherwise
      81          10 :         break;
      82             : 
      83          31 :     case Mode::FIXED:
      84             :         // keep heading pointing in the direction held in fixed_yaw
      85             :         // caller should set the fixed_yaw
      86          31 :         break;
      87             : 
      88          23 :     case Mode::LOOK_AHEAD:
      89             :         // Commanded Yaw to automatically look ahead.
      90          23 :         _look_ahead_yaw_rad = copter.ahrs.get_yaw_rad();
      91          23 :         break;
      92             : 
      93          19 :     case Mode::RESET_TO_ARMED_YAW:
      94             :         // initial_armed_bearing_rad will be set during arming so no init required
      95          19 :         break;
      96             : 
      97          21 :     case Mode::ANGLE_RATE:
      98          21 :         break;
      99             : 
     100          12 :     case Mode::RATE:
     101             :         // initialise target yaw rate to zero
     102          12 :         _yaw_rate_rads = 0.0;
     103          12 :         break;
     104             : 
     105          30 :     case Mode::CIRCLE:
     106             :     case Mode::PILOT_RATE:
     107             :     case Mode::WEATHERVANE:
     108             :         // no initialisation required
     109          30 :         break;
     110             :     }
     111             : }
     112             : 
     113             : // set_fixed_yaw_rad - sets the yaw look at heading for auto mode
     114         129 : void Mode::AutoYaw::set_fixed_yaw_rad(float yaw_rad, float yaw_rate_rads, int8_t direction, bool relative_angle)
     115             : {
     116         129 :     _last_update_ms = millis();
     117         129 :     const float angle_rad = yaw_rad;
     118             : 
     119             :     // calculate final angle as relative to vehicle heading or absolute
     120         129 :     if (relative_angle) {
     121          95 :         if (_mode == Mode::HOLD) {
     122           2 :             _yaw_angle_rad = copter.ahrs.get_yaw_rad();
     123             :         }
     124          95 :         _fixed_yaw_offset_rad = angle_rad * (direction >= 0 ? 1.0 : -1.0);
     125             :     } else {
     126             :         // absolute angle
     127          34 :         _fixed_yaw_offset_rad = wrap_PI(angle_rad - _yaw_angle_rad);
     128          34 :         if (direction < 0 && is_positive(_fixed_yaw_offset_rad)) {
     129           0 :             _fixed_yaw_offset_rad -= M_2PI;
     130          34 :         } else if (direction > 0 && is_negative(_fixed_yaw_offset_rad)) {
     131           4 :             _fixed_yaw_offset_rad += M_2PI;
     132             :         }
     133             :     }
     134             : 
     135             :     // get turn speed
     136         129 :     if (!is_positive(yaw_rate_rads)) {
     137             :         // default to default slew rate
     138          90 :         _fixed_yaw_slewrate_rads = copter.attitude_control->get_slew_yaw_max_rads();
     139             :     } else {
     140          39 :         _fixed_yaw_slewrate_rads = MIN(copter.attitude_control->get_slew_yaw_max_rads(), yaw_rate_rads);
     141             :     }
     142             : 
     143             :     // set yaw mode
     144         129 :     set_mode(Mode::FIXED);
     145         129 : }
     146             : 
     147             : // set_fixed_yaw_rad - sets the yaw look at heading for auto mode
     148         906 : void Mode::AutoYaw::set_yaw_angle_and_rate_rad(float yaw_angle_rad, float yaw_rate_rads)
     149             : {
     150         906 :     _last_update_ms = millis();
     151             : 
     152         906 :     _yaw_angle_rad = yaw_angle_rad;
     153         906 :     _yaw_rate_rads = yaw_rate_rads;
     154             : 
     155             :     // set yaw mode
     156         906 :     set_mode(Mode::ANGLE_RATE);
     157         906 : }
     158             : 
     159             : // set_yaw_angle_offset_deg - sets the yaw look at heading for auto mode, as an offset from the current yaw angle
     160           3 : void Mode::AutoYaw::set_yaw_angle_offset_deg(const float yaw_angle_offset_deg)
     161             : {
     162           3 :     _last_update_ms = millis();
     163             : 
     164           3 :     _yaw_angle_rad = wrap_2PI(_yaw_angle_rad + radians(yaw_angle_offset_deg));
     165           3 :     _yaw_rate_rads = 0.0f;
     166             : 
     167             :     // set yaw mode
     168           3 :     set_mode(Mode::ANGLE_RATE);
     169           3 : }
     170             : 
     171             : // set_roi - sets the yaw to look at roi_ned_m for auto mode
     172          52 : void Mode::AutoYaw::set_roi(const Location &roi_location)
     173             : {
     174             :     // if location is zero lat, lon and altitude turn off ROI
     175          52 :     if (!roi_location.initialised()) {
     176             :         // set auto yaw mode back to default assuming the active command is a waypoint command.  A more sophisticated method is required to ensure we return to the proper yaw control for the active command
     177          18 :         auto_yaw.set_mode_to_default(false);
     178             : #if HAL_MOUNT_ENABLED
     179             :         // switch off the camera tracking if enabled
     180          18 :         copter.camera_mount.clear_roi_target();
     181             : #endif  // HAL_MOUNT_ENABLED
     182             :     } else {
     183             : #if HAL_MOUNT_ENABLED
     184             :         // check if mount type requires us to rotate the quad
     185          34 :         if (!copter.camera_mount.has_pan_control()) {
     186          14 :             if (roi_location.get_vector_from_origin_NED_m(roi_ned_m)) {
     187          14 :                 auto_yaw.set_mode(Mode::ROI);
     188             :             }
     189             :         }
     190             :         // send the command to the camera mount
     191          34 :         copter.camera_mount.set_roi_target(roi_location);
     192             : 
     193             :         // TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink.  Currently we only handle mode 4 (see below)
     194             :         //      0: do nothing
     195             :         //      1: point at next waypoint
     196             :         //      2: point at a waypoint taken from WP# parameter (2nd parameter?)
     197             :         //      3: point at a location given by alt, lon, lat parameters
     198             :         //      4: point at a target given a target id (can't be implemented)
     199             : #else
     200             :         // if we have no camera mount aim the quad at the location
     201             :         if (roi_location.get_vector_from_origin_NED_m(roi_ned_m)) {
     202             :             auto_yaw.set_mode(Mode::ROI);
     203             :         }
     204             : #endif  // HAL_MOUNT_ENABLED
     205             :     }
     206          52 : }
     207             : 
     208             : // set auto yaw rate in radians per second
     209        1162 : void Mode::AutoYaw::set_rate_rad(float turn_rate_rads)
     210             : {
     211        1162 :     set_mode(Mode::RATE);
     212        1162 :     _yaw_rate_rads = turn_rate_rads;
     213        1162 : }
     214             : 
     215             : // return true if fixed yaw target has been reached
     216       10225 : bool Mode::AutoYaw::reached_fixed_yaw_target()
     217             : {
     218       10225 :     if (mode() != Mode::FIXED) {
     219             :         // should not happen, not in the right mode
     220           0 :         return true;
     221             :     }
     222             : 
     223       10225 :     if (!is_zero(_fixed_yaw_offset_rad)) {
     224             :         // still slewing yaw target
     225       10225 :         return false;
     226             :     }
     227             : 
     228             :     // Within 2 deg of target
     229           0 :     return (fabsf(wrap_PI(_yaw_angle_rad - copter.ahrs.get_yaw_rad())) <= radians(2));
     230             : }
     231             : 
     232             : // yaw_rad - returns target heading depending upon auto_yaw.mode()
     233    11832320 : float Mode::AutoYaw::yaw_rad()
     234             : {
     235    11832320 :     switch (_mode) {
     236             : 
     237       14717 :     case Mode::ROI:
     238             :         // point towards a location held in roi_ned_m
     239       14717 :         _yaw_angle_rad = roi_yaw_rad();
     240       14717 :         break;
     241             : 
     242      247345 :     case Mode::FIXED: {
     243             :         // keep heading pointing in the direction held in fixed_yaw
     244             :         // with no pilot input allowed
     245      247345 :         const uint32_t now_ms = millis();
     246      247345 :         float dt = (now_ms - _last_update_ms) * 0.001;
     247      247345 :         _last_update_ms = now_ms;
     248      247345 :         float yaw_angle_step_rad = constrain_float(_fixed_yaw_offset_rad, - dt * _fixed_yaw_slewrate_rads, dt * _fixed_yaw_slewrate_rads);
     249      247345 :         _fixed_yaw_offset_rad -= yaw_angle_step_rad;
     250      247345 :         _yaw_angle_rad += yaw_angle_step_rad;
     251      247345 :         break;
     252             :     }
     253             : 
     254      394024 :     case Mode::LOOK_AHEAD:
     255             :         // Commanded Yaw to automatically look ahead.
     256      394024 :         _yaw_angle_rad = look_ahead_yaw_rad();
     257      394024 :         break;
     258             : 
     259       38022 :     case Mode::RESET_TO_ARMED_YAW:
     260             :         // changes yaw to be same as when quad was armed
     261       38022 :         _yaw_angle_rad = copter.initial_armed_bearing_rad;
     262       38022 :         break;
     263             : 
     264      156551 :     case Mode::CIRCLE:
     265             : #if MODE_CIRCLE_ENABLED
     266      156551 :         if (copter.circle_nav->is_active()) {
     267             :             // standalone Circle mode provides its own yaw
     268      109881 :             _yaw_angle_rad = copter.circle_nav->get_yaw_rad();
     269      109881 :             break;
     270             :         }
     271       46670 :         if (!copter.circle_nav->face_direction_of_travel()) {
     272             :             // Auto's S-curve circle: face the circle center held by circle_nav, the parameter
     273             :             // store the orbit leg was built from.  The position target is used rather than the
     274             :             // estimate so the yaw target carries no position noise and does not lag by the
     275             :             // tracking error (AC_Circle does the same).  If the target is exactly at the
     276             :             // center, the last yaw target is held
     277       46670 :             const Vector2f pos_to_center_ne = (copter.circle_nav->get_center_NED_m().xy() - copter.pos_control->get_pos_desired_NED_m().xy()).tofloat();
     278       46670 :             if (!pos_to_center_ne.is_zero()) {
     279       46670 :                 _yaw_angle_rad = pos_to_center_ne.angle();
     280             :             }
     281       46670 :             break;
     282             :         }
     283             :         {
     284             :             // FACE_DIRECTION_OF_TRAVEL option set: yaw follows the desired direction of travel.
     285             :             // The last yaw target is held while the desired velocity is too small to define one
     286           0 :             const Vector2f vel_desired_ne_ms = copter.pos_control->get_vel_desired_NED_ms().xy();
     287           0 :             if (vel_desired_ne_ms.length() > 0.1f) {
     288           0 :                 _yaw_angle_rad = vel_desired_ne_ms.angle();
     289             :             }
     290             :         }
     291             : #else
     292             :         // circle mode compiled out: yaw follows the position controller's travel direction
     293             :         _yaw_angle_rad = copter.pos_control->get_yaw_rad();
     294             : #endif
     295           0 :         break;
     296             : 
     297       92231 :     case Mode::ANGLE_RATE:{
     298       92231 :         const uint32_t now_ms = millis();
     299       92231 :         float dt = (now_ms - _last_update_ms) * 0.001;
     300       92231 :         _last_update_ms = now_ms;
     301       92231 :         _yaw_angle_rad += _yaw_rate_rads * dt;
     302       92231 :         break;
     303             :     }
     304             : 
     305      160246 :     case Mode::RATE:
     306             :     case Mode::WEATHERVANE:
     307             :     case Mode::PILOT_RATE:
     308      160246 :         _yaw_angle_rad = copter.attitude_control->get_att_target_euler_rad().z;
     309      160246 :         break;
     310             : 
     311    10729184 :     case Mode::LOOK_AT_NEXT_WP:
     312             :     default:
     313             :         // point towards next waypoint.
     314             :         // we don't use wp_bearing_deg because we don't want the copter to turn too much during flight
     315    10729184 :         _yaw_angle_rad = copter.pos_control->get_yaw_rad();
     316    10729184 :     break;
     317             :     }
     318    11832320 :     return _yaw_angle_rad;
     319             : }
     320             : 
     321             : // returns yaw rate normally set by SET_POSITION_TARGET mavlink
     322             : // messages (positive is clockwise, negative is counter clockwise)
     323    11832320 : float Mode::AutoYaw::rate_rads()
     324             : {
     325    11832320 :     switch (_mode) {
     326             : 
     327     9788274 :     case Mode::HOLD:
     328             :     case Mode::ROI:
     329             :     case Mode::FIXED:
     330             :     case Mode::LOOK_AHEAD:
     331             :     case Mode::RESET_TO_ARMED_YAW:
     332             :     case Mode::CIRCLE:
     333     9788274 :         _yaw_rate_rads = 0.0f;
     334     9788274 :         break;
     335             : 
     336     1791569 :     case Mode::LOOK_AT_NEXT_WP:
     337     1791569 :         _yaw_rate_rads = copter.pos_control->get_yaw_rate_rads();
     338     1791569 :         break;
     339             : 
     340       14769 :     case Mode::PILOT_RATE:
     341       14769 :         _yaw_rate_rads = _pilot_yaw_rate_rads;
     342       14769 :         break;
     343             : 
     344      237708 :     case Mode::ANGLE_RATE:
     345             :     case Mode::RATE:
     346             :     case Mode::WEATHERVANE:
     347      237708 :         break;
     348             :     }
     349             : 
     350             :     // return zero turn rate (this should never happen)
     351    11832320 :     return _yaw_rate_rads;
     352             : }
     353             : 
     354    11832320 : AC_AttitudeControl::HeadingCommand Mode::AutoYaw::get_heading()
     355             : {
     356             :     // process pilot's yaw input
     357    11832320 :     _pilot_yaw_rate_rads = 0.0;
     358    11832320 :     if (rc().has_valid_input() && copter.flightmode->use_pilot_yaw()) {
     359             :         // get pilot's desired yaw rate
     360    11472622 :         _pilot_yaw_rate_rads = copter.flightmode->get_pilot_desired_yaw_rate_rads();
     361    11472622 :         if (!is_zero(_pilot_yaw_rate_rads)) {
     362       14769 :             auto_yaw.set_mode(AutoYaw::Mode::PILOT_RATE);
     363             :         }
     364      359698 :     } else if (auto_yaw.mode() == AutoYaw::Mode::PILOT_RATE) {
     365             :         // RC failsafe, or disabled make sure not in pilot control
     366           0 :         auto_yaw.set_mode(AutoYaw::Mode::HOLD);
     367             :     }
     368             : 
     369             : #if WEATHERVANE_ENABLED
     370    11832320 :     update_weathervane(_pilot_yaw_rate_rads);
     371             : #endif
     372             : 
     373             :     AC_AttitudeControl::HeadingCommand heading;
     374    11832320 :     heading.yaw_angle_rad = auto_yaw.yaw_rad();
     375    11832320 :     heading.yaw_rate_rads = auto_yaw.rate_rads();
     376             : 
     377    11832320 :     switch (auto_yaw.mode()) {
     378     9097861 :         case Mode::HOLD:
     379             :         case Mode::RATE:
     380             :         case Mode::PILOT_RATE:
     381             :         case Mode::WEATHERVANE:
     382     9097861 :             heading.heading_mode = AC_AttitudeControl::HeadingMode::Rate_Only;
     383     9097861 :             break;
     384     2734459 :         case Mode::LOOK_AT_NEXT_WP:
     385             :         case Mode::ROI:
     386             :         case Mode::FIXED:
     387             :         case Mode::LOOK_AHEAD:
     388             :         case Mode::RESET_TO_ARMED_YAW:
     389             :         case Mode::ANGLE_RATE:
     390             :         case Mode::CIRCLE:
     391     2734459 :             heading.heading_mode = AC_AttitudeControl::HeadingMode::Angle_And_Rate;
     392     2734459 :             break;
     393             :     }
     394             : 
     395    11832320 :     return heading;
     396             : }
     397             : 
     398             : // handle the interface to the weathervane library
     399             : // pilot_yaw can be an angle or a rate or rcin from yaw channel. It just needs to represent a pilot's request to yaw the vehicle to enable pilot overrides.
     400             : #if WEATHERVANE_ENABLED
     401    11832320 : void Mode::AutoYaw::update_weathervane(const float pilot_yaw_rads)
     402             : {
     403             :     float yaw_rate_cds;
     404    11868839 :     if (copter.flightmode->allows_weathervaning() &&
     405       36519 :         copter.g2.weathervane.get_yaw_out(yaw_rate_cds, rad_to_cd(pilot_yaw_rads), copter.flightmode->get_alt_above_ground_m(),
     406       36519 :                                                                        copter.pos_control->get_roll_cd()-copter.attitude_control->get_roll_trim_cd(),
     407       36519 :                                                                        copter.pos_control->get_pitch_cd(),
     408       36519 :                                                                        copter.flightmode->is_taking_off(),
     409       36519 :                                                                        copter.flightmode->is_landing())) {
     410       23914 :         set_mode(Mode::WEATHERVANE);
     411       23914 :         _yaw_rate_rads = cd_to_rad(yaw_rate_cds);
     412       23914 :         return;
     413             :     }
     414             : 
     415             :     // Weathervane not allowed in current mode, or weathervane thresholds not met
     416             : 
     417             :     // if the weathervane controller has previously been activated we need to ensure we return control back to what was previously set
     418    11808406 :     if (mode() == Mode::WEATHERVANE) {
     419           1 :         _yaw_rate_rads = 0.0;
     420           1 :         if (_last_mode == Mode::HOLD) {
     421           0 :             set_mode_to_default(false);
     422             :         } else {
     423           1 :             set_mode(_last_mode);
     424             :         }
     425             :     }
     426             : }
     427             : #endif // WEATHERVANE_ENABLED