Browse Source

Rover: rename abs to true wind

mission-4.1.18
Peter Hall 6 years ago committed by Randy Mackay
parent
commit
412be730e4
  1. 4
      APMrover2/Log.cpp
  2. 9
      APMrover2/sailboat.cpp

4
APMrover2/Log.cpp

@ -133,12 +133,12 @@ void Rover::Log_Write_Sail() @@ -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,

9
APMrover2/sailboat.cpp

@ -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;

Loading…
Cancel
Save