def _tack(self, requested_heading, wind_course_angle, for_seconds):
deflection = self._initial_tack_deflection(wind_course_angle)
leg1, leg2 = self._leg_times(deflection, for_seconds)
first_tack_course = requested_heading + deflection
if leg2 <= self.config['min tack duration']:
self.helm.steer_course(first_tack_course, for_seconds,self.no_go)
else:
self.helm.steer_course(to_360(first_tack_course), leg1,self.no_go)
self.helm.steer_course(to_360(first_tack_course - copysign(2 * self.no_go, deflection)), leg2,self.no_go)
def _start_tack(self, requested_heading, wind_course_angle, time_to_next_review):
deflection = self._initial_tack_deflection(wind_course_angle)
leg1, leg2 = self._leg_times(deflection, time_to_next_review)
first_tack_course = to_360(requested_heading + deflection)
second_tack_course = to_360(first_tack_course - copysign(2 * self.no_go, deflection))
self.logger.debug('SailingHelm: starting tack on {:5.1f} for {:5.0f} seconds'.format(first_tack_course, leg1))
if leg2 <= self.config['min tack duration']:
self.exchange.publish(Event(EventName.tack, heading=first_tack_course))
else:
second_tack_event = Event(EventName.tack, heading = second_tack_course)
self.exchange.publish(Event(EventName.tack, heading=first_tack_course))
self.exchange.publish(Event(EventName.after, seconds = leg1, next_event= second_tack_event))
def rotate_boat(self, rudder_effect, jitter):
self.compass.bearing = to_360(self.compass.bearing +
randint(-jitter, jitter) -
self.rudder_servo.get_position() *
rudder_effect)
self.time += 0.2
self.exchange.publish(Event(EventName.tick)) # for compass smoothing!
self.exchange.publish(Event(EventName.update_averages))
def _turn(self,distance):
turn_radius = self._turn_radius(self.rudder.angle)
track_delta = - copysign(self._track_delta(distance, turn_radius),self.rudder.angle)
new_track = to_360(self.track + track_delta)
move_bearing = self.track + self._straightline_angle(track_delta)
move_distance = self._straightline_distance(turn_radius,track_delta)
new_position = self.globe.new_position(self.position,move_bearing,move_distance)
self._set_position(new_position,new_track)
def _turn(self, distance):
turn_radius = self._turn_radius(self.rudder.angle)
track_delta = -copysign(self._track_delta(distance, turn_radius),
self.rudder.angle)
new_track = to_360(self.track + track_delta)
move_bearing = self.track + self._straightline_angle(track_delta)
move_distance = self._straightline_distance(turn_radius, track_delta)
new_position = self.globe.new_position(self.position, move_bearing,
move_distance)
self._set_position(new_position, new_track)
def __init__(self,gps,wind_direction,compass_heading):
self.gps = gps
self.time = 0
self.speed = 1
self.track_error = 1
self.speed_error = 0.1
self.rate_of_turn = 1
self.rate_of_turn_average = 1
self.wind_direction_abs_average = wind_direction
self.wind_direction_relative_instant = to_360(angle_between(wind_direction,compass_heading))
self.wind_direction_relative_average = self.wind_direction_relative_instant
self.compass_heading_average = compass_heading
self.compass_heading_smoothed = compass_heading
def __init__(self, gps, wind_direction, compass_heading):
self.gps = gps
self.time = 0
self.speed = 1
self.track_error = 1
self.speed_error = 0.1
self.rate_of_turn = 1
self.rate_of_turn_average = 1
self.wind_direction_abs_average = wind_direction
self.wind_direction_relative_instant = to_360(
angle_between(wind_direction, compass_heading))
self.wind_direction_relative_average = self.wind_direction_relative_instant
self.compass_heading_average = compass_heading
self.compass_heading_smoothed = compass_heading
def test_should_turn_a_corner_if_the_rudder_is_deflected(self):
bearing,time_s = 30,2
starting_position = Position(53,-2)
rudder_deflection = 10
gps = SimulatedGPS(starting_position,bearing,INITIAL_SPEED_IN_MS,True)
vehicle = SimulatedVehicle(gps,self.globe,self.mock_logger)
turn_radius = vehicle._turn_radius(rudder_deflection)
bearing_change = - vehicle._track_delta(INITIAL_SPEED_IN_MS*time_s,turn_radius)
expected_track = to_360(bearing + bearing_change)
expected_position = self.globe.new_position(starting_position,bearing + (0.5 * bearing_change),vehicle._straightline_distance(turn_radius,bearing_change))
vehicle.rudder.set_position(rudder_deflection)
vehicle.timer.wait_for(time_s)
new_position = vehicle.gps.position
self.assertEqual(new_position.longitude, expected_position.longitude)
self.assertEqual(new_position.latitude, expected_position.latitude)
self.assertEqual(round(expected_track,5),round(vehicle.gps.track,5))
self.assertEqual(round(expected_track,5), round(vehicle.compass.bearing,5))
def bearing(self):
self._initialise()
x_acc = self.accel.read12_2s_comp(0x28)
y_acc = self.accel.read12_2s_comp(0x2a)
z_acc = self.accel.read12_2s_comp(0x2c)
x_m = self.compass.read16_2s_comp(0x03)
y_m = self.compass.read16_2s_comp(0x07)
z_m = self.compass.read16_2s_comp(0x05)
roll = math.atan2(y_acc, z_acc)
pitch = math.atan2(-x_acc, z_acc) # reversing x accel makes it work
sin_roll = math.sin(roll)
cos_roll = math.cos(roll)
cos_pitch = math.cos(pitch)
sin_pitch = math.sin(pitch)
x_final = x_m * cos_pitch + y_m * sin_roll * sin_pitch + z_m * cos_roll * sin_pitch
y_final = y_m * cos_roll - z_m * sin_roll
bearing = round(math.degrees(math.atan2(y_final, x_final)), 0)
return to_360(bearing)
def bearing(self):
self._initialise()
x_acc = self.accel.read12_2s_comp(0x28)
y_acc = self.accel.read12_2s_comp(0x2a)
z_acc = self.accel.read12_2s_comp(0x2c)
x_m = self.compass.read16_2s_comp(0x03)
y_m = self.compass.read16_2s_comp(0x07)
z_m = self.compass.read16_2s_comp(0x05)
roll = math.atan2(y_acc,z_acc)
pitch = math.atan2(-x_acc,z_acc) # reversing x accel makes it work
sin_roll = math.sin(roll)
cos_roll = math.cos(roll)
cos_pitch = math.cos(pitch)
sin_pitch = math.sin(pitch)
x_final = x_m*cos_pitch + y_m*sin_roll*sin_pitch+z_m*cos_roll*sin_pitch
y_final = y_m*cos_roll-z_m*sin_roll
bearing = round(math.degrees(math.atan2(y_final,x_final)),0)
return to_360(bearing)
def test_should_turn_a_corner_if_the_rudder_is_deflected(self):
bearing, time_s = 30, 2
starting_position = Position(53, -2)
rudder_deflection = 10
gps = SimulatedGPS(starting_position, bearing, INITIAL_SPEED_IN_MS,
True)
vehicle = SimulatedVehicle(gps, self.globe, self.mock_logger)
turn_radius = vehicle._turn_radius(rudder_deflection)
bearing_change = -vehicle._track_delta(INITIAL_SPEED_IN_MS * time_s,
turn_radius)
expected_track = to_360(bearing + bearing_change)
expected_position = self.globe.new_position(
starting_position, bearing + (0.5 * bearing_change),
vehicle._straightline_distance(turn_radius, bearing_change))
vehicle.rudder.set_position(rudder_deflection)
vehicle.timer.wait_for(time_s)
new_position = vehicle.gps.position
self.assertEqual(new_position.longitude, expected_position.longitude)
self.assertEqual(new_position.latitude, expected_position.latitude)
self.assertEqual(round(expected_track, 5), round(vehicle.gps.track, 5))
self.assertEqual(round(expected_track, 5),
round(vehicle.compass.bearing, 5))
def wind_direction_abs_average(self):
return round(to_360(self._compass_avg + self._wind_relative_avg), 0)
def rotate_boat(self,rudder_effect,jitter): self.compass.bearing = to_360(self.compass.bearing + randint(-jitter,jitter) - self.rudder_servo.get_position() * rudder_effect) self.time += 0.2 self.exchange.publish(Event(EventName.tick)) # for compass smoothing! self.exchange.publish(Event(EventName.update_averages))
def test_to_360_should_add_360_to_values_less_than_zero(self):
self.assertEqual(to_360(-5),355)
self.assertEqual(to_360(-89),271)
self.assertEqual(to_360(-177),183)
def test_to_360_should_leave_alone_positive_values_less_than_360(self):
self.assertEqual(to_360(5),5)
self.assertEqual(to_360(177),177)
self.assertEqual(to_360(359),359)
def angle(self):
return to_360(angle_between(self._vehicle_bearing, self._abs_wind))
def wind_direction_abs_average(self):
return round(to_360(self._compass_avg + self._wind_relative_avg),0)
def test_to_360_should_make_360_zero_as_they_are_the_same(self):
self.assertEqual(to_360(0),0)
self.assertEqual(to_360(360),0)
def test_to_360_should_subtract_360_from_values_greater_than_360(self):
self.assertEqual(to_360(380),20)
self.assertEqual(to_360(800),80)
self.assertEqual(to_360(719),359)
def angle(self):
return to_360(angle_between(self._vehicle_bearing,self._abs_wind))
