// 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;
}
}
