def handle_write_zero_position(req): """ A handle for the service to be able to handle write_zero_position :param req: The requester object which contains the input from the user :type req: object """ lock.acquire() response = as5048a.write_zero_position() if as5048a.is_error: errors = as5048a.clear_and_get_error() lock.release() message = """Failed to write zero position, the following errors were encountered: Parity error: {} Command invalid: {} Framing error: {}""".format(**errors) return TriggerResponse(False, message) if as5048a.is_parity_mismatch: lock.release() message = "Failed to write zero position, the received package has a mismatched parity" return TriggerResponse(False, message) lock.release() old = AS5048.calc_angle(response['old_zero_position']) new = AS5048.calc_angle(response['new_zero_position']) message = """Successfully wrote a new zero position Old Zero Position: {} rad New Zero Position: {} rad""".format(old, new) return TriggerResponse(True, message)
def test_read(self, mock_spi): as5048 = AS5048(mock_spi) # Check so read bit is set with mock.patch.object(AS5048, 'transfer', return_value=0x3fff) as mock_transfer: as5048.read(AS5048.ADDR_NOP) self.assertEqual(mock_transfer.call_count, 2) calls = [call(0x4000), call(0x4000)] mock_transfer.assert_has_calls(calls) mock_transfer.reset_mock() as5048.read(AS5048.ADDR_ANGLE) self.assertEqual(mock_transfer.call_count, 2) calls = [call(0x7fff), call(0x4000)] mock_transfer.assert_has_calls(calls) # Test default operation of read without any errors mock_spi.transfer.side_effect = [[0x00, 0x00], [0x3f, 0xff]] response = as5048.read(AS5048.ADDR_ANGLE) # Check calls self.assertEqual(mock_spi.transfer.call_count, 2) calls = [call([0xff, 0xff]), call([0xc0, 0x00])] mock_spi.transfer.assert_has_calls(calls) # Check final returns self.assertEqual( response, 0x3fff, 'Wrong response from read, should be the same as the transfer return' ) self.assertFalse(as5048.is_error, 'Error is set') self.assertFalse(as5048.is_parity_mismatch, 'Parity mismatch')
def test_read_cordic_magnitude(self, mock_spi): mock_spi.transfer.return_value = [0x00, 0x00] as5048 = AS5048(mock_spi) magnitude = as5048.read_cordic_magnitude() mock_spi.transfer.assert_has_calls( [call([0x7f, 0xfe]), call([0xc0, 0x00])]) self.assertEqual(magnitude, 0) mock_spi.transfer.return_value = [0x3f, 0xff] magnitude = as5048.read_cordic_magnitude() self.assertEqual(magnitude, 0x3fff)
def test_read_angle(self, mock_spi): mock_spi.transfer.return_value = [0x00, 0x00] as5048 = AS5048(mock_spi) angle = as5048.read_angle() mock_spi.transfer.assert_has_calls( [call([0xff, 0xff]), call([0xc0, 0x00])]) self.assertEqual(angle, 0) mock_spi.transfer.return_value = [0x3f, 0xff] angle = as5048.read_angle() self.assertAlmostEqual(angle, 6, delta=0.3)
def test_read_diagnostics(self, mock_spi): mock_spi.transfer.return_value = [0b1010, 0b10011010] as5048 = AS5048(mock_spi) response = as5048.read_diagnostics() self.assertEqual(mock_spi.transfer.call_count, 2) self.assertDictEqual(response, { 'comp_hi': 1, 'comp_lo': 0, 'cof': 1, 'ocf': 0, 'agc_val': 0b10011010 }) self.assertFalse(as5048.is_error) self.assertFalse(as5048.is_parity_mismatch)
def test_write(self, mock_spi): as5048 = AS5048(mock_spi) # Test default operation of write without any errors mock_spi.transfer.side_effect = [[0x00, 0x00], [0x01, 0x02], [0x80, 0x04]] response = as5048.write(AS5048.ADDR_ZERO_POSITION_HI, 0x0004) # Check calls self.assertEqual(mock_spi.transfer.call_count, 3) calls = [call([0x80, 0x16]), call([0x80, 0x04]), call([0xc0, 0x00])] mock_spi.transfer.assert_has_calls(calls) # Check final returns self.assertEqual(response['old_register'], 0x0102) self.assertEqual(response['new_register'], 0x0004) self.assertFalse(as5048.is_error, 'Error is set') self.assertFalse(as5048.is_parity_mismatch, 'Parity mismatch')
def test_write_zero_position(self, mock_spi): mock_spi.transfer.side_effect = [ [0, 0], [0, 3], # read angle [0, 0], [0, 0], # read pos high [0, 0], [0, 0], # read pos low [0, 0], [0, 0], [0, 0], # write pos high [0, 0], [0, 0], [0, 3] ] # write pos low as5048 = AS5048(mock_spi) response = as5048.write_zero_position() self.assertEqual(mock_spi.transfer.call_count, 12) self.assertDictEqual(response, { 'old_zero_position': 0, 'new_zero_position': 3 }) self.assertFalse(as5048.is_error) self.assertFalse(as5048.is_parity_mismatch)
def test_transfer_and_clear_error(self, mock_spi): as5048 = AS5048(mock_spi) # Normal behaviour without error mock_spi.transfer.return_value = [0x3f, 0xff] value = as5048.transfer(0x3fff) mock_spi.transfer.assert_called_once_with([0x3f, 0xff]) self.assertEqual(value, 0x3fff) self.assertFalse(as5048.is_error, 'Error is set') self.assertFalse(as5048.is_parity_mismatch, 'Parity is mismatched') # Receives a parity mismatch from the spi mock_spi.reset_mock() mock_spi.transfer.return_value = [0xbf, 0xff] value = as5048.transfer(0xbfff) mock_spi.transfer.assert_called_once_with( [0xbf, 0xff]) # 0xffff since a even parity should be set self.assertEqual(value, 0x3fff) self.assertFalse(as5048.is_error, 'Error is set') self.assertTrue(as5048.is_parity_mismatch, 'Parity is not mismatched') # Clear errors mock_spi.reset_mock() mock_spi.transfer.return_value = [0, 3] errors = as5048.clear_and_get_error() self.assertEqual(mock_spi.transfer.call_count, 2) self.assertDictEqual(errors, { 'parity_error': 0, 'command_invalid': 1, 'framing_error': 1 }) self.assertFalse(as5048.is_error) self.assertFalse(as5048.is_parity_mismatch) # SPI returns with an error bit set mock_spi.reset_mock() mock_spi.transfer.return_value = [0x7f, 0x1f] value = as5048.transfer(0x0001) mock_spi.transfer.assert_called_once_with([0x80, 0x01]) self.assertEqual(value, 0x3f1f) self.assertTrue(as5048.is_error, 'Error is not set') self.assertFalse(as5048.is_parity_mismatch, 'Parity is mismatched') # Clear errors mock_spi.reset_mock() mock_spi.transfer.return_value = [0x80, 4] errors = as5048.clear_and_get_error() self.assertDictEqual(errors, { 'parity_error': 1, 'command_invalid': 0, 'framing_error': 0 }) self.assertFalse(as5048.is_error) self.assertFalse(as5048.is_parity_mismatch) # Parity mismatch and error bit set mock_spi.reset_mock() mock_spi.transfer.return_value = [0xff, 0x45] value = as5048.transfer(0xffff) mock_spi.transfer.assert_called_once_with([0xff, 0xff]) self.assertEqual(value, 0x3f45) self.assertTrue(as5048.is_error, 'Error is not set') self.assertTrue(as5048.is_parity_mismatch, 'Parity is not mismatched') # Make another call with error still set mock_spi.reset_mock() mock_spi.transfer.return_value = [0x3f, 0xff] value = as5048.transfer(0xffff) mock_spi.transfer.assert_called_once_with([0xff, 0xff]) self.assertTrue(mock_spi.transfer.called) self.assertEqual(value, 0x3fff) self.assertTrue(as5048.is_error, 'Error is not set') self.assertTrue(as5048.is_parity_mismatch, 'Parity is mismatched') # Clear errors mock_spi.reset_mock() mock_spi.transfer.return_value = [0, 0] errors = as5048.clear_and_get_error() self.assertDictEqual(errors, { 'parity_error': 0, 'command_invalid': 0, 'framing_error': 0 }) self.assertFalse(as5048.is_error) self.assertFalse(as5048.is_parity_mismatch)
def test_calc_angle(self): self.assertEqual(AS5048.calc_angle(0), 0) self.assertEqual(AS5048.calc_angle(0x1000), math.pi / 2) self.assertEqual(AS5048.calc_angle(0x2000), math.pi) self.assertEqual(AS5048.calc_angle(0x3000), math.pi * 3 / 2) self.assertEqual(AS5048.calc_angle(0x4000), 2 * math.pi)
def test_calc_even_parity(self): # The parity should alternate when removing the MSB p = 1 for i in range(0, 15): self.assertEqual(AS5048.calc_even_parity(0x7fff >> i), p) p = 1 - p # Random tests self.assertEqual(AS5048.calc_even_parity(0b000010000010000), 0) self.assertEqual(AS5048.calc_even_parity(0b000010010010000), 1) self.assertEqual(AS5048.calc_even_parity(0b001010011000101), 0) self.assertEqual(AS5048.calc_even_parity(0b101100101001011), 0) self.assertEqual(AS5048.calc_even_parity(0b011001100010000), 1) self.assertEqual(AS5048.calc_even_parity(0b11110000010000), 1) self.assertEqual(AS5048.calc_even_parity(0b001011011110100), 0) self.assertEqual(AS5048.calc_even_parity(0b110000110010111), 0) self.assertEqual(AS5048.calc_even_parity(0b110111100110101), 0) self.assertEqual(AS5048.calc_even_parity(0b101010101010101), 0) self.assertEqual(AS5048.calc_even_parity(0b010101010101010), 1)
def test_check_error_flag(self): self.assertTrue(AS5048.check_error_flag(0b0100100101100001)) self.assertFalse(AS5048.check_error_flag(0b0000100101100001))
def test_check_parity_error(self): self.assertTrue(AS5048.check_parity_error(0b1001001001010000)) self.assertTrue(AS5048.check_parity_error(0b0010110101100100)) self.assertFalse(AS5048.check_parity_error(0b1000010001010000)) self.assertFalse(AS5048.check_parity_error(0b0000110101100001))
# Get parameters device = rospy.get_param('~spi/device', '/dev/spidev0.0') mode = rospy.get_param('~spi/mode', 1) max_hz = rospy.get_param('~spi/max_speed', 1000000) queue_size = rospy.get_param('~publisher_queue_size', 10) rate = rospy.get_param('~rate') create_mock = rospy.get_param('~create_mock', False) # Check if the mock parameter is set to run without a sensor if create_mock: spi = create_autospec(SPI) spi.transfer.return_value = [0, 0] else: spi = SPI(device, mode, max_hz) as5048a = AS5048(spi) # Setup publishers pub_angle = rospy.Publisher('angle_rad', Float32, queue_size=queue_size) pub_mag = rospy.Publisher('magnitude_raw', Int16, queue_size=queue_size) # Setup services s1 = rospy.Service('windvane_write_zero_position', Trigger, handle_write_zero_position) s2 = rospy.Service('windvane_read_diagnostics', Trigger, handle_read_diagnostics) # Apply node rate rate = rospy.Rate(rate) # Clear existing errors errors = as5048a.clear_and_get_error() if any(errors.values()): rospy.loginfo( 'Clear Error; Parity Error {parity_error}, Command Invalid {command_invalid}, Framing Error {framing_error}'