def __set_parameters(self, force, **kwargs):
"""Sends Sonar head command to set sonar properties.
Only the parameters specified will be modified.
If initialized, arguments are compared to current properties in order
to see if sending the command is necessary.
Args:
force: Whether to force set parameters.
See set().
"""
rospy.logwarn("Setting parameters...")
# Set and compare sonar properties.
necessary = not self.has_cfg or self.no_params or force
only_reverse = not necessary
for key, value in kwargs.iteritems():
if value is not None:
if hasattr(self, key) and self.__getattribute__(key) != value:
self.__setattr__(key, value)
necessary = True
if key != "scanright":
only_reverse = False
# Return if unnecessary.
if not necessary:
rospy.logwarn("Parameters are already set")
return
# Return if only switching the motor's direction is necessary.
if only_reverse:
rospy.logwarn("Only reversing direction")
self.scanright = not self.scanright
return self.reverse()
# Log properties.
rospy.loginfo("CONTINUOUS: %s", self.continuous)
rospy.loginfo("LEFT LIMIT: %s rad", self.left_limit)
rospy.loginfo("RIGHT LIMIT: %s rad", self.right_limit)
rospy.loginfo("STEP SIZE: %s rad", self.step)
rospy.loginfo("N BINS: %s", self.nbins)
rospy.loginfo("RANGE: %s m", self.range)
rospy.loginfo("INVERTED: %s", self.inverted)
rospy.loginfo("AD HIGH: %s dB", self.ad_high)
rospy.loginfo("AD LOW: %s dB", self.ad_low)
rospy.loginfo("ADC 8 ON: %s", self.adc8on)
rospy.loginfo("GAIN: %s%%", self.gain * 100)
rospy.loginfo("MOTOR TIME: %s us", self.mo_time)
rospy.loginfo("CLOCKWISE: %s", self.scanright)
rospy.loginfo("SPEED: %s m/s", self.speed)
# This device is not Dual Channel so skip the “V3B” Gain Parameter
# block: 0x01 for normal, 0x1D for extended V3B Gain Parameters.
v3b = bitstring.pack("0x01")
# Construct the HdCtrl bytes to control operation:
# Bit 0: adc8on 0: 4-bit 1: 8-bit
# Bit 1: cont 0: sector-scan 1: continuous
# Bit 2: scanright 0: left 1: right
# Bit 3: invert 0: upright 1: inverted
# Bit 4: motoff 0: on 1: off
# Bit 5: txoff 0: on 1: off (for testing)
# Bit 6: spare 0: default 1: N/A
# Bit 7: chan2 0: default 1: N/A
# Bit 8: raw 0: N/A 1: default
# Bit 9: hasmot 0: lol 1: has a motor (always)
# Bit 10: applyoffset 0: default 1: heading offset
# Bit 11: pingpong 0: default 1: side-scanning sonar
# Bit 12: stareLLim 0: default 1: N/A
# Bit 13: ReplyASL 0: N/A 1: default
# Bit 14: ReplyThr 0: default 1: N/A
# Bit 15: IgnoreSensor 0: default 1: emergencies
hd_ctrl = bitstring.pack("0b001000110000, bool, bool, bool, bool",
self.inverted, self.scanright,
self.continuous, self.adc8on)
hd_ctrl.byteswap() # Little endian please.
# Set the sonar type: 0x11 for DST.
hd_type = bitstring.pack("0x11")
# TX/RX transmitter constants: N/A to DST so fill with 15 zero bytes.
# TX pulse length: N/A to DST so fill with 2 zero bytes.
tx_rx = bitstring.BitStream(144)
# Range scale does not control the sonar, only provides a way to note
# the current settings in a human readable format.
# The lower 14 bits are the range scale * 10 units and the higher 2
# bits are coded units:
# 0: meters
# 1: feet
# 2: fathoms
# 3: yards
# Only the metric system is implemented for now, because it is better.
range_scale = bitstring.pack("uintle:16", int(self.range * 10))
# Left/right angles limits are in 1/16th of a gradian.
_left_angle = to_sonar_angles(self.left_limit)
_right_angle = to_sonar_angles(self.right_limit)
left_limit = bitstring.pack("uintle:16", _left_angle)
right_limit = bitstring.pack("uintle:16", _right_angle)
# Set the mapping of the received sonar echo amplitudes.
# If the ADC is set to 8-bit, MAX = 255 else MAX = 15.
# ADLow = MAX * low / 80 where low is the desired minimum amplitude.
# ADSpan = MAX * range / 80 where range is the desired amplitude range.
# The full range is between ADLow and ADLow + ADSpan.
MAX_SIZE = 255 if self.adc8on else 15
ad_low = bitstring.pack("uint:8", int(MAX_SIZE * self.ad_low / 80))
_ad_diff = self.ad_high - self.ad_low
ad_span = bitstring.pack("uint:8", int(MAX_SIZE * _ad_diff / 80))
# Set the initial gain of each channel of the sonar receiver.
# The gain ranges from 0 to 210.
_mapped_gain = int(self.gain * 210)
gain = bitstring.pack("uint:8, uint:8", _mapped_gain, _mapped_gain)
# Slope setting is N/A to DST: fill 4 bytes with zeroes.
slope = bitstring.BitStream(32)
# Set the high speed limit of the motor in units of 10 microseconds.
mo_time = bitstring.pack("uint:8", int(self.mo_time / 10))
# Set the step angle size in 1/16th of a gradian.
# 32: low resolution
# 16: medium resolution
# 8: high resolution
# 4: ultimate resolution
_step_size = to_sonar_angles(self.step)
step = bitstring.pack("uint:8", _step_size)
# ADInterval defines the sampling interval of each bin and is in units
# of 640 nanoseconds.
nbins = bitstring.pack("uintle:16", self.nbins)
_interval = 2 * self.range / self.speed / self.nbins / 640e-9
_interval = _interval + 1 if _interval % 2 else _interval
ad_interval = bitstring.pack("uintle:16", int(_interval))
# Factory defaults. Don't ask.
max_ad_buf = bitstring.pack("uintle:16", 500)
lockout = bitstring.pack("uintle:16", 100)
minor_axis = bitstring.pack("uintle:16", 1600)
major_axis = bitstring.pack("uint:8", 1)
# Ctl2 is for testing.
ctl2 = bitstring.pack("uint:8", 0)
# Special devices setting. Should be left blank.
scanz = bitstring.pack("uint:8, uint:8", 0, 0)
# Order and construct bitstream.
bitstream = (v3b, hd_ctrl, hd_type, tx_rx, range_scale, left_limit,
right_limit, ad_span, ad_low, gain, slope, mo_time, step,
ad_interval, nbins, max_ad_buf, lockout, minor_axis,
major_axis, ctl2, scanz)
payload = bitstring.BitStream()
for chunk in bitstream:
payload.append(chunk)
self.send(Message.HEAD_COMMAND, payload)
rospy.logwarn("Parameters are sent")
def __set_parameters(self, force, **kwargs):
"""Sends Sonar head command to set sonar properties.
Only the parameters specified will be modified.
If initialized, arguments are compared to current properties in order
to see if sending the command is necessary.
Args:
force: Whether to force set parameters.
See set().
"""
rospy.logwarn("Setting parameters...")
# Set and compare sonar properties.
necessary = not self.has_cfg or self.no_params or force
only_reverse = not necessary
for key, value in kwargs.iteritems():
if value is not None:
if hasattr(self, key) and self.__getattribute__(key) != value:
self.__setattr__(key, value)
necessary = True
if key != "scanright":
only_reverse = False
# Return if unnecessary.
if not necessary:
rospy.logwarn("Parameters are already set")
return
# Return if only switching the motor's direction is necessary.
if only_reverse:
rospy.logwarn("Only reversing direction")
self.scanright = not self.scanright
return self.reverse()
# Log properties.
rospy.loginfo("CONTINUOUS: %s", self.continuous)
rospy.loginfo("LEFT LIMIT: %s rad", self.left_limit)
rospy.loginfo("RIGHT LIMIT: %s rad", self.right_limit)
rospy.loginfo("STEP SIZE: %s rad", self.step)
rospy.loginfo("N BINS: %s", self.nbins)
rospy.loginfo("RANGE: %s m", self.range)
rospy.loginfo("INVERTED: %s", self.inverted)
rospy.loginfo("AD HIGH: %s dB", self.ad_high)
rospy.loginfo("AD LOW: %s dB", self.ad_low)
rospy.loginfo("ADC 8 ON: %s", self.adc8on)
rospy.loginfo("GAIN: %s%%", self.gain * 100)
rospy.loginfo("MOTOR TIME: %s us", self.mo_time)
rospy.loginfo("CLOCKWISE: %s", self.scanright)
rospy.loginfo("SPEED: %s m/s", self.speed)
# This device is not Dual Channel so skip the “V3B” Gain Parameter
# block: 0x01 for normal, 0x1D for extended V3B Gain Parameters.
v3b = bitstring.pack("0x01")
# Construct the HdCtrl bytes to control operation:
# Bit 0: adc8on 0: 4-bit 1: 8-bit
# Bit 1: cont 0: sector-scan 1: continuous
# Bit 2: scanright 0: left 1: right
# Bit 3: invert 0: upright 1: inverted
# Bit 4: motoff 0: on 1: off
# Bit 5: txoff 0: on 1: off (for testing)
# Bit 6: spare 0: default 1: N/A
# Bit 7: chan2 0: default 1: N/A
# Bit 8: raw 0: N/A 1: default
# Bit 9: hasmot 0: lol 1: has a motor (always)
# Bit 10: applyoffset 0: default 1: heading offset
# Bit 11: pingpong 0: default 1: side-scanning sonar
# Bit 12: stareLLim 0: default 1: N/A
# Bit 13: ReplyASL 0: N/A 1: default
# Bit 14: ReplyThr 0: default 1: N/A
# Bit 15: IgnoreSensor 0: default 1: emergencies
hd_ctrl = bitstring.pack(
"0b001000110000, bool, bool, bool, bool",
self.inverted, self.scanright, self.continuous, self.adc8on
)
hd_ctrl.byteswap() # Little endian please.
# Set the sonar type: 0x11 for DST.
hd_type = bitstring.pack("0x11")
# TX/RX transmitter constants: N/A to DST so fill with 15 zero bytes.
# TX pulse length: N/A to DST so fill with 2 zero bytes.
tx_rx = bitstring.BitStream(144)
# Range scale does not control the sonar, only provides a way to note
# the current settings in a human readable format.
# The lower 14 bits are the range scale * 10 units and the higher 2
# bits are coded units:
# 0: meters
# 1: feet
# 2: fathoms
# 3: yards
# Only the metric system is implemented for now, because it is better.
range_scale = bitstring.pack("uintle:16", int(self.range * 10))
# Left/right angles limits are in 1/16th of a gradian.
_left_angle = to_sonar_angles(self.left_limit)
_right_angle = to_sonar_angles(self.right_limit)
left_limit = bitstring.pack("uintle:16", _left_angle)
right_limit = bitstring.pack("uintle:16", _right_angle)
# Set the mapping of the received sonar echo amplitudes.
# If the ADC is set to 8-bit, MAX = 255 else MAX = 15.
# ADLow = MAX * low / 80 where low is the desired minimum amplitude.
# ADSpan = MAX * range / 80 where range is the desired amplitude range.
# The full range is between ADLow and ADLow + ADSpan.
MAX_SIZE = 255 if self.adc8on else 15
ad_low = bitstring.pack("uint:8", int(MAX_SIZE * self.ad_low / 80))
_ad_diff = self.ad_high - self.ad_low
ad_span = bitstring.pack("uint:8", int(MAX_SIZE * _ad_diff / 80))
# Set the initial gain of each channel of the sonar receiver.
# The gain ranges from 0 to 210.
_mapped_gain = int(self.gain * 210)
gain = bitstring.pack("uint:8, uint:8", _mapped_gain, _mapped_gain)
# Slope setting is N/A to DST: fill 4 bytes with zeroes.
slope = bitstring.BitStream(32)
# Set the high speed limit of the motor in units of 10 microseconds.
mo_time = bitstring.pack("uint:8", int(self.mo_time / 10))
# Set the step angle size in 1/16th of a gradian.
# 32: low resolution
# 16: medium resolution
# 8: high resolution
# 4: ultimate resolution
_step_size = to_sonar_angles(self.step)
step = bitstring.pack("uint:8", _step_size)
# ADInterval defines the sampling interval of each bin and is in units
# of 640 nanoseconds.
nbins = bitstring.pack("uintle:16", self.nbins)
_interval = 2 * self.range / self.speed / self.nbins / 640e-9
_interval = _interval + 1 if _interval % 2 else _interval
ad_interval = bitstring.pack("uintle:16", int(_interval))
# Factory defaults. Don't ask.
max_ad_buf = bitstring.pack("uintle:16", 500)
lockout = bitstring.pack("uintle:16", 100)
minor_axis = bitstring.pack("uintle:16", 1600)
major_axis = bitstring.pack("uint:8", 1)
# Ctl2 is for testing.
ctl2 = bitstring.pack("uint:8", 0)
# Special devices setting. Should be left blank.
scanz = bitstring.pack("uint:8, uint:8", 0, 0)
# Order and construct bitstream.
bitstream = (
v3b, hd_ctrl, hd_type, tx_rx, range_scale, left_limit, right_limit,
ad_span, ad_low, gain, slope, mo_time, step, ad_interval, nbins,
max_ad_buf, lockout, minor_axis, major_axis, ctl2, scanz
)
payload = bitstring.BitStream()
for chunk in bitstream:
payload.append(chunk)
self.send(Message.HEAD_COMMAND, payload)
rospy.logwarn("Parameters are sent")
def __set_parameters(self, force, **kwargs):
"""
Sends Sonar head command to set sonar properties. This function is called
by ``set()`` method. See ``set()`` method for more information
Only the parameters specified will be modified.
If initialized, arguments are compared to current properties in order
to see if sending the command is necessary.
:param force: Whether to force set parameters.
"""
rospy.logwarn("Setting parameters...")
# Set and compare sonar properties.
necessary = not self.has_cfg or self.no_params or force
only_reverse = not necessary
for key, value in kwargs.iteritems():
if value is not None:
if hasattr(self, key) and self.__getattribute__(key) != value:
self.__setattr__(key, value)
necessary = True
if key != "scanright":
only_reverse = False
# Return if unnecessary.
if not necessary:
rospy.logwarn("Parameters are already set")
return
port_enabled = kwargs.get('port_enabled')
self.port_enabled = port_enabled
self.conn.port_enabled = port_enabled
print ('cambiando port enabled', port_enabled, self.port_enabled)
# if port_enabled:
# self.port_enabled = True
#
# else:
# self.port_enabled = False
#self.port_enabled = True
self.port = kwargs.get('profiler_port')
self.baudrate = kwargs.get('profiler_port_baudrate')
# Return if only switching the motor's direction is necessary.
if only_reverse:
rospy.logwarn("Only reversing direction")
self.scanright = not self.scanright
return self.reverse()
# self.left_limit = to_radians(16)
#
# self.range = 1.00
# self.right_limit = to_radians(16)
# self.scanright = True
# self.step = Resolution.ULTIMATE
# Log properties.
rospy.loginfo("PRF Alternate scan: %s", self.prf_alt)
rospy.loginfo("LEFT LIMIT: %s rad", self.left_limit)
rospy.loginfo("RIGHT LIMIT: %s rad", self.right_limit)
rospy.loginfo("STEP SIZE: %s rad", self.step)
rospy.loginfo("RANGE: %s m", self.range)
rospy.loginfo("INVERTED: %s", self.inverted)
rospy.loginfo("ADC THRESHOLD: %s ", self.adc_threshold)
rospy.loginfo("LOCKOUT: %s%%", self.lockout)
rospy.loginfo("ADAPTIVE GAIN CONTROL %s", self.agc)
rospy.loginfo("GAIN: %s%%", self.gain * 100)
rospy.loginfo("MOTOR TIME: %s us", self.mo_time)
rospy.loginfo("CLOCKWISE: %s", self.scanright)
rospy.loginfo("SPEED: %s m/s", self.speed)
rospy.loginfo("PORT ENABLED: %s", self.port_enabled)
rospy.loginfo("PORT: %s", self.port)
rospy.loginfo("BAUDRATE: %s bauds", self.baudrate)
# This device is not Dual Channel so skip the “V3B” Gain Parameter
# block: 0x01 for normal, 0x1D for extended V3B Gain Parameters.
v3b = bitstring.pack("0x1D")
# Construct the HdCtrl bytes to control operation:
# Bit 0: prf_AGC 0: AGC off(def) 1: AGC On
# Bit 1: prf_alt 0: continuous 1: alternating scan
# Bit 2: scanright 0: left 1: right
# Bit 3: invert 0: upright 1: inverted
# Bit 4: motoff 0: on 1: off
# Bit 5: txoff 0: on 1: off (for testing)
# Bit 6: prf_t10 0: default 1: N/A
# Bit 7: chan2 0: default 1: N/A
# Bit 8: prf_first 0: N/A 1: default
# Bit 9: hasmot 0: lol 1: has a motor (always)
# Bit 10: prf_PingSync 0: no sync 1: Ping Sync control on, default
# Bit 11: prf_ScanSync 0: default 1: use when dual head profiler
# Bit 12: stareLLim 0: default 1: N/A
# Bit 13: prf_Master 0: N/A 1: default for single profiler
# Bit 14: prf_Mirror 0: default 1: N/A
# Bit 15: IgnoreSensor 0: default 1: emergencies
# hd_ctrl = bitstring.pack(
# "0b001000110000, bool, bool, bool, bool",
# self.inverted, self.scanright, self.prf_alt, self.agc
# )
# hd_ctrl = bitstring.pack(
# "0b 0010 1111 0000, bool, bool, bool, bool",
# self.inverted, self.scanright, self.prf_alt, self.agc
# )
hd_ctrl = bitstring.pack(
"0b 011000111000, bool, bool, bool, bool",
self.inverted, self.scanright, self.prf_alt, self.agc
)
hd_ctrl.byteswap() # Little endian please.
# Set the sonar type: 0x05 for Profiler.
# Profiler allows also Imaging functionality (0x02)
hd_type = bitstring.pack("0x05")
# TX/RX transmitter constants:
# f = transmitter frequency in Hertz
f_txrx_ch1 = 600000 # in Hz
f_txrx_ch2 = 1100000 # in Hz
TxN_ch1 = int(f_txrx_ch1*pow(2,32)/(32*pow(10,6))) # = 147639500
TxN_ch2 = int(f_txrx_ch2 * pow(2, 32) / (32 * pow(10, 6))) #= 147639500
RxN_ch1 = int((f_txrx_ch1 + 455000)*pow(2,32)/(32 * pow(10,6)))
RxN_ch2 = int((f_txrx_ch2 + 455000)*pow(2,32)/(32 * pow(10,6)))
# TX pulse length: length of sonar pulse in microseconds. Variable with rangescale
# use default ofs and mul
Ofs = 10
Mul = 25
TxPulseLen = (self.range + Ofs)*Mul/10
# Packing Tx/Rx constants and TX pulse length
tx_rx = bitstring.pack('uintle:32,uintle:32,uintle:32,uintle:32,uintle:16',
TxN_ch1, TxN_ch2, RxN_ch1, RxN_ch2, TxPulseLen)
# Range scale does not control the sonar, only provides a way to note
# the current settings in a human readable format.
# The lower 14 bits are the range scale * 10 units and the higher 2
# bits are coded units:
# 0: meters
# 1: feet
# 2: fathoms
# 3: yards
# Only the metric system is implemented for now, because it is better.
range_scale = bitstring.pack("uintle:16", int(self.range * 10))
# Left/right angles limits are in 1/16th of a gradian.
_left_angle = to_sonar_angles(self.left_limit)
_right_angle = to_sonar_angles(self.right_limit)
left_limit = bitstring.pack("uintle:16", _left_angle)
right_limit = bitstring.pack("uintle:16", _right_angle)
# The ADC Threshold sets the analogue threshold level on the receiver and controls
# the sensitive of the profiler. Higher threshold makes the device less sensitive
# Default value for Seaking Profiler is 50
adc_threshold = bitstring.pack("uint:8", int(50))
# This is the level of Adaptive Gain Control (%) that is applied to the receiver.
# For SeaKing Profilers use default of 20
filt_gain = bitstring.pack("uint:8", int(20))
# Set the initial gain of each channel of the sonar receiver.
# The gain ranges from 0 to 210.
AGC_Max = int(self.gain * 210)
AGC_SetPoint = 189 # default value = 90, but Seanet sends this value...
AGC_args = bitstring.pack("uint:8, uint:8",AGC_Max, AGC_SetPoint)
#
# Slope setting is according to the sonar frequency
# The equation is slope = (f*7/1000+11560)/142
_slope_ch1 = (f_txrx_ch1*7/1000+11560)/142
_slope_ch2 = (f_txrx_ch2*7/1000+11560)/142
slope = bitstring.pack('uintle:16, uintle:16', _slope_ch1,_slope_ch2)
# Set the high speed limit of the motor in units of 10 microseconds.
mo_time = bitstring.pack("uint:8", int(self.mo_time / 10))
# Set the step angle size in 1/16th of a gradian.
# 32: low resolution
# 24: medium resolution
# 16: high resolution
# 8: ultimate resolution
_step_size = to_sonar_angles(self.step)
step = bitstring.pack("uint:8", _step_size)
# ScanTime in 10 ms units.
nbins = bitstring.pack("uintle:16", self.nbins)
#_interval = 2 * self.range * self.nbins/ self.speed / 10e-3
#_interval = _interval + 1 if _interval % 2 else _interval
_interval = 30 # Seanet as well
ScanTime = bitstring.pack("uintle:16", int(_interval))
# Spare bytes, fill with zero
PrfSp1 = bitstring.pack('uintle:16', 0)
# Controls for special operation. Normally set to zero
PrfCtl2 = bitstring.pack('uintle:16', 0)
# Factory defaults. Don't ask.
lockout = bitstring.pack("uintle:16", self.lockout)
minor_axis = bitstring.pack("uintle:16", 1600)
major_axis = bitstring.pack("uint:8", 1)
# Ctl2 is for testing.
ctl2 = bitstring.pack("uint:8", 0)
# Special devices setting. Should be left blank.
scanz = bitstring.pack("uint:8, uint:8", 0, 0)
v3b_adth1 = bitstring.pack('uintle:8',self.adc_threshold)
v3b_adth2 = bitstring.pack('uintle:8',self.adc_threshold)
v3b_filtgain1 = bitstring.pack('0x01')
v3b_filtgain2 = bitstring.pack('0x14')
v3b_agcmax1 = bitstring.pack('uintle:8', AGC_Max)
v3b_agcmax2 = bitstring.pack('uintle:8', AGC_Max)
v3b_setpoint1 = bitstring.pack('uintle:8', AGC_SetPoint)
v3b_setpoint2 = bitstring.pack('uintle:8', AGC_SetPoint)
v3b_slope1 = bitstring.pack('uintle:16', _slope_ch1)
v3b_slope2 = bitstring.pack('uintle:16', _slope_ch2)
v3b_sloped1 = bitstring.pack('uintle:16', 0)
v3b_sloped2 = bitstring.pack('uintle:16', 0)
# Order and construct bitstream.
bitstream = (
v3b, hd_ctrl, hd_type, tx_rx, range_scale, left_limit, right_limit,
adc_threshold, filt_gain, AGC_args, slope, mo_time, step, ScanTime, PrfSp1,
PrfCtl2, lockout, minor_axis, major_axis, ctl2, scanz, v3b_adth1,
v3b_adth2, v3b_filtgain1, v3b_filtgain2, v3b_agcmax1, v3b_agcmax2,
v3b_setpoint1, v3b_setpoint2, v3b_slope1, v3b_slope2, v3b_sloped1,
v3b_sloped2
)
payload = bitstring.BitStream()
for chunk in bitstream:
payload.append(chunk)
if port_enabled :
self.conn.conn.port = self.port
self.conn.conn.baudrate = self.baudrate
self.conn.open()
self.send(Message.HEAD_COMMAND, payload)
rospy.logwarn("Parameters are sent")
elif not port_enabled and self.initialized:
self.conn.close()
# while not self.port_enabled:
# pass
return hd_ctrl,hd_type,TxN_ch1,TxN_ch2,RxN_ch1,RxN_ch2,TxPulseLen,tx_rx,
range_scale, left_limit,right_limit,adc_threshold,filt_gain,
AGC_args,slope,mo_time,step,ScanTime,lockout,minor_axis,
major_axis,payload