def test_yaw_angles_again(self): accs = [ [-509, -175, 16452], [55, 766, 16242], [-24, -1059, 16365], [-670, -189, 16359], ] mags = [ [-5892, 2311, 4848], [-553, 1624, 5065], [-791, 7577, 5341], [-5849, 6998, 4939], ] expected_angles = [ (0, 2, 167.0), # (13+39) = 52 (3, 0, 48.0), # 119 (-4, 0, -72.0), # 120 (-1, 2, -141.0) # 69 ] for j, mag in enumerate(mags): mag = self.correct_mag(mag) acc = [a_i/32768.0 * sensorfusion.GRAVITY_EARTH for a_i in accs[j]] q = self._compute_attitude(mag, acc) roll, pitch, yaw = sensorfusion.roll_pitch_yaw(q) self.assertAlmostEqual(roll, expected_angles[j][0], 0) self.assertAlmostEqual(pitch, expected_angles[j][1], 0) self.assertAlmostEqual(yaw, expected_angles[j][2], 0)
def test_roll_angles_thrice(self): accs = [ [-17913, 103, -394], [-1007, -565, -16685], [16047, 844, -542], [-579, 805, 16121] ] mags = [ [1477, 7194, 2185], [-294, 7193, -4427], [-7156, 6615, -2931], [-5256, 7179, 4075], ] expected_angles = [ (166, 89, 107), (-178, 4, 58), (130, -86, -177), (3, 2, -137), ] for j, mag in enumerate(mags): mag = self.correct_mag(mag) acc = accs[j] q = self._compute_attitude(mag, acc) roll, pitch, yaw = sensorfusion.roll_pitch_yaw(q) self.assertAlmostEqual(roll, expected_angles[j][0], 0) self.assertAlmostEqual(pitch, expected_angles[j][1], 0) self.assertAlmostEqual(yaw, expected_angles[j][2], 0)
def test_roll_angles_again(self): accs = [ [514, 16009, -706], [726, 15930, -284], [221, 16006, -457], [1038, 16102, 69] ] mags = [ [-5913, -125, -1103], [-720, -599, -1646], [-1961, -366, 2760], [-5058, -177, 2515], ] expected_angles = [ (94, -2, -160), # 117 (91, -3, -43), # 125 (92, -2, 82), # 51 (88, -5, 133) # 67 ] for j, mag in enumerate(mags): mag = self.correct_mag(mag) acc = accs[j] q = self._compute_attitude(mag, acc) roll, pitch, yaw = sensorfusion.roll_pitch_yaw(q) self.assertAlmostEqual(roll, expected_angles[j][0], 0) self.assertAlmostEqual(pitch, expected_angles[j][1], 0) self.assertAlmostEqual(yaw, expected_angles[j][2], 0)
def _test_roll_angles(self): mags = [ [-4385, -1112, -1782], [-452, -1381, -793], [-5137, -870, 2033], [-1663, -974, 3062], ] expected_angles = [ (104.3, 0.0, 0.0), (131.6, 0.0, 0.0), (24.4, 0.0, 0.0), (121.3, 0.0, 0.0) ] for j, mag in enumerate(mags): mag = self.correct_mag(mag) q = self._compute_attitude_y(mag) roll, pitch, yaw = sensorfusion.roll_pitch_yaw(q) print roll, pitch, yaw self.fail('end-roll')
def test_yaw_angles(self): mags = [ [-4473, 6554, 4485], # BACK [-3629, 1532, 4938], # LEFT [-793, 6677, 4569], # RIGHT [-730, 2380, 5387], # FRONT ] expected_angles = [ (0, 0, -134.0), (0, 0, 138.0), (0, 0, -66.0), (0, 0, 31.0) ] for j, mag in enumerate(mags): mag = self.correct_mag(mag) q = self._compute_attitude_z(mag) roll, pitch, yaw = sensorfusion.roll_pitch_yaw(q) self.assertAlmostEqual(roll, expected_angles[j][0], 0) self.assertAlmostEqual(pitch, expected_angles[j][1], 0) self.assertAlmostEqual(yaw, expected_angles[j][2], 0)
def _test_pitch_angles(self): mags = [ [-6978, 5034, -2936], # DOOR WALL (0.6881979703903198, -0.162129208445549, -0.689984142780304, -0.1549822986125946) (93.02473730638378, -0.2234653060239575, -0.9510162856920528, -0.21361460479461566) [-7456, 1348, -1137], # MIRROR (0.6673268675804138, 0.22581857442855835, -0.6823622584342957, 0.19509662687778473) (96.27783020031163, 0.3032078377778739, -0.9162115449743273, 0.2619573103898254) [-7231, 6709, 732], # RIGHT WALL (0.5264194011688232, -0.46728214621543884, -0.5616878867149353, -0.4347834289073944) (116.47230823977264, -0.5495981954289173, -0.6606343756829183, -0.5113745300676028) [-7525, 3502, 2523] # WINDOW (0.63744056224823, -0.03323207050561905, -0.7693279385566711, -0.026450788602232933) (100.7975352980979, -0.043130552457690007, -0.998479435865259, -0.03432940253189996) ] expected_angles = [ (0, -134.0, 0), (0, 138.0, 0), (0, -66.0, 0), (0, 31.0, 0) ] for j, mag in enumerate(mags): mag = self.correct_mag(mag) q = self._compute_attitude_x(mag) roll, pitch, yaw = sensorfusion.roll_pitch_yaw(q) print roll, pitch, yaw # self.assertAlmostEqual(roll, expected_angles[j][0], 0) # self.assertAlmostEqual(pitch, expected_angles[j][1], 0) # self.assertAlmostEqual(yaw, expected_angles[j][2], 0) self.fail('end-pitch')