def get_heading_error(self, current_heading, desired_heading,
apparent_wind_angle):
"""
Calculates the heading error. The heading error varies depending on the current control
mode of the boat.
Arguments
---------
float : current_heading
The current direction of the boat in radians.
float : desired_heading
The desired direction of the boat in radians.
float : apparent_wind_angle
The apparent wind angle in radians.
Returns
-------
float
Returns the heading error depending on the current control mode in radians.
"""
if (self.__controlModeID == ControlModes.JIBE_ONLY.value):
jibe_direction = JibeOnlyRudderController.get_jibe_controller_direction(
current_heading=current_heading,
desired_heading=desired_heading,
apparent_wind_angle=apparent_wind_angle)
error = JibeOnlyRudderController.get_jibe_controller_error(
current_heading=current_heading,
desired_heading=desired_heading,
jibe_direction=jibe_direction)
return error
# TACKABLE
elif (self.__controlModeID == ControlModes.TACKABLE.value):
error = TackController.get_heading_error_tackable(
setPoint=desired_heading, measure=current_heading)
return error
# LOW POWER or UNKNOWN
else:
if (ControllerOutputRefiner.lowPowerAngle(
inputSignal=desired_heading, currAngle=current_heading)):
error = TackController.get_heading_error_tackable(
setPoint=desired_heading, measure=current_heading)
return error
else:
return 0
def test_jibe_error_function(self):
mock_speed = sailbot_constants.SPEED_THRESHOLD_FOR_JIBING_KNOTS - 0.1
# Enter JIBE_ONLY mode from UNKNOWN
hc = HeadingController(mock_speed, ControlModes.UNKNOWN.value)
self.assertEqual(hc.getControlModeID(), ControlModes.JIBE_ONLY.value)
mock_current_heading = -math.pi
mock_desired_heading = math.pi / 2
mock_wind_angle = math.pi / 4
jibe_direction = JibeOnlyRudderController.get_jibe_controller_direction(
mock_current_heading, mock_desired_heading, mock_wind_angle)
# Error function in heading controller should be the same as jibe controller error functions
self.assertAlmostEqual(
hc.get_heading_error(mock_current_heading, mock_desired_heading,
mock_wind_angle),
JibeOnlyRudderController.get_jibe_controller_error(
mock_current_heading, mock_desired_heading, jibe_direction))
def test_getJibeDirection_Same_Opp(self):
self.assertEqual(
JibeOnlyRudderController.get_jibe_controller_direction(
-math.pi / 100, math.pi / 100, math.pi / 4), 1)
def test_getJibeError_wrap(self):
self.assertAlmostEqual(
JibeOnlyRudderController.get_jibe_controller_error(
math.pi / 2 + 2 * math.pi, -math.pi / 2 + 2 * math.pi, -1),
-math.pi)
def test_getJibeDirection_wrap(self):
self.assertEqual(
JibeOnlyRudderController.get_jibe_controller_direction(
math.pi / 2 + 2 * math.pi, -math.pi / 2 + 2 * math.pi,
-math.pi / 2 + 2 * math.pi), 1)
def test_getJibeError_smallAngle(self):
self.assertAlmostEqual(
JibeOnlyRudderController.get_jibe_controller_error(
math.pi / 100, -math.pi / 100, 1), (99 / 100) * 2 * math.pi)
def test_getJibeDirection_smallAngle(self):
self.assertEqual(
JibeOnlyRudderController.get_jibe_controller_direction(
math.pi / 100, -math.pi / 100, math.pi * 99 / 100), -1)
def test_getJibeError_normalCaseInverse(self):
self.assertAlmostEqual(
JibeOnlyRudderController.get_jibe_controller_error(
math.pi / 2, -math.pi / 2, 1), math.pi)
def test_getJibeDirection_normalCaseInverse(self):
self.assertEqual(
JibeOnlyRudderController.get_jibe_controller_direction(
math.pi / 2, -math.pi / 2, math.pi / 2), -1)