// get_auto_heading - returns target heading depending upon auto_yaw_mode // 100hz update rate float Copter::get_auto_heading(void) { switch(auto_yaw_mode) { case AUTO_YAW_ROI: // point towards a location held in roi_WP return get_roi_yaw(); break; case AUTO_YAW_LOOK_AT_HEADING: // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed return yaw_look_at_heading; break; case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. return get_look_ahead_yaw(); break; case AUTO_YAW_RESETTOARMEDYAW: // changes yaw to be same as when quad was armed return initial_armed_bearing; break; case AUTO_YAW_LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the copter to turn too much during flight return wp_nav.get_yaw(); break; } }
// get_auto_heading - returns target heading depending upon auto_yaw_mode // 100hz update rate float Sub::get_auto_heading(void) { switch (auto_yaw_mode) { case AUTO_YAW_ROI: // point towards a location held in roi_WP return get_roi_yaw(); break; case AUTO_YAW_LOOK_AT_HEADING: // keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed return yaw_look_at_heading; break; case AUTO_YAW_LOOK_AHEAD: // Commanded Yaw to automatically look ahead. return get_look_ahead_yaw(); break; case AUTO_YAW_RESETTOARMEDYAW: // changes yaw to be same as when quad was armed return initial_armed_bearing; break; case AUTO_YAW_CORRECT_XTRACK: { // TODO return current yaw if not in appropriate mode // Bearing of current track (centidegrees) float track_bearing = wp_nav.get_bearing_cd(wp_nav.get_wp_origin(), wp_nav.get_wp_destination()); // Bearing from current position towards intermediate position target (centidegrees) float desired_angle = wp_nav.get_loiter_bearing_to_target(); float angle_error = wrap_180_cd(desired_angle - track_bearing); float angle_limited = constrain_float(angle_error, -g.xtrack_angle_limit * 100.0f, g.xtrack_angle_limit * 100.0f); return wrap_360_cd(track_bearing + angle_limited); } break; case AUTO_YAW_LOOK_AT_NEXT_WP: default: // point towards next waypoint. // we don't use wp_bearing because we don't want the vehicle to turn too much during flight return wp_nav.get_yaw(); break; } }