diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index c68626e01a..3cfa25aa58 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -133,12 +133,12 @@ void Rover::Log_Write_Sail() float wind_speed_true = logger.quiet_nanf(); float wind_speed_apparent = logger.quiet_nanf(); if (rover.g2.windvane.enabled()) { - wind_dir_abs = degrees(g2.windvane.get_absolute_wind_direction_rad()); + wind_dir_abs = degrees(g2.windvane.get_true_wind_direction_rad()); wind_dir_rel = degrees(g2.windvane.get_apparent_wind_direction_rad()); wind_speed_true = g2.windvane.get_true_wind_speed(); wind_speed_apparent = g2.windvane.get_apparent_wind_speed(); } - logger.Write("SAIL", "TimeUS,WindDirAbs,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG", + logger.Write("SAIL", "TimeUS,WindDirTrue,WindDirApp,WindSpdTrue,WindSpdApp,SailOut,VMG", "shhnn%n", "F000000", "Qffffff", AP_HAL::micros64(), (double)wind_dir_abs, diff --git a/APMrover2/sailboat.cpp b/APMrover2/sailboat.cpp index 02f8ffe734..da3ab164b2 100644 --- a/APMrover2/sailboat.cpp +++ b/APMrover2/sailboat.cpp @@ -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 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) } // 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;