|
|
|
@ -242,7 +242,7 @@ void Sailboat::handle_tack_request_acro()
@@ -242,7 +242,7 @@ void Sailboat::handle_tack_request_acro()
|
|
|
|
|
} |
|
|
|
|
// set tacking heading target to the current angle relative to the true wind but on the new tack
|
|
|
|
|
currently_tacking = true; |
|
|
|
|
tack_heading_rad = wrap_2PI(rover.ahrs.yaw + 2.0f * wrap_PI((rover.g2.windvane.get_absolute_wind_direction_rad() - rover.ahrs.yaw))); |
|
|
|
|
tack_heading_rad = wrap_2PI(rover.ahrs.yaw + 2.0f * wrap_PI((rover.g2.windvane.get_true_wind_direction_rad() - rover.ahrs.yaw))); |
|
|
|
|
|
|
|
|
|
auto_tack_request_ms = AP_HAL::millis(); |
|
|
|
|
} |
|
|
|
@ -290,7 +290,7 @@ bool Sailboat::use_indirect_route(float desired_heading_cd) const
@@ -290,7 +290,7 @@ bool Sailboat::use_indirect_route(float desired_heading_cd) const
|
|
|
|
|
const float desired_heading_rad = radians(desired_heading_cd * 0.01f); |
|
|
|
|
|
|
|
|
|
// check if desired heading is in the no go zone, if it is we can't go direct
|
|
|
|
|
return fabsf(wrap_PI(rover.g2.windvane.get_absolute_wind_direction_rad() - desired_heading_rad)) <= radians(sail_no_go); |
|
|
|
|
return fabsf(wrap_PI(rover.g2.windvane.get_true_wind_direction_rad() - desired_heading_rad)) <= radians(sail_no_go); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// if we can't sail on the desired heading then we should pick the best heading that we can sail on
|
|
|
|
@ -312,8 +312,9 @@ float Sailboat::calc_heading(float desired_heading_cd)
@@ -312,8 +312,9 @@ float Sailboat::calc_heading(float desired_heading_cd)
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// calculate left and right no go headings looking upwind
|
|
|
|
|
const float left_no_go_heading_rad = wrap_2PI(rover.g2.windvane.get_absolute_wind_direction_rad() + radians(sail_no_go)); |
|
|
|
|
const float right_no_go_heading_rad = wrap_2PI(rover.g2.windvane.get_absolute_wind_direction_rad() - radians(sail_no_go)); |
|
|
|
|
const float true_wind_rad = rover.g2.windvane.get_true_wind_direction_rad(); |
|
|
|
|
const float left_no_go_heading_rad = wrap_2PI(true_wind_rad + radians(sail_no_go)); |
|
|
|
|
const float right_no_go_heading_rad = wrap_2PI(true_wind_rad - radians(sail_no_go)); |
|
|
|
|
|
|
|
|
|
// calculate current tack, Port if heading is left of no-go, STBD if right of no-go
|
|
|
|
|
Sailboat_Tack current_tack; |
|
|
|
|