librobotcontrol-sys 0.4.0

Rust port of librobotcontrol
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117

/**
 * @file mpu_orientation_dev.c
 *
 * This prints out the DMP's integer interpretation of the rotation matrices.
 * It's meant to be compiled separately and only has the 'txt' extension to
 * prevent from being compiled in the library source.
 *
 * This is NOT part of the mpu library module and is NOT accessible by the user.
 */

#include <stdio.h>

/**
 * takes a single row on a rotation matrix and returns the associated scalar for
 * use by __inv_orientation_matrix_to_scalar.
 *
 * @param      row   The row
 *
 * @return     { description_of_the_return_value }
 */
static unsigned short __inv_row_2_scale(signed char row[])
{
	unsigned short b;

	if (row[0] > 0)
		b = 0;
	else if (row[0] < 0)
		b = 4;
	else if (row[1] > 0)
		b = 1;
	else if (row[1] < 0)
		b = 5;
	else if (row[2] > 0)
		b = 2;
	else if (row[2] < 0)
		b = 6;
	else
		b = 7;      // error
	return b;
}


/**
 * This take in a rotation matrix and returns the corresponding 16 bit short
 * which is sent to the DMP to set the orientation. This function is actually
 * not used in normal operation and only served to retrieve the orientation
 * scalars once to populate the rc_mpu_orientation_t enum during development.
 *
 * @param      mtx   The mtx
 *
 * @return     { description_of_the_return_value }
 */
static unsigned short __inv_orientation_matrix_to_scalar(signed char mtx[])
{
	unsigned short scalar;

	scalar = __inv_row_2_scale(mtx);
	scalar |= __inv_row_2_scale(mtx + 3) << 3;
	scalar |= __inv_row_2_scale(mtx + 6) << 6;
	return scalar;
}


/**
 * this was a debugging and development function and purely serves to print out
 * orientation values and rotation matrices which form the rc_imu_orientation_t
 * enum. This is not called inside this C file and is not exposed to the user.
 * left here for future reference.
 */
void main(void)
{
	printf("\n");
	//char mtx[9];
	unsigned short orient;

	// Z-UP (identity matrix)
	signed char zup[] = {1,0,0, 0,1,0, 0,0,1};
	orient = __inv_orientation_matrix_to_scalar(zup);
	printf("Z-UP: %d\n", orient);

	// Z-down Y-forward
	signed char zdown[] = {-1,0,0, 0,1,0, 0,0,-1};
	orient = __inv_orientation_matrix_to_scalar(zdown);
	printf("Z-down: %d\n", orient);

	// X-up Y-forward
	signed char xup[] = {0,0,-1, 0,1,0, 1,0,0};
	orient = __inv_orientation_matrix_to_scalar(xup);
	printf("x-up: %d\n", orient);

	// X-down Y-forward
	signed char xdown[] = {0,0,1, 0,1,0, -1,0,0};
	orient = __inv_orientation_matrix_to_scalar(xdown);
	printf("x-down: %d\n", orient);

	// Y-up Z-back
	signed char yup[] = {1,0,0, 0,0,-1, 0,1,0};
	orient = __inv_orientation_matrix_to_scalar(yup);
	printf("y-up: %d\n", orient);

	// Y-down Z forward
	signed char ydown[] = {1,0,0, 0,0,1, 0,-1,0};
	orient = __inv_orientation_matrix_to_scalar(ydown);
	printf("y-down: %d\n", orient);

	// X-forward Z up
	signed char xforward[] = {0,-1,0, 1,0,0, 0,0,1};
	orient = __inv_orientation_matrix_to_scalar(xforward);
	printf("x-forward: %d\n", orient);

	// X-back Z up
	signed char xback[] = {0,1,0, -1,0,0, 0,0,1};
	orient = __inv_orientation_matrix_to_scalar(xback);
	printf("yx-back: %d\n", orient);


}