00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 import roslib
00021 import rospy
00022 from time import sleep
00023 from sr_ronex_msgs.srv import SPI
00024
00025 roslib.load_manifest('sr_ronex_examples')
00026
00027
00028 class DW1000SpiInterface(object):
00029 """
00030 """
00031
00032 def __init__(self, ):
00033 """
00034 """
00035 self.spi_srvs_ = []
00036
00037 for spi_out_index in range(0, 4):
00038 self.spi_srvs_.append(rospy.ServiceProxy("/ronex/spi/35/command/passthrough/"+str(spi_out_index), SPI))
00039
00040 def read_register(self, spi_out_index, address, data_packet):
00041 """
00042 Reads from the DW1000 register.
00043
00044 @spi_out_index: which spi are you using (0 to 3)
00045 @address: trying to read from which address
00046 @data_packet: the packet that will be sent
00047
00048 @return values read from SPI, None if failed
00049 """
00050 return_data_packet = []
00051
00052 if address < 64:
00053 data_packet.insert(0, address)
00054 else:
00055 rospy.logerr("Address provided ("+str(address)+") is > 64")
00056 return None
00057
00058 try:
00059 resp = self.spi_srvs_[spi_out_index](data_packet)
00060 except rospy.ServiceException, e:
00061 rospy.logerr("Service call failed: %s" % e)
00062 return None
00063
00064
00065 return_data_packet = resp.data[1:]
00066
00067 print "SPI[", spi_out_index, "]: reading ", self.hexify_list(return_data_packet), " from address: ", address
00068
00069 return return_data_packet
00070
00071 def write_register(self, spi_out_index, address, data_packet):
00072 """
00073 Writes to the DW1000 register.
00074
00075 @spi_out_index: which spi are you using (0 to 3)
00076 @address: trying to write to which address
00077 @data_packet: the packet of data you want to write
00078
00079 @return True if success, False otherwise
00080 """
00081 print "SPI[", spi_out_index, "]: writing ", self.hexify_list(data_packet), "] to address: ", address
00082 if address < 64:
00083 data_packet.insert(0, address + 0x80)
00084 else:
00085 rospy.logerr("Address provided ("+str(address)+") is > 64")
00086 return False
00087
00088 try:
00089 resp = self.spi_srvs_[spi_out_index](data_packet)
00090 except rospy.ServiceException, e:
00091 rospy.logerr("Service call failed: %s" % e)
00092 return False
00093
00094 print " ... OK data written"
00095 return True
00096
00097 def hexify_list(self, l):
00098 hex_l = []
00099 for item in l:
00100 hex_l.append(str(hex(int(item))))
00101 return hex_l
00102
00103
00104 if __name__ == "__main__":
00105 rospy.init_node('sr_ronex_spi_dw1000')
00106
00107 dw1000 = DW1000SpiInterface()
00108
00109 print "--- new write"
00110 success = dw1000.write_register(0, 0x03, [1, 1, 1, 1])
00111 packet = dw1000.read_register(0, 0x03, [0, 1, 2, 3])
00112
00113 print "--- new write"
00114 success = dw1000.write_register(0, 0x03, [0x01, 0x20, 0x30, 0x40])
00115 packet = dw1000.read_register(0, 0x03, [1, 1, 1, 1])