def set_gain_index(self, gain_index, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_GAIN_INDEX)
m.gain_index = gain_index
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_GAIN_INDEX) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.gain_index != gain_index)):
return False
return True # success
def set_ping_enable(self, ping_enabled, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_PING_ENABLE)
m.ping_enabled = ping_enabled
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_PING_ENABLE) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.ping_enabled != ping_enabled)):
return False
return True # success
def set_mode_auto(self, mode_auto, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_MODE_AUTO)
m.mode_auto = mode_auto
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_MODE_AUTO) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.mode_auto != mode_auto)):
return False
return True # success
def set_ping_interval(self, ping_interval, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_PING_INTERVAL)
m.ping_interval = ping_interval
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_PING_INTERVAL) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.ping_interval != ping_interval)):
return False
return True # success
def set_device_id(self, device_id, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_DEVICE_ID)
m.device_id = device_id
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_DEVICE_ID) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.device_id != device_id)):
return False
return True # success
def set_speed_of_sound(self, speed_of_sound, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_SPEED_OF_SOUND)
m.speed_of_sound = speed_of_sound
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_SPEED_OF_SOUND) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.speed_of_sound != speed_of_sound)):
return False
return True # success
def set_ping_rate(self, ping_rate, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_PING_RATE)
m.ping_rate = ping_rate
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_PING_RATE) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and self.ping_rate != ping_rate
or False):
return False
return True # success
def set_range(self, scan_start, scan_length, verify=True):
m = PingMessage.PingMessage(PingMessage.PING1D_SET_RANGE)
m.scan_start = scan_start
m.scan_length = scan_length
m.packMsgData()
self.write(m.msgData)
if self.request(PingMessage.PING1D_RANGE) is None:
return False
# Read back the data and check that changes have been applied
if (verify
and (self.scan_start != scan_start or self.scan_length != scan_length)):
return False
return True # success
def __init__(self, deviceName, baudrate=115200):
if deviceName is None:
print("Device name is required")
return
try:
print("Opening %s at %d bps" % (deviceName, baudrate))
## Serial object for device communication
self.iodev = serial.Serial(deviceName, baudrate)
self.iodev.timeout = 1
except Exception as e:
print("Failed to open the given serial port")
print("\t", e)
exit(1)
## A helper class to take care of decoding the input stream
self.parser = PingMessage.PingParser()
## device id of this Ping1D object, used for dst_device_id in outgoing messages
self.my_id = 255
def request(self, m_id, timeout=0.35):
msg = PingMessage.PingMessage()
msg.request_id = m_id
msg.packMsgData()
self.write(msg.msgData)
return self.waitReply(m_id, timeout)
def sendPing1DRequest():
data = PingMessage.PingMessage()
data.request_id = PingMessage.PING1D_DISTANCE_SIMPLE
data.src_device_id = 0
data.packMsgData()
ping1D_io.sendto(data.msgData, pingserver)
type=str,
default="0.0.0.0:9000",
help="Mavlink udp address and port. ex \"0.0.0.0:9000\"")
parser.add_argument(
'--min-confidence',
action="store",
type=int,
default=50,
help="Minimum acceptable confidence percentage for depth measurements.\"")
args = parser.parse_args()
## The time that this script was started
tboot = time.time()
## Parser to decode incoming PingMessage
ping_parser = PingMessage.PingParser()
## Messages that have the current distance measurement in the payload
distance_messages = [
PingMessage.PING1D_DISTANCE, PingMessage.PING1D_DISTANCE_SIMPLE,
PingMessage.PING1D_PROFILE
]
## The minimum interval time for distance updates to the autopilot
ping_interval_ms = 0.1
## The last time a distance measurement was received
last_distance_measurement_time = 0
## The last time a distance measurement was requested
last_ping_request_time = 0
# stop pingproxy
print("Stopping proxy server...")
os.system("screen -X -S pingproxy quit")
# Connect to Ping
myPing = Ping1D.Ping1D(args.device, args.baudrate)
# Make sure we have a Ping on the line
if myPing.initialize() is False:
print 'Could not communicate with Ping device!'
exit(1)
# send ping device to bootloader
print("Sending device to bootloader...")
bootloader_msg = PingMessage.PingMessage(PingMessage.PING1D_GOTO_BOOTLOADER)
bootloader_msg.packMsgData()
myPing.iodev.write(bootloader_msg.msgData)
#TODO check for ack here
options = ''
if args.verifyOption is True:
options += '-v'
print("Attempting to load new program...")
# Try five times, maybe not necessary
for x in range (0,5):
time.sleep(0.5)
cmd = '$COMPANION_DIR/tools/stm32flash_' + machine + ' ' + options + ' -g 0x0 -b 115200 -w ' + args.file + ' ' + args.device
def __init__(self):
## Queued messages received from client
self.rxMsgs = deque([])
## Parser to verify client comms
self.parser = PingMessage.PingParser()