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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
|
#include "mmc5983ma.hpp"
int16_t MMC5983MA::sat_sub(int16_t a, int16_t b) {
int32_t result = (int32_t) a - (int32_t) b;
if (result < INT16_MIN) {
result = INT16_MIN;
}
if (result > INT16_MAX) {
result = INT16_MAX;
}
return (int16_t) result;
};
void MMC5983MA::initialize() {
// Restart device prior to configuration
buffer[0] = R_MMC5983MA_INTERNAL_CTL1;
internal_ctl1.fields.RESTART = true;
buffer[1] = internal_ctl1.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
internal_ctl1.data = 0;
sleep_ms(100);
calibrate();
//Configure decimation filter bandwidth for 200 Hz
buffer[0] = R_MMC5983MA_INTERNAL_CTL1;
internal_ctl1.fields.BANDWIDTH = B_MMC5983MA_BANDWIDTH_200HZ;
buffer[1] = internal_ctl1.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
//Configure and enable continuous measurement mode for 200 Hz
buffer[0] = R_MMC5983MA_INTERNAL_CTL2;
internal_ctl2.fields.CONTINUOUS_MODE_ENABLE = true;
internal_ctl2.fields.CONTINUOUS_MODE_FREQ = B_MMC5983_CONTINUOUS_MODE_FREQ_200HZ;
internal_ctl2.fields.PERIODIC_SET_ENABLE = false;
internal_ctl2.fields.PERIODIC_SET_RATE = B_MMC5983_PERIODIC_SET_RATE_MEAS_TIMES_1000;
buffer[1] = internal_ctl2.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
printf("MMC5983MA Initialized!\n");
}
void MMC5983MA::sample() {
buffer[0] = R_MMC5983MA_XOUT0;
i2c_write_blocking(i2c, addr, buffer, 1, true);
i2c_read_blocking(i2c, addr, buffer, 6, false);
raw_x = (int16_t) ((((uint16_t) buffer[0] << 8) | buffer[1]) - 32768);
raw_y = (int16_t) ((((uint16_t) buffer[2] << 8) | buffer[3]) - 32768);
raw_z = (int16_t) ((((uint16_t) buffer[4] << 8) | buffer[5]) - 32768);
}
void MMC5983MA::apply_offset() {
ax = sat_sub(raw_x, offset_x);
ay = sat_sub(raw_y, offset_y);
az = sat_sub(raw_z, offset_z);
}
void MMC5983MA::calibrate() {
sleep_ms(100);
printf("MMC5983MA Calibration | Set command");
// Take measurement after SET command completes
buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
internal_ctl0.fields.SET_CMD = true;
buffer[1] = internal_ctl0.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
internal_ctl0.data = 0;
sleep_ms(100);
printf(" | Measurement Request");
buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
internal_ctl0.fields.TAKE_MAG_MEAS = true;
buffer[1] = internal_ctl0.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
internal_ctl0.data = 0;
printf(" | Request sent, awaiting completion...\n");
size_t counter = 0;
while (!dev_status.fields.MEAS_M_DONE) {
sleep_ms(1);
buffer[0] = R_MMC5983MA_DEV_STATUS;
i2c_write_blocking(i2c, addr, buffer, 1, true);
i2c_read_blocking(i2c, addr, buffer, 1, false);
dev_status.data = buffer[0];
printf("W | ");
counter++;
if (counter == 20) {
printf("\n");
counter = 0;
}
}
printf("\n\nMeasurement taken!");
sample();
printf(" | Sample taken!");
int16_t set_ax = raw_x, set_ay = raw_y, set_az = raw_z;
// Take measurement after RESET command completes
buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
internal_ctl0.fields.RESET_CMD = true;
buffer[1] = internal_ctl0.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
internal_ctl0.data = 0;
sleep_ms(100);
buffer[0] = R_MMC5983MA_INTERNAL_CTL0;
internal_ctl0.fields.TAKE_MAG_MEAS = true;
buffer[1] = internal_ctl0.data;
i2c_write_blocking(i2c, addr, buffer, 2, false);
internal_ctl0.data = 0;
while (!dev_status.fields.MEAS_M_DONE) {
sleep_ms(1);
buffer[0] = R_MMC5983MA_DEV_STATUS;
i2c_write_blocking(i2c, addr, buffer, 1, true);
i2c_read_blocking(i2c, addr, buffer, 1, false);
dev_status.data = buffer[0];
}
sample();
int16_t reset_ax = raw_x, reset_ay = raw_y, reset_az = raw_z;
sleep_ms(100);
// Average the two measurements taken and assign them as offsets
offset_x = (int16_t) (((int32_t) set_ax + (int32_t) reset_ax) / 2);
offset_y = (int16_t) (((int32_t) set_ay + (int32_t) reset_ay) / 2);
offset_z = (int16_t) (((int32_t) set_az + (int32_t) reset_az) / 2);
#if ( DEBUG == 1 )
printf("MMC5983MA: Calibrated!\n\toffset_x: %" PRIi16 "\n\toffset_y: %" PRIi16 "\n\toffset_z: %" PRIi16 "\n", offset_x, offset_y, offset_z);
#endif
}
#if (USE_FREERTOS == 1)
void MMC5983MA::update_mmc5983ma_task(void* pvParameters) {
TickType_t xLastWakeTime;
const TickType_t xFrequency = pdMS_TO_TICKS(1000 / MMC5983_SAMPLE_RATE_HZ);
xLastWakeTime = xTaskGetTickCount();
while (1) {
vTaskDelayUntil(&xLastWakeTime, xFrequency);
taskENTER_CRITICAL();
MMC5983MA* mag = (MMC5983MA *) pvParameters;
mag->sample();
mag->apply_offset();
taskEXIT_CRITICAL();
if ((xLastWakeTime + xFrequency) < xTaskGetTickCount()) {
xLastWakeTime = xTaskGetTickCount();
}
}
}
#endif
|
