def compute_heading(self):
self.compute_offsets()
ap = self.ap
wind = ap.wind_direction.value
mode = ap.mode.value
if mode == 'gps':
# if gps drops out switch to wind mode
if ap.sensors.gps.source.value == 'none':
ap.mode_lost('wind')
gps = resolv(ap.wind_compass_offset.value - wind, 180)
ap.heading.set(gps)
if mode == 'true wind':
# for true wind, need gps
if ap.sensors.gps.source.value == 'none':
ap.mode_lost('wind')
true_wind = resolv(ap.true_wind_wind_offset.value + wind, 180)
ap.heading.set(true_wind)
if mode == 'compass':
# compute compass from the wind. this causes the boat
# to follow wind shifts with an overall average compass course
compass = resolv(self.compass_wind_offset.value - wind, 180)
ap.heading.set(compass)
if mode == 'wind':
ap.heading.set(wind)
def compute_offsets(self):
# compute the difference from wind to other headings
wind = self.ap.wind_direction.value
compass = self.ap.boatimu.SensorValues['heading_lowpass'].value
d = .005
self.compass_wind_offset.update(wind+compass, d)
sensors = self.ap.sensors
if sensors.gps.source.value != 'none':
gps_track = sensors.gps.track.value
# difference from gps to wind
self.gps_wind_offset.update(gps_track+wind, d)
# compute offset between wind and true wind
gps_speed = sensors.gps.speed.value
wind_speed = sensors.wind.speed.value
wind_direction = sensors.wind.direction.value
rd = math.radians(wind_direction)
windv = wind_speed*math.sin(rd), wind_speed*math.cos(rd)
true_wind = math.degrees(math.atan2(windv[0], windv[1] - gps_speed))
offset = resolv(true_wind - wind, self.true_wind_wind_offset.value)
d = .05
self.true_wind_wind_offset.update(offset, d)
# compensate compass relative to wind offset
if self.ap.compass_change:
self.compass_wind_offset.value += self.ap.compass_change
def process(self, reset):
t = time.time()
ap = self.ap
if reset:
self.heading_command_rate.set(0)
# reset feed-forward gain
self.last_heading_mode = False
# reset feed-forward error if mode changed, or last command is older than 1 second
if self.last_heading_mode != ap.mode.value or t - self.heading_command_rate.time > 1:
self.last_heading_command = ap.heading_command.value
# if disabled, only compute if a client cares
if not ap.enabled.value:
compute = False
for gain in self.gains:
if self.gains[gain]['sensor'].watchers:
compute = True
break
if not compute:
return
# filter the heading command to compute feed-forward gain
heading_command_diff = resolv(ap.heading_command.value - self.last_heading_command)
self.last_heading_command = ap.heading_command.value
self.last_heading_mode = ap.mode.value
self.heading_command_rate.time = t;
lp = .1
command_rate = (1-lp)*self.heading_command_rate.value + lp*heading_command_diff
self.heading_command_rate.set(command_rate)
# compute command
headingrate = ap.boatimu.SensorValues['headingrate_lowpass'].value
headingraterate = ap.boatimu.SensorValues['headingraterate_lowpass'].value
feedforward_value = self.heading_command_rate.value
reactive_value = self.servocommand_queue.take(t - self.reactive_time.value)
self.reactive_value.set(reactive_value)
if not 'wind' in ap.mode.value: # wind mode needs opposite gain
feedforward_value = -feedforward_value
gain_values = {'P': ap.heading_error.value,
'I': ap.heading_error_int.value,
'D': headingrate,
'DD': headingraterate,
'FF': feedforward_value,
'R': -reactive_value}
PR = math.sqrt(abs(gain_values['P']))
if gain_values['P'] < 0:
PR = -PR
gain_values['PR'] = PR
command = self.Compute(gain_values)
rval = self.gains['R']['sensor'].value
# don't include R contribution to command
self.servocommand_queue.add(command - rval)
if ap.enabled.value:
ap.servo.command.set(command)
