def ChangeBaudrate(self, baudrate):
"""Change the baudrate, reset the device and reopen communication."""
self.GoToConfig()
brid = Baudrates.get_BRID(baudrate)
bridAck = self.SetBaudrate(brid)
if bridAck: # Testing if the BR was set correctly
self.device.baudrate = baudrate
print "Baudrate set to %d bps" % baudrate
time.sleep(0.01)
else:
print "NOK:Baudrate not configured."
def ChangeBaudrate(self, baudrate): """Change the baudrate, reset the device and reopen communication.""" self.GoToConfig() brid = Baudrates.get_BRID(baudrate) bridAck = self.SetBaudrate(brid) if bridAck: # Testing if the BR was set correctly self.device.baudrate=baudrate print "Baudrate set to %d bps"%baudrate time.sleep(0.01) else: print "NOK:Baudrate not configured."
def ChangeBaudrate(self, baudrate):
"""Change the baudrate, reset the device and reopen communication."""
self.GoToConfig()
brid = Baudrates.get_BRID(baudrate)
self.SetBaudrate(brid)
self.Reset()
#self.device.flush()
self.device.baudrate = baudrate
#self.device.flush()
time.sleep(0.01)
self.read_msg()
self.write_msg(0x3f)
def ChangeBaudrate(self, baudrate): """Change the baudrate, reset the device and reopen communication.""" self.GoToConfig() brid = Baudrates.get_BRID(baudrate) self.SetBaudrate(brid) self.Reset() #self.device.flush() self.device.baudrate=baudrate #self.device.flush() time.sleep(0.01) self.read_msg() self.write_msg(0x3f)
def configureMti(self, mtiSampleRate, mtiMode):
"""Configure the mode and settings of the MTMk4 device."""
self.GoToConfig()
self.timeout = math.pow(
mtiSampleRate,
-1) + MID.additionalTimeOutOffset # additional 5ms leeway
print "Timeout changed to %1.3fs based on current settings." % (
self.timeout)
mid = MID.SetOutputConfiguration
midReqDID = MID.ReqDID
dataReqDID = (0x00, 0x00)
dataDID = self.write_ack(midReqDID, dataReqDID)
try:
masterID = struct.unpack('!L', dataDID)
except struct.error:
raise MTException("could not parse configuration.")
# to have a clear distinction between MTi-G-700 and 100-series devices
deviceIDProductMask = hex(masterID[0] & 0x00f00000)
deviceTypeMask = hex(masterID[0] & 0x0f000000)
# check for user input for the delta q and delta v quantities
new_imu_period = XDIMessage.DeltaQFs
if mtiSampleRate < int(new_imu_period):
new_imu_period = mtiSampleRate
# check for user input for the rate IMU quantities
rate_imu_period = XDIMessage.RateOfTurnFs
if mtiSampleRate < int(rate_imu_period):
rate_imu_period = mtiSampleRate
# check for user input for the mag quantities
new_mag_period = XDIMessage.MagneticFieldFs
if mtiSampleRate < int(new_mag_period):
new_mag_period = mtiSampleRate
# check for user input for the baro samples
new_pressure_period = XDIMessage.PressureFs
if mtiSampleRate < int(new_pressure_period):
new_pressure_period = mtiSampleRate
if (deviceIDProductMask[2] == XDIProductMask.MTi1Series):
new_imu_period = rate_imu_period = new_mag_period = XDIMessage.FsModule
if mtiSampleRate < int(new_imu_period):
new_imu_period = rate_imu_period = new_mag_period = mtiSampleRate
# All messages with corresponding output data rates
"Packet couter, SampleTimeFine"
# mPc = self.getMtiConfigBytes(XDIMessage.PacketCounter, XDIMessage.PaddedFs)
mStf = self.getMtiConfigBytes(XDIMessage.SampleTimeFine,
XDIMessage.PaddedFs)
"Sensor data"
mImuDq = self.getMtiConfigBytes(XDIMessage.DeltaQ, new_imu_period)
mImuDv = self.getMtiConfigBytes(XDIMessage.DeltaV, new_imu_period)
"Sensor data (rate quantities)"
mImuMag = self.getMtiConfigBytes(XDIMessage.MagneticField,
new_mag_period)
mImuGyr = self.getMtiConfigBytes(XDIMessage.RateOfTurn,
rate_imu_period)
mImuAcc = self.getMtiConfigBytes(XDIMessage.Acceleration,
rate_imu_period)
"Baro data"
mImuP = self.getMtiConfigBytes(XDIMessage.Pressure,
new_pressure_period)
"GNSS data"
mGnssPvt = self.getMtiConfigBytes(XDIMessage.GnssPvtData,
XDIMessage.GnssFs)
mGnssSat = self.getMtiConfigBytes(XDIMessage.GnssSatInfo,
XDIMessage.GnssFs)
"Status word"
mSw = self.getMtiConfigBytes(XDIMessage.StatusWord,
XDIMessage.PaddedFs)
# Filter related messages
"Filter estimate"
mOrientationQuat = self.getMtiConfigBytes(XDIMessage.OrientationQuat,
rate_imu_period)
mOrientation = self.getMtiConfigBytes(XDIMessage.Orientation,
rate_imu_period)
mVelocity = self.getMtiConfigBytes(XDIMessage.Velocity,
rate_imu_period)
mPosition = self.getMtiConfigBytes(XDIMessage.PositionLatLon,
rate_imu_period)
mHeight = self.getMtiConfigBytes(XDIMessage.PositionHeight,
rate_imu_period)
# Output configuration set based on the product ID and user specification
if (deviceIDProductMask[2] == XDIProductMask.MTi100Series) & (
deviceTypeMask[2] == XDIProductMask.MTi700Device):
print "MTi-G-700/710 (GNSS/INS) device detected."
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mImuP + mGnssPvt + mGnssSat + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mImuP + mGnssPvt + mGnssSat + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation + mVelocity + mPosition + mHeight
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi100Series:
print "MTi-100/200/300 device detected."
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mImuP + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mImuP + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi10Series:
print "MTi-10/20/30 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif (deviceIDProductMask[2] == XDIProductMask.MTi1Series) & (
deviceTypeMask[2] == XDIProductMask.MTi7Device):
print "MTi-7 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mImuP + mGnssPvt + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mImuP + mGnssPvt + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation + mVelocity + mPosition + mHeight
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi1Series:
print "MTi-1/2/3 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf + mImuDq + mImuDv + mImuMag + mSw + mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf + mImuGyr + mImuAcc + mImuMag + mSw + mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf + mSw + mOrientation
else:
raise MTException("unknown mtiMode: (%d)." % (mtiMode))
else:
raise MTException("Unknown device")
self.write_msg(mid, data)
# check for the set baudrate
dataAck = self.write_ack(MID.SetBaudrate, ())
bridAck = struct.unpack('!B', dataAck)
brSettings = Baudrates.get_BR(bridAck[0])
print "Device configured at %1.0f bps" % (brSettings)
self.GoToMeasurement()
def configureMti(self, mtiSampleRate, mtiMode):
"""Configure the mode and settings of the MTMk4 device."""
self.GoToConfig()
self.timeout = math.pow(mtiSampleRate,-1)+MID.additionalTimeOutOffset # additional 5ms leeway
print "Timeout changed to %1.3fs based on current settings."%(self.timeout)
mid = MID.SetOutputConfiguration
midReqDID = MID.ReqDID
dataReqDID = (0x00, 0x00)
dataDID = self.write_ack(midReqDID, dataReqDID)
try:
masterID = struct.unpack('!L', dataDID)
except struct.error:
raise MTException("could not parse configuration.")
# to have a clear distinction between MTi-G-700 and 100-series devices
deviceIDProductMask = hex(masterID[0]&0x00f00000)
deviceTypeMask = hex(masterID[0]&0x0f000000)
# check for user input for the delta q and delta v quantities
new_imu_period = XDIMessage.DeltaQFs
if mtiSampleRate < int(new_imu_period):
new_imu_period = mtiSampleRate
# check for user input for the rate IMU quantities
rate_imu_period = XDIMessage.RateOfTurnFs
if mtiSampleRate < int(rate_imu_period):
rate_imu_period = mtiSampleRate
# check for user input for the mag quantities
new_mag_period = XDIMessage.MagneticFieldFs
if mtiSampleRate < int(new_mag_period):
new_mag_period = mtiSampleRate
# check for user input for the baro samples
new_pressure_period = XDIMessage.PressureFs
if mtiSampleRate < int(new_pressure_period):
new_pressure_period = mtiSampleRate
if (deviceIDProductMask[2] == XDIProductMask.MTi1Series) | (deviceIDProductMask[2] == XDIProductMask.FMT1000Series):
new_imu_period = rate_imu_period = new_mag_period = XDIMessage.FsModule
if mtiSampleRate < int(new_imu_period):
new_imu_period = rate_imu_period = new_mag_period = mtiSampleRate
# All messages with corresponding output data rates
"Packet couter, SampleTimeFine"
# mPc = self.getMtiConfigBytes(XDIMessage.PacketCounter, XDIMessage.PaddedFs)
mStf = self.getMtiConfigBytes(XDIMessage.SampleTimeFine, XDIMessage.PaddedFs)
"Sensor data"
mImuDq = self.getMtiConfigBytes(XDIMessage.DeltaQ, new_imu_period)
mImuDv = self.getMtiConfigBytes(XDIMessage.DeltaV, new_imu_period)
"Sensor data (rate quantities)"
mImuMag = self.getMtiConfigBytes(XDIMessage.MagneticField, new_mag_period)
mImuGyr = self.getMtiConfigBytes(XDIMessage.RateOfTurn, rate_imu_period)
mImuAcc = self.getMtiConfigBytes(XDIMessage.Acceleration, rate_imu_period)
"Baro data"
mImuP = self.getMtiConfigBytes(XDIMessage.Pressure, new_pressure_period)
"GNSS data"
mGnssPvt = self.getMtiConfigBytes(XDIMessage.GnssPvtData, XDIMessage.GnssFs)
mGnssSat = self.getMtiConfigBytes(XDIMessage.GnssSatInfo, XDIMessage.GnssFs)
"Status word"
mSw = self.getMtiConfigBytes(XDIMessage.StatusWord, XDIMessage.PaddedFs)
# Filter related messages
"Filter estimate"
mOrientationQuat = self.getMtiConfigBytes(XDIMessage.OrientationQuat, rate_imu_period)
mOrientation = self.getMtiConfigBytes(XDIMessage.Orientation,rate_imu_period)
mVelocity = self.getMtiConfigBytes(XDIMessage.Velocity,rate_imu_period)
mPosition = self.getMtiConfigBytes(XDIMessage.PositionLatLon,rate_imu_period)
mHeight = self.getMtiConfigBytes(XDIMessage.PositionHeight,rate_imu_period)
# Output configuration set based on the product ID and user specification
if (deviceIDProductMask[2] == XDIProductMask.MTi100Series) & (deviceTypeMask[2] == XDIProductMask.MTi700Device):
print "MTi-G-700/710 (GNSS/INS) device detected."
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf+mImuDq+mImuDv+mImuMag+mImuP+mGnssPvt+mGnssSat+mSw+mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf+mImuGyr+mImuAcc+mImuMag+mImuP+mGnssPvt+mGnssSat+mSw+mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf+mSw+mOrientation+mVelocity+mPosition+mHeight
else:
raise MTException("unknown mtiMode: (%d)."% (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi100Series:
print "MTi-100/200/300 device detected."
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf+mImuDq+mImuDv+mImuMag+mImuP+mSw+mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf+mImuGyr+mImuAcc+mImuMag+mImuP+mSw+mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf+mSw+mOrientation
else:
raise MTException("unknown mtiMode: (%d)."% (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi10Series:
print "MTi-10/20/30 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf+mImuDq+mImuDv+mImuMag+mSw+mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf+mImuGyr+mImuAcc+mImuMag+mSw+mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf+mSw+mOrientation
else:
raise MTException("unknown mtiMode: (%d)."% (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.MTi1Series:
print "MTi-1/2/3 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf+mImuDq+mImuDv+mImuMag+mSw+mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf+mImuGyr+mImuAcc+mImuMag+mSw+mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf+mSw+mOrientation
else:
raise MTException("unknown mtiMode: (%d)."% (mtiMode))
elif deviceIDProductMask[2] == XDIProductMask.FMT1000Series:
print "FMT-1010/1020/1030 device detected"
if mtiMode == 1:
print "Enabled publishing all sensor data"
data = mStf+mImuDq+mImuDv+mImuMag+mSw+mOrientationQuat
elif mtiMode == 2:
print "Enabled publishing all sensor data (rate quantities)"
data = mStf+mImuGyr+mImuAcc+mImuMag+mSw+mOrientationQuat
elif mtiMode == 3:
print "Enabled publishing all filter estimates"
data = mStf+mSw+mOrientation
else:
raise MTException("unknown mtiMode: (%d)."% (mtiMode))
else:
raise MTException("Unknown device")
self.write_msg(mid, data)
# check for the set baudrate
dataAck = self.write_ack(MID.SetBaudrate, ())
bridAck = struct.unpack('!B',dataAck)
brSettings = Baudrates.get_BR(bridAck[0])
print "Device configured at %1.0f bps"%(brSettings)
self.GoToMeasurement()
