Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
700 | killagreg | 1 | /* |
2 | |||
741 | killagreg | 3 | Copyright 2008, by Killagreg |
700 | killagreg | 4 | |
741 | killagreg | 5 | This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify |
700 | killagreg | 6 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
7 | either version 3 of the License, or (at your option) any later version. |
||
8 | This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; |
||
9 | without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||
10 | GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License |
||
11 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
||
12 | |||
741 | killagreg | 13 | Please note: The original implementation was done by Niklas Nold. |
14 | All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
||
700 | killagreg | 15 | */ |
16 | #include <stdlib.h> |
||
17 | #include <avr/io.h> |
||
18 | #include <avr/interrupt.h> |
||
19 | |||
20 | #include "mm3.h" |
||
21 | #include "main.h" |
||
22 | #include "mymath.h" |
||
23 | #include "fc.h" |
||
24 | #include "timer0.h" |
||
25 | #include "rc.h" |
||
26 | #include "eeprom.h" |
||
27 | |||
28 | #define MAX_AXIS_VALUE 500 |
||
29 | |||
30 | |||
31 | typedef struct |
||
32 | { |
||
33 | uint8_t STATE; |
||
34 | uint16_t DRDY; |
||
35 | uint8_t AXIS; |
||
36 | int16_t x_axis; |
||
37 | int16_t y_axis; |
||
38 | int16_t z_axis; |
||
39 | } MM3_working_t; |
||
40 | |||
41 | |||
42 | // MM3 State Machine |
||
43 | #define MM3_STATE_RESET 0 |
||
44 | #define MM3_STATE_START_TRANSFER 1 |
||
45 | #define MM3_STATE_WAIT_DRDY 2 |
||
46 | #define MM3_STATE_DRDY 3 |
||
47 | #define MM3_STATE_BYTE2 4 |
||
48 | |||
49 | #define MM3_X_AXIS 0x01 |
||
50 | #define MM3_Y_AXIS 0x02 |
||
51 | #define MM3_Z_AXIS 0x03 |
||
52 | |||
53 | |||
54 | #define MM3_PERIOD_32 0x00 |
||
55 | #define MM3_PERIOD_64 0x10 |
||
56 | #define MM3_PERIOD_128 0x20 |
||
57 | #define MM3_PERIOD_256 0x30 |
||
58 | #define MM3_PERIOD_512 0x40 |
||
59 | #define MM3_PERIOD_1024 0x50 |
||
60 | #define MM3_PERIOD_2048 0x60 |
||
61 | #define MM3_PERIOD_4096 0x70 |
||
62 | |||
63 | MM3_calib_t MM3_calib; |
||
64 | volatile MM3_working_t MM3; |
||
754 | killagreg | 65 | volatile uint8_t MM3_Timeout = 0; |
700 | killagreg | 66 | |
67 | |||
68 | |||
69 | /*********************************************/ |
||
70 | /* Initialize Interface to MM3 Compass */ |
||
71 | /*********************************************/ |
||
726 | killagreg | 72 | void MM3_Init(void) |
700 | killagreg | 73 | { |
74 | uint8_t sreg = SREG; |
||
75 | |||
76 | cli(); |
||
77 | |||
78 | // Configure Pins for SPI |
||
79 | // set SCK (PB7), MOSI (PB5) as output |
||
80 | DDRB |= (1<<DDB7)|(1<<DDB5); |
||
81 | // set MISO (PB6) as input |
||
82 | DDRB &= ~(1<<DDB6); |
||
83 | |||
84 | // Output Pins PC4->MM3_SS ,PC5->MM3_RESET |
||
85 | DDRC |= (1<<DDC4)|(1<<DDC5); |
||
86 | // set pins permanent to low |
||
87 | PORTC &= ~((1<<PORTC4)|(1<<PORTC5)); |
||
88 | |||
89 | // Initialize SPI-Interface |
||
90 | // Enable interrupt (SPIE=1) |
||
91 | // Enable SPI bus (SPE=1) |
||
92 | // MSB transmitted first (DORD = 0) |
||
93 | // Master SPI Mode (MSTR=1) |
||
94 | // Clock polarity low whn idle (CPOL=0) |
||
95 | // clock phase sample at leading edge (CPHA=0) |
||
96 | // clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz |
||
97 | SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0); |
||
98 | SPSR &= ~(1<<SPI2X); |
||
99 | |||
100 | // Init Statemachine |
||
101 | MM3.AXIS = MM3_X_AXIS; |
||
102 | MM3.STATE = MM3_STATE_RESET; |
||
103 | |||
104 | // Read calibration from EEprom |
||
105 | MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF); |
||
106 | MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF); |
||
107 | MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
||
108 | MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
||
109 | MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
||
110 | MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
||
111 | |||
741 | killagreg | 112 | MM3_Timeout = 0; |
113 | |||
700 | killagreg | 114 | SREG = sreg; |
115 | } |
||
116 | |||
117 | |||
118 | /*********************************************/ |
||
119 | /* Get Data from MM3 */ |
||
120 | /*********************************************/ |
||
754 | killagreg | 121 | void MM3_Update(void) // called every 102.4 µs by timer 0 ISR |
700 | killagreg | 122 | { |
123 | switch (MM3.STATE) |
||
124 | { |
||
125 | case MM3_STATE_RESET: |
||
741 | killagreg | 126 | PORTC &= ~(1<<PORTC4); // select slave |
700 | killagreg | 127 | PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset |
128 | MM3.STATE = MM3_STATE_START_TRANSFER; |
||
129 | return; |
||
130 | |||
131 | case MM3_STATE_START_TRANSFER: |
||
132 | PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level) |
||
133 | |||
134 | // write to SPDR triggers automatically the transfer MOSI MISO |
||
135 | // MM3 Period, + AXIS code |
||
741 | killagreg | 136 | switch(MM3.AXIS) |
137 | { |
||
138 | case MM3_X_AXIS: |
||
139 | SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
||
140 | break; |
||
141 | case MM3_Y_AXIS: |
||
142 | SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
||
143 | break; |
||
144 | case MM3_Z_AXIS: |
||
145 | SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; |
||
146 | break; |
||
147 | default: |
||
148 | MM3.AXIS = MM3_X_AXIS; |
||
149 | MM3.STATE = MM3_STATE_RESET; |
||
150 | return; |
||
151 | } |
||
700 | killagreg | 152 | |
153 | // DRDY line is not connected, therefore |
||
154 | // wait before reading data back |
||
155 | MM3.DRDY = SetDelay(8); // wait 8ms for data ready |
||
156 | MM3.STATE = MM3_STATE_WAIT_DRDY; |
||
157 | return; |
||
158 | |||
159 | case MM3_STATE_WAIT_DRDY: |
||
160 | if (CheckDelay(MM3.DRDY)) |
||
161 | { |
||
162 | // write something into SPDR to trigger data reading |
||
163 | SPDR = 0x00; |
||
164 | MM3.STATE = MM3_STATE_DRDY; |
||
165 | } |
||
166 | return; |
||
167 | } |
||
168 | } |
||
169 | |||
170 | |||
171 | /*********************************************/ |
||
172 | /* Interrupt SPI transfer complete */ |
||
173 | /*********************************************/ |
||
174 | ISR(SPI_STC_vect) |
||
175 | { |
||
176 | static int8_t tmp; |
||
177 | int16_t value; |
||
178 | |||
179 | switch (MM3.STATE) |
||
180 | { |
||
181 | // 1st byte received |
||
182 | case MM3_STATE_DRDY: |
||
183 | tmp = SPDR; // store 1st byte |
||
184 | SPDR = 0x00; // trigger transfer of 2nd byte |
||
185 | MM3.STATE = MM3_STATE_BYTE2; |
||
186 | return; |
||
187 | |||
188 | case MM3_STATE_BYTE2: // 2nd byte received |
||
189 | value = (int16_t)tmp; // combine the 1st and 2nd byte to a word |
||
190 | value <<= 8; // shift 1st byte to MSB-Position |
||
191 | value |= (int16_t)SPDR; // add 2nd byte |
||
192 | |||
193 | if(abs(value) < MAX_AXIS_VALUE) // ignore spikes |
||
194 | { |
||
195 | switch (MM3.AXIS) |
||
196 | { |
||
197 | case MM3_X_AXIS: |
||
198 | MM3.x_axis = value; |
||
199 | MM3.AXIS = MM3_Y_AXIS; |
||
200 | break; |
||
201 | case MM3_Y_AXIS: |
||
202 | MM3.y_axis = value; |
||
203 | MM3.AXIS = MM3_Z_AXIS; |
||
204 | break; |
||
205 | case MM3_Z_AXIS: |
||
206 | MM3.z_axis = value; |
||
207 | MM3.AXIS = MM3_X_AXIS; |
||
208 | break; |
||
209 | default: |
||
210 | MM3.AXIS = MM3_X_AXIS; |
||
211 | break; |
||
212 | } |
||
213 | } |
||
727 | killagreg | 214 | PORTC |= (1<<PORTC4); // deselect slave |
700 | killagreg | 215 | MM3.STATE = MM3_STATE_RESET; |
741 | killagreg | 216 | // Update timeout is called every 102.4 µs. |
217 | // It takes 2 cycles to write a measurement data request for one axis and |
||
218 | // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back. |
||
219 | // I.e. 81 cycles * 102.4 µs = 8.3ms per axis. |
||
220 | // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() - |
||
221 | // decremtent the MM3_Timeout every 100 ms. |
||
222 | // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout. |
||
223 | if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis)) |
||
224 | { // if all axis measurements give diffrent readings the data should be valid |
||
225 | if(MM3_Timeout < 20) MM3_Timeout++; |
||
226 | } |
||
227 | else // something is very strange here |
||
228 | { |
||
229 | if(MM3_Timeout ) MM3_Timeout--; |
||
230 | } |
||
231 | return; |
||
232 | |||
233 | default: |
||
234 | return; |
||
700 | killagreg | 235 | } |
236 | } |
||
237 | |||
238 | |||
239 | |||
240 | /*********************************************/ |
||
241 | /* Calibrate Compass */ |
||
242 | /*********************************************/ |
||
726 | killagreg | 243 | void MM3_Calibrate(void) |
700 | killagreg | 244 | { |
245 | int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
||
246 | uint8_t measurement = 50, beeper = 0; |
||
247 | uint16_t timer; |
||
248 | |||
249 | GRN_ON; |
||
250 | ROT_OFF; |
||
251 | |||
252 | // get maximum and minimum reading of all axis |
||
741 | killagreg | 253 | while (measurement && !MM3_Timeout) |
700 | killagreg | 254 | { |
255 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
||
256 | else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
||
257 | |||
258 | if (MM3.y_axis > y_max) y_max = MM3.y_axis; |
||
259 | else if (MM3.y_axis < y_min) y_min = MM3.y_axis; |
||
260 | |||
261 | if (MM3.z_axis > z_max) z_max = MM3.z_axis; |
||
262 | else if (MM3.z_axis < z_min) z_min = MM3.z_axis; |
||
263 | |||
264 | if (!beeper) |
||
265 | { |
||
266 | ROT_FLASH; |
||
267 | GRN_FLASH; |
||
268 | BeepTime = 50; |
||
269 | beeper = 50; |
||
270 | } |
||
271 | beeper--; |
||
272 | // loop with period of 10 ms / 100 Hz |
||
273 | timer = SetDelay(10); |
||
274 | while(!CheckDelay(timer)); |
||
275 | |||
707 | killagreg | 276 | // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
277 | if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
||
700 | killagreg | 278 | } |
741 | killagreg | 279 | if(!MM3_Timeout) |
280 | { |
||
281 | // Rage of all axis |
||
282 | MM3_calib.X_range = (x_max - x_min); |
||
283 | MM3_calib.Y_range = (y_max - y_min); |
||
284 | MM3_calib.Z_range = (z_max - z_min); |
||
700 | killagreg | 285 | |
741 | killagreg | 286 | // Offset of all axis |
287 | MM3_calib.X_off = (x_max + x_min) / 2; |
||
288 | MM3_calib.Y_off = (y_max + y_min) / 2; |
||
289 | MM3_calib.Z_off = (z_max + z_min) / 2; |
||
700 | killagreg | 290 | |
741 | killagreg | 291 | // save to EEProm |
292 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
||
293 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
||
294 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
||
295 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
||
296 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
||
297 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
||
298 | } |
||
700 | killagreg | 299 | } |
300 | |||
301 | |||
302 | /*********************************************/ |
||
303 | /* Calculate north direction (heading) */ |
||
304 | /*********************************************/ |
||
726 | killagreg | 305 | int16_t MM3_Heading(void) |
700 | killagreg | 306 | { |
701 | killagreg | 307 | int32_t sin_pitch, cos_pitch, sin_roll, cos_roll, sin_yaw, cos_yaw; |
700 | killagreg | 308 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
309 | int16_t angle; |
||
310 | uint16_t div_factor; |
||
311 | int16_t heading; |
||
312 | |||
741 | killagreg | 313 | if (MM3_Timeout) |
314 | { |
||
315 | // calibration factor for transforming Gyro Integrals to angular degrees |
||
316 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
||
700 | killagreg | 317 | |
741 | killagreg | 318 | // Offset correction and normalization (values of H are +/- 512) |
319 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
||
320 | Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
||
321 | Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
||
700 | killagreg | 322 | |
741 | killagreg | 323 | // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
324 | // assuming the MM3 board is mounted parallel to the frame. |
||
325 | // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
||
326 | // in a top view counter clockwise direction. |
||
327 | // North is in opposite direction of the small arrow on the MM3 board. |
||
328 | // Therefore 180 deg must be added to that angle. |
||
329 | angle = ((int16_t)ParamSet.UserParam4 + 180); |
||
330 | // wrap angle to interval of 0°- 359° |
||
331 | angle += 360; |
||
332 | angle %= 360; |
||
333 | sin_yaw = (int32_t)(c_sin_8192(angle)); |
||
334 | cos_yaw = (int32_t)(c_cos_8192(angle)); |
||
700 | killagreg | 335 | |
741 | killagreg | 336 | Hx_corr = Hx; |
337 | Hy_corr = Hy; |
||
700 | killagreg | 338 | |
741 | killagreg | 339 | // rotate |
340 | Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
||
341 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
||
700 | killagreg | 342 | |
343 | |||
741 | killagreg | 344 | // tilt compensation |
700 | killagreg | 345 | |
741 | killagreg | 346 | // calibration factor for transforming Gyro Integrals to angular degrees |
347 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
||
700 | killagreg | 348 | |
741 | killagreg | 349 | // calculate sinus cosinus of pitch and tilt angle |
350 | angle = (IntegralPitch/div_factor); |
||
351 | sin_pitch = (int32_t)(c_sin_8192(angle)); |
||
352 | cos_pitch = (int32_t)(c_cos_8192(angle)); |
||
700 | killagreg | 353 | |
741 | killagreg | 354 | angle = (IntegralRoll/div_factor); |
355 | sin_roll = (int32_t)(c_sin_8192(angle)); |
||
356 | cos_roll = (int32_t)(c_cos_8192(angle)); |
||
700 | killagreg | 357 | |
741 | killagreg | 358 | Hx_corr = Hx * cos_pitch; |
359 | Hx_corr -= Hz * sin_pitch; |
||
360 | Hx_corr /= 8192; |
||
700 | killagreg | 361 | |
741 | killagreg | 362 | Hy_corr = Hy * cos_roll; |
363 | Hy_corr += Hz * sin_roll; |
||
364 | Hy_corr /= 8192; |
||
700 | killagreg | 365 | |
741 | killagreg | 366 | // calculate Heading |
367 | heading = c_atan2(Hy_corr, Hx_corr); |
||
700 | killagreg | 368 | |
741 | killagreg | 369 | // atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
370 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
||
371 | if (heading < 0) heading = -heading; |
||
372 | else heading = 360 - heading; |
||
373 | } |
||
374 | else // MM3_Timeout = 0 i.e now new data from external board |
||
375 | { |
||
765 | killagreg | 376 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
741 | killagreg | 377 | heading = -1; |
378 | } |
||
700 | killagreg | 379 | return heading; |
380 | } |