def probe_serial(self):
# probe new nmea data devices
if not self.probedevice:
try:
self.probeindex = self.devices.index(False)
except:
self.probeindex = len(self.devices)
self.probedevicepath = serialprobe.probe('nmea%d' % self.probeindex, [38400, 4800])
if self.probedevicepath:
try:
self.probedevice = NMEASerialDevice(self.probedevicepath)
self.probetime = time.time()
except serial.serialutil.SerialException:
print('failed to open', self.probedevicepath, 'for nmea data')
pass
elif time.time() - self.probetime > 5:
print('nmea serial probe timeout', self.probedevicepath)
self.probedevice = None # timeout
else:
# see if the probe device gets a valid nmea message
if self.probedevice:
if self.probedevice.readline():
print('new nmea device', self.probedevicepath)
serialprobe.success('nmea%d' % self.probeindex, self.probedevicepath)
if self.probeindex < len(self.devices):
self.devices[self.probeindex] = self.probedevice
else:
self.devices.append(self.probedevice)
fd = self.probedevice.device.fileno()
self.device_fd[fd] = self.probedevice
self.poller.register(fd, select.POLLIN)
self.devices_lastmsg[self.probedevice] = time.time()
self.probedevice = None
def poll(self):
# no gpsd devices: probe
t0 = time.monotonic()
while True:
events = self.poller.poll(0)
if not events:
break
while events:
event = events.pop()
fd, flag = event
if flag != select.POLLIN:
# hope this is never hit
print('gpsd got flag for pipe:', flag)
continue
self.last_read_time = t0
self.read()
return # don't probe gpsd anymore
if (not self.devices is False) and (t0 - self.last_read_time > 20 or not self.devices):
device_path = serialprobe.probe('gpsd', [4800], 4)
if device_path:
print('gpsd serial probe', device_path)
self.probe_device, baud = device_path
self.process.pipe.send(self.probe_device)
def probe_serial(self):
# probe new nmea data devices
if not self.probedevice:
try:
index = self.devices.index(False)
except:
index = len(self.devices)
if self.probeindex != index and \
(self.probeindex >= len(self.devices) or not self.devices[self.probeindex]):
serialprobe.relinquish('nmea%d' % self.probeindex)
self.probeindex = index
self.probedevicepath = serialprobe.probe(
'nmea%d' % self.probeindex, [38400, 4800], 8)
if self.probedevicepath:
print('nmea probe', self.probedevicepath)
try:
self.probedevice = NMEASerialDevice(self.probedevicepath)
self.probetime = time.monotonic()
except Exception as e: # serial.serialutil.SerialException:
print(_('failed to open'), self.probedevicepath,
'for nmea data', e)
return
# see if the probe device gets a valid nmea message
if self.probedevice.readline():
#print('nmea new device', self.probedevicepath)
serialprobe.success('nmea%d' % self.probeindex,
self.probedevicepath)
if self.probeindex < len(self.devices):
self.devices[self.probeindex] = self.probedevice
else:
self.devices.append(self.probedevice)
fd = self.probedevice.device.fileno()
self.device_fd[fd] = self.probedevice
self.poller.register(fd, select.POLLIN)
self.devices_lastmsg[self.probedevice] = time.monotonic()
self.probedevice = None
elif time.monotonic() - self.probetime > 5:
#print('nmea serial probe timeout', self.probeindex, index, self.probedevicepath)
self.probedevice = None # timeout
def poll(self):
if not self.driver:
device_path = serialprobe.probe('servo', [38400], 1)
if device_path:
#from arduino_servo.arduino_servo_python import ArduinoServo
from arduino_servo.arduino_servo import ArduinoServo
try:
device = serial.Serial(*device_path)
device.timeout = 0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
except Exception as e:
print 'failed to open servo on:', device_path, e
return
self.driver = ArduinoServo(device.fileno(), device_path[1])
uncorrected_max_current = max(
0, self.max_current.value -
self.current.offset.value) / self.current.factor.value
self.send_driver_params(uncorrected_max_current)
self.device = device
self.device.path = device_path[0]
self.lastpolltime = time.time()
if not self.driver:
return
self.servo_calibration.poll()
result = self.driver.poll()
if result == -1:
print 'servo lost'
self.close_driver()
return
if result == 0:
d = time.time() - self.lastpolltime
if d > 5: # correct for clock skew
self.lastpolltime = time.time()
elif d > 4:
print 'servo timeout', d
self.close_driver()
else:
self.lastpolltime = time.time()
if self.controller.value == 'none':
device_path = [self.device.port, self.device.baudrate]
print 'arduino servo found on', device_path
serialprobe.success('servo', device_path)
self.controller.set('arduino')
self.driver.command(0)
t = time.time()
self.server.TimeStamp('servo', t)
if result & ServoTelemetry.VOLTAGE:
# apply correction
corrected_voltage = self.voltage.factor.value * self.driver.voltage
corrected_voltage += self.voltage.offset.value
self.voltage.set(round(corrected_voltage, 3))
if result & ServoTelemetry.CONTROLLER_TEMP:
self.controller_temp.set(self.driver.controller_temp)
if result & ServoTelemetry.MOTOR_TEMP:
self.motor_temp.set(self.driver.motor_temp)
if result & ServoTelemetry.RUDDER:
if self.driver.rudder and not math.isnan(self.driver.rudder):
data = {
'angle': self.driver.rudder,
'timestamp': t,
'device': self.device.path
}
self.sensors.write('rudder', data, 'servo')
if result & ServoTelemetry.CURRENT:
# apply correction
corrected_current = self.current.factor.value * self.driver.current
corrected_current = max(
0, corrected_current + self.current.offset.value)
self.current.set(round(corrected_current, 3))
# integrate power consumption
dt = (t - self.current_timestamp)
#print 'have current', round(1/dt), dt
self.current_timestamp = t
if self.current.value:
amphours = self.current.value * dt / 3600
self.amphours.set(self.amphours.value + amphours)
lp = .003 * dt # 5 minute time constant to average wattage
self.watts.set((1 - lp) * self.watts.value +
lp * self.voltage.value * self.current.value)
if result & ServoTelemetry.FLAGS:
self.max_current.set_max(40 if self.driver.flags
& ServoFlags.CURRENT_RANGE else 20)
flags = self.flags.value & ~ServoFlags.DRIVER_MASK | self.driver.flags
# if rudder angle comes from serial or tcp, may need to set these flags
angle = self.sensors.rudder.angle.value
if angle: # note, this is ok here for both False and 0
if abs(angle) > self.sensors.rudder.range.value:
if angle > 0:
flags |= ServoFlags.MIN_RUDDER
else:
flags |= ServoFlags.MAX_RUDDER
self.flags.update(flags)
self.engaged.update(not not self.driver.flags & ServoFlags.ENGAGED)
if result & ServoTelemetry.EEPROM: # occurs only once after connecting
self.max_current.set(self.driver.max_current)
self.max_controller_temp.set(self.driver.max_controller_temp)
self.max_motor_temp.set(self.driver.max_motor_temp)
self.max_slew_speed.set(self.driver.max_slew_speed)
self.max_slew_slow.set(self.driver.max_slew_slow)
#print "GOT EEPROM", self.driver.rudder_scale, self.driver.rudder_offset, self.driver.rudder_range
self.sensors.rudder.scale.set(self.driver.rudder_scale)
self.sensors.rudder.nonlinearity.set(
self.driver.rudder_nonlinearity)
self.sensors.rudder.offset.set(self.driver.rudder_offset)
self.sensors.rudder.range.set(self.driver.rudder_range)
self.current.factor.set(self.driver.current_factor)
self.current.offset.set(self.driver.current_offset)
self.voltage.factor.set(self.driver.voltage_factor)
self.voltage.offset.set(self.driver.voltage_offset)
self.min_speed.set(self.driver.min_motor_speed)
self.max_speed.set(self.driver.max_motor_speed)
self.gain.set(self.driver.gain)
if self.fault():
if not self.flags.value & ServoFlags.FWD_FAULT and \
not self.flags.value & ServoFlags.REV_FAULT:
self.faults.set(self.faults.value + 1)
# if overcurrent then fault in the direction traveled
# this prevents moving further in this direction
if self.flags.value & ServoFlags.OVERCURRENT:
if self.lastdir > 0:
self.flags.fwd_fault()
self.position.set(1)
elif self.lastdir < 0:
self.flags.rev_fault()
self.position.set(-1)
self.reset() # clear fault condition
if not self.sensors.rudder.invalid():
self.position.set(self.sensors.rudder.angle.value)
self.send_command()
def poll(self):
if not self.driver:
device_path = serialprobe.probe('servo', [38400], 5)
if device_path:
print('servo probe', device_path, time.monotonic())
try:
device = serial.Serial(*device_path)
except Exception as e:
print('failed to open servo on:', device_path, e)
return
try:
device.timeout=0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
except Exception as e:
print('failed set nonblocking/exclusive', e)
device.close()
return
#print('driver', device_path, device)
from pypilot.arduino_servo.arduino_servo import ArduinoServo
self.driver = ArduinoServo(device.fileno())
self.send_driver_params()
self.device = device
self.device.path = device_path[0]
self.lastpolltime = time.monotonic()
if not self.driver:
return
result = self.driver.poll()
if result == -1:
print('servo lost')
self.close_driver()
return
t = time.monotonic()
if result == 0:
d = t - self.lastpolltime
if d > 4:
#print('servo timeout', d)
self.close_driver()
else:
self.lastpolltime = t
if self.controller.value == 'none':
device_path = [self.device.port, self.device.baudrate]
print('arduino servo found on', device_path)
serialprobe.success('servo', device_path)
self.controller.set('arduino')
self.driver.command(0)
if result & ServoTelemetry.VOLTAGE:
# apply correction
corrected_voltage = self.voltage.factor.value*self.driver.voltage
corrected_voltage += self.voltage.offset.value
self.voltage.set(round(corrected_voltage, 3))
if result & ServoTelemetry.CONTROLLER_TEMP:
self.controller_temp.set(self.driver.controller_temp)
if result & ServoTelemetry.MOTOR_TEMP:
self.motor_temp.set(self.driver.motor_temp)
if result & ServoTelemetry.RUDDER:
if self.driver.rudder:
if math.isnan(self.driver.rudder): # rudder no longer valid
if self.sensors.rudder.source.value == 'servo':
self.sensors.lostsensor(self.sensors.rudder)
else:
data = {'angle': self.driver.rudder, 'timestamp' : t,
'device': self.device.path}
self.sensors.write('rudder', data, 'servo')
if result & ServoTelemetry.CURRENT:
# apply correction
corrected_current = self.current.factor.value*self.driver.current
if self.driver.current:
corrected_current = max(0, corrected_current + self.current.offset.value)
self.current.set(round(corrected_current, 3))
# integrate power consumption
dt = (t - self.current.lasttime)
self.current.lasttime = t
if self.current.value:
amphours = self.current.value*dt/3600
self.amphours.set(self.amphours.value + amphours)
lp = .003*dt # 5 minute time constant to average wattage
self.watts.set((1-lp)*self.watts.value + lp*self.voltage.value*self.current.value)
if result & ServoTelemetry.FLAGS:
self.max_current.set_max(40 if self.driver.flags & ServoFlags.CURRENT_RANGE else 20)
flags = self.flags.value & ~ServoFlags.DRIVER_MASK | self.driver.flags
# if rudder angle comes from serial or tcp, may need to set these flags
# to prevent rudder movement
angle = self.sensors.rudder.angle.value
if angle: # note, this is ok here for both False and 0
if abs(angle) > self.sensors.rudder.range.value:
if angle > 0:
flags |= ServoFlags.MAX_RUDDER_FAULT
else:
flags |= ServoFlags.MIN_RUDDER_FAULT
self.flags.update(flags)
self.engaged.update(not not self.driver.flags & ServoFlags.ENGAGED)
if result & ServoTelemetry.EEPROM and self.use_eeprom.value: # occurs only once after connecting
self.max_current.set(self.driver.max_current)
self.max_controller_temp.set(self.driver.max_controller_temp)
self.max_motor_temp.set(self.driver.max_motor_temp)
self.max_slew_speed.set(self.driver.max_slew_speed)
self.max_slew_slow.set(self.driver.max_slew_slow)
self.sensors.rudder.scale.set(self.driver.rudder_scale)
self.sensors.rudder.nonlinearity.set(self.driver.rudder_nonlinearity)
self.sensors.rudder.offset.set(self.driver.rudder_offset)
self.sensors.rudder.range.set(self.driver.rudder_range)
self.sensors.rudder.update_minmax()
self.current.factor.set(self.driver.current_factor)
self.current.offset.set(self.driver.current_offset)
self.voltage.factor.set(self.driver.voltage_factor)
self.voltage.offset.set(self.driver.voltage_offset)
self.speed.min.set(self.driver.min_speed)
self.speed.max.set(self.driver.max_speed)
self.gain.set(self.driver.gain)
if self.fault():
if not self.flags.value & ServoFlags.PORT_OVERCURRENT_FAULT and \
not self.flags.value & ServoFlags.STARBOARD_OVERCURRENT_FAULT:
self.faults.set(self.faults.value + 1)
# if overcurrent then fault in the direction traveled
# this prevents moving further in this direction
if self.flags.value & ServoFlags.OVERCURRENT_FAULT:
if self.lastdir > 0:
self.flags.port_overcurrent_fault()
elif self.lastdir < 0:
self.flags.starboard_overcurrent_fault()
if self.sensors.rudder.invalid() and self.lastdir:
rudder_range = self.sensors.rudder.range.value
new_position = self.lastdir*rudder_range
if self.hardover_calculation_valid * self.lastdir < 0:
# estimate hardover time if possible, this helps with
# clearing the overcurrent conditions at reasonable positions
d = (new_position + self.position.value) / (2*rudder_range)
hardover_time = self.hardover_time.value*abs(d)
hardover_time = min(max(hardover_time, 1), 30)
self.hardover_time.set(hardover_time)
self.hardover_calculation_valid = self.lastdir
self.position.set(new_position)
self.reset() # clear fault condition
# update position from rudder feedback
if not self.sensors.rudder.invalid():
self.position.set(self.sensors.rudder.angle.value)
self.send_command()
self.controller_temp.timeout()
self.motor_temp.timeout()
def poll(self):
if not self.driver:
device_path = serialprobe.probe('servo', [38400], 1)
if device_path:
#from arduino_servo.arduino_servo_python import ArduinoServo
from arduino_servo.arduino_servo import ArduinoServo
try:
device = serial.Serial(*device_path)
device.timeout=0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
except Exception as e:
print 'failed to open servo:', e
return
self.driver = ArduinoServo(device.fileno())
self.send_driver_max_values(self.max_current.value)
t0 = time.time()
if self.driver.initialize(device_path[1]):
self.device = device
print 'arduino servo found on', device_path
serialprobe.success('servo', device_path)
self.controller.set('arduino')
self.driver.command(0)
self.lastpolltime = time.time()
else:
print 'failed in ', time.time()-t0
device.close()
self.driver = False
print 'failed to initialize servo on', device
if not self.driver:
return
self.servo_calibration.poll()
result = self.driver.poll()
#print 'servo poll', result
if result == -1:
print 'servo poll -1'
self.close_driver()
return
if result == 0:
d = time.time() - self.lastpolltime
if d > 5: # correct for clock skew
self.lastpolltime = time.time()
elif d > 4:
print 'servo timeout', d
self.close_driver()
else:
self.lastpolltime = time.time()
t = time.time()
self.server.TimeStamp('servo', t)
if result & ServoTelemetry.VOLTAGE:
self.voltage.set(self.driver.voltage)
if result & ServoTelemetry.CONTROLLER_TEMP:
self.controller_temp.set(self.driver.controller_temp)
if result & ServoTelemetry.MOTOR_TEMP:
self.motor_temp.set(self.driver.motor_temp)
if result & ServoTelemetry.RUDDER:
if math.isnan(self.driver.rudder):
self.rudder.update(False)
else:
self.rudder.set(self.rudder_scale.value *
(self.driver.rudder +
self.rudder_offset.value - 0.5))
if result & ServoTelemetry.CURRENT:
self.current.set(self.driver.current)
# integrate power consumption
dt = (t - self.current_timestamp)
#print 'have current', round(1/dt), dt
self.current_timestamp = t
if self.current.value:
amphours = self.current.value*dt/3600
self.amphours.set(self.amphours.value + amphours)
lp = .003*dt # 5 minute time constant to average wattage
self.watts.set((1-lp)*self.watts.value + lp*self.voltage.value*self.current.value)
if result & ServoTelemetry.FLAGS:
self.max_current.set_max(60 if self.driver.flags & ServoFlags.CURRENT_RANGE else 20)
self.flags.update(self.flags.value & ~ServoFlags.DRIVER_MASK | self.driver.flags)
self.engaged.update(not not self.driver.flags & ServoFlags.ENGAGED)
if self.fault():
if not self.flags.value & ServoFlags.FWD_FAULT and \
not self.flags.value & ServoFlags.REV_FAULT:
self.faults.set(self.faults.value + 1)
# if overcurrent then fault in the direction traveled
# this prevents moving further in this direction
if self.flags.value & ServoFlags.OVERCURRENT:
if self.lastdir > 0:
self.flags.fwd_fault()
self.position.set(1)
elif self.lastdir < 0:
self.flags.rev_fault()
self.position.set(-1)
self.reset() # clear fault condition
self.send_command()
