Rev 1179 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1179 | Rev 1199 | ||
---|---|---|---|
1 | /* |
1 | /* |
2 | 2 | ||
3 | Copyright 2008, by Killagreg |
3 | Copyright 2008, by Killagreg |
4 | 4 | ||
5 | This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify |
5 | This program (files mm3.c and mm3.h) is free software; you can redistribute it and/or modify |
6 | it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; |
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. |
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; |
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 |
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 |
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/>. |
11 | along with this program. If not, see <http://www.gnu.org/licenses/>. |
12 | 12 | ||
13 | Please note: The original implementation was done by Niklas Nold. |
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 |
14 | All the other files for the project "Mikrokopter" by H. Buss are under the license (license_buss.txt) published by www.mikrokopter.de |
15 | */ |
15 | */ |
16 | #include <stdlib.h> |
16 | #include <stdlib.h> |
17 | #include <avr/io.h> |
17 | #include <avr/io.h> |
18 | #include <avr/interrupt.h> |
18 | #include <avr/interrupt.h> |
19 | #include <inttypes.h> |
19 | #include <inttypes.h> |
20 | 20 | ||
21 | #include "mm3.h" |
21 | #include "mm3.h" |
22 | #include "main.h" |
22 | #include "main.h" |
23 | #include "mymath.h" |
23 | #include "mymath.h" |
24 | #include "fc.h" |
24 | #include "fc.h" |
25 | #include "timer0.h" |
25 | #include "timer0.h" |
26 | #include "rc.h" |
26 | #include "rc.h" |
27 | #include "eeprom.h" |
27 | #include "eeprom.h" |
28 | #include "printf_P.h" |
28 | #include "printf_P.h" |
- | 29 | ||
- | 30 | ||
- | 31 | // for compatibility reasons gcc3.x <-> gcc4.x |
|
- | 32 | #ifndef SPCR |
|
- | 33 | #define SPCR SPCR0 |
|
- | 34 | #endif |
|
- | 35 | #ifndef SPIE |
|
- | 36 | #define SPIE SPIE0 |
|
- | 37 | #endif |
|
- | 38 | #ifndef SPE |
|
- | 39 | #define SPE SPE0 |
|
- | 40 | #endif |
|
- | 41 | #ifndef DORD |
|
- | 42 | #define DORD DORD0 |
|
- | 43 | #endif |
|
- | 44 | #ifndef MSTR |
|
- | 45 | #define MSTR MSTR0 |
|
- | 46 | #endif |
|
- | 47 | #ifndef CPOL |
|
- | 48 | #define CPOL CPOL0 |
|
- | 49 | #endif |
|
- | 50 | #ifndef CPHA |
|
- | 51 | #define CPHA CPHA0 |
|
- | 52 | #endif |
|
- | 53 | #ifndef SPR1 |
|
- | 54 | #define SPR1 SPR01 |
|
- | 55 | #endif |
|
- | 56 | #ifndef SPR0 |
|
- | 57 | #define SPR0 SPR00 |
|
- | 58 | #endif |
|
- | 59 | ||
- | 60 | #ifndef SPDR |
|
- | 61 | #define SPDR SPDR0 |
|
- | 62 | #endif |
|
- | 63 | ||
- | 64 | #ifndef SPSR |
|
- | 65 | #define SPSR SPSR0 |
|
- | 66 | #endif |
|
- | 67 | #ifndef SPIF |
|
- | 68 | #define SPIF SPIF0 |
|
- | 69 | #endif |
|
- | 70 | #ifndef WCOL |
|
- | 71 | #define WCOL WCOL0 |
|
- | 72 | #endif |
|
- | 73 | #ifndef SPI2X |
|
- | 74 | #define SPI2X SPI2X0 |
|
- | 75 | #endif |
|
- | 76 | // ------------------------- |
|
- | 77 | ||
29 | 78 | ||
30 | #define MAX_AXIS_VALUE 500 |
79 | #define MAX_AXIS_VALUE 500 |
31 | 80 | ||
32 | 81 | ||
33 | typedef struct |
82 | typedef struct |
34 | { |
83 | { |
35 | uint8_t STATE; |
84 | uint8_t STATE; |
36 | uint16_t DRDY; |
85 | uint16_t DRDY; |
37 | uint8_t AXIS; |
86 | uint8_t AXIS; |
38 | int16_t x_axis; |
87 | int16_t x_axis; |
39 | int16_t y_axis; |
88 | int16_t y_axis; |
40 | int16_t z_axis; |
89 | int16_t z_axis; |
41 | } MM3_working_t; |
90 | } MM3_working_t; |
42 | 91 | ||
43 | 92 | ||
44 | // MM3 State Machine |
93 | // MM3 State Machine |
45 | #define MM3_STATE_RESET 0 |
94 | #define MM3_STATE_RESET 0 |
46 | #define MM3_STATE_START_TRANSFER 1 |
95 | #define MM3_STATE_START_TRANSFER 1 |
47 | #define MM3_STATE_WAIT_DRDY 2 |
96 | #define MM3_STATE_WAIT_DRDY 2 |
48 | #define MM3_STATE_DRDY 3 |
97 | #define MM3_STATE_DRDY 3 |
49 | #define MM3_STATE_BYTE2 4 |
98 | #define MM3_STATE_BYTE2 4 |
50 | 99 | ||
51 | #define MM3_X_AXIS 0x01 |
100 | #define MM3_X_AXIS 0x01 |
52 | #define MM3_Y_AXIS 0x02 |
101 | #define MM3_Y_AXIS 0x02 |
53 | #define MM3_Z_AXIS 0x03 |
102 | #define MM3_Z_AXIS 0x03 |
54 | 103 | ||
55 | 104 | ||
56 | #define MM3_PERIOD_32 0x00 |
105 | #define MM3_PERIOD_32 0x00 |
57 | #define MM3_PERIOD_64 0x10 |
106 | #define MM3_PERIOD_64 0x10 |
58 | #define MM3_PERIOD_128 0x20 |
107 | #define MM3_PERIOD_128 0x20 |
59 | #define MM3_PERIOD_256 0x30 |
108 | #define MM3_PERIOD_256 0x30 |
60 | #define MM3_PERIOD_512 0x40 |
109 | #define MM3_PERIOD_512 0x40 |
61 | #define MM3_PERIOD_1024 0x50 |
110 | #define MM3_PERIOD_1024 0x50 |
62 | #define MM3_PERIOD_2048 0x60 |
111 | #define MM3_PERIOD_2048 0x60 |
63 | #define MM3_PERIOD_4096 0x70 |
112 | #define MM3_PERIOD_4096 0x70 |
64 | 113 | ||
65 | #if defined(USE_WALTER_EXT) // walthers board |
114 | #if defined(USE_WALTER_EXT) // walthers board |
66 | // Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET |
115 | // Output Pins (J9)PC6->MM3_SS ,(J8)PB2->MM3_RESET |
67 | #define MM3_SS_PORT PORTC //J9->MM3_SS |
116 | #define MM3_SS_PORT PORTC //J9->MM3_SS |
68 | #define MM3_SS_DDR DDRC |
117 | #define MM3_SS_DDR DDRC |
69 | #define MM3_SS_PIN PC6 |
118 | #define MM3_SS_PIN PC6 |
70 | #define MM3_RESET_PORT PORTB //J8->MM3_RESET |
119 | #define MM3_RESET_PORT PORTB //J8->MM3_RESET |
71 | #define MM3_RESET_DDR DDRB |
120 | #define MM3_RESET_DDR DDRB |
72 | #define MM3_RESET_PIN PB2 |
121 | #define MM3_RESET_PIN PB2 |
73 | #elif defined(USE_NICK666) // nick666 version 0.67g |
122 | #elif defined(USE_NICK666) // nick666 version 0.67g |
74 | #define MM3_SS_PORT PORTD //J5->MM3_SS |
123 | #define MM3_SS_PORT PORTD //J5->MM3_SS |
75 | #define MM3_SS_DDR DDRD |
124 | #define MM3_SS_DDR DDRD |
76 | #define MM3_SS_PIN PD3 |
125 | #define MM3_SS_PIN PD3 |
77 | #define MM3_RESET_PORT PORTB //J8->MM3_RESET |
126 | #define MM3_RESET_PORT PORTB //J8->MM3_RESET |
78 | #define MM3_RESET_DDR DDRB |
127 | #define MM3_RESET_DDR DDRB |
79 | #define MM3_RESET_PIN PB2 |
128 | #define MM3_RESET_PIN PB2 |
80 | #else // killagregs board |
129 | #else // killagregs board |
81 | // Output Pins PC4->MM3_SS ,PC5->MM3_RESET |
130 | // Output Pins PC4->MM3_SS ,PC5->MM3_RESET |
82 | #define MM3_SS_PORT PORTC |
131 | #define MM3_SS_PORT PORTC |
83 | #define MM3_SS_DDR DDRC |
132 | #define MM3_SS_DDR DDRC |
84 | #define MM3_SS_PIN PC4 |
133 | #define MM3_SS_PIN PC4 |
85 | #define MM3_RESET_PORT PORTC |
134 | #define MM3_RESET_PORT PORTC |
86 | #define MM3_RESET_DDR DDRC |
135 | #define MM3_RESET_DDR DDRC |
87 | #define MM3_RESET_PIN PC5 |
136 | #define MM3_RESET_PIN PC5 |
88 | #endif |
137 | #endif |
89 | 138 | ||
90 | #define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN); |
139 | #define MM3_SS_ON MM3_SS_PORT &= ~(1<<MM3_SS_PIN); |
91 | #define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN); |
140 | #define MM3_SS_OFF MM3_SS_PORT |= (1<<MM3_SS_PIN); |
92 | #define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN); |
141 | #define MM3_RESET_ON MM3_RESET_PORT |= (1<<MM3_RESET_PIN); |
93 | #define MM3_RESET_OFF MM3_RESET_PORT &= ~(1<<MM3_RESET_PIN); |
142 | #define MM3_RESET_OFF MM3_RESET_PORT &= ~(1<<MM3_RESET_PIN); |
94 | 143 | ||
95 | 144 | ||
96 | 145 | ||
97 | MM3_calib_t MM3_calib; |
146 | MM3_calib_t MM3_calib; |
98 | volatile MM3_working_t MM3; |
147 | volatile MM3_working_t MM3; |
99 | volatile uint8_t MM3_Timeout = 0; |
148 | volatile uint8_t MM3_Timeout = 0; |
100 | 149 | ||
101 | 150 | ||
102 | 151 | ||
103 | /*********************************************/ |
152 | /*********************************************/ |
104 | /* Initialize Interface to MM3 Compass */ |
153 | /* Initialize Interface to MM3 Compass */ |
105 | /*********************************************/ |
154 | /*********************************************/ |
106 | void MM3_Init(void) |
155 | void MM3_Init(void) |
107 | { |
156 | { |
108 | uint8_t sreg = SREG; |
157 | uint8_t sreg = SREG; |
109 | 158 | ||
110 | cli(); |
159 | cli(); |
111 | 160 | ||
112 | // Configure Pins for SPI |
161 | // Configure Pins for SPI |
113 | // set SCK (PB7), MOSI (PB5) as output |
162 | // set SCK (PB7), MOSI (PB5) as output |
114 | DDRB |= (1<<DDB7)|(1<<DDB5); |
163 | DDRB |= (1<<DDB7)|(1<<DDB5); |
115 | // set MISO (PB6) as input |
164 | // set MISO (PB6) as input |
116 | DDRB &= ~(1<<DDB6); |
165 | DDRB &= ~(1<<DDB6); |
117 | 166 | ||
118 | 167 | ||
119 | // Output Pins MM3_SS ,MM3_RESET |
168 | // Output Pins MM3_SS ,MM3_RESET |
120 | MM3_SS_DDR |= (1<<MM3_SS_PIN); |
169 | MM3_SS_DDR |= (1<<MM3_SS_PIN); |
121 | MM3_RESET_DDR |= (1<<MM3_RESET_PIN); |
170 | MM3_RESET_DDR |= (1<<MM3_RESET_PIN); |
122 | // set pins permanent to low |
171 | // set pins permanent to low |
123 | MM3_SS_PORT &= ~((1<<MM3_SS_PIN)); |
172 | MM3_SS_PORT &= ~((1<<MM3_SS_PIN)); |
124 | MM3_RESET_PORT &= ~((1<<MM3_RESET_PIN)); |
173 | MM3_RESET_PORT &= ~((1<<MM3_RESET_PIN)); |
125 | 174 | ||
126 | // Initialize SPI-Interface |
175 | // Initialize SPI-Interface |
127 | // Enable interrupt (SPIE=1) |
176 | // Enable interrupt (SPIE=1) |
128 | // Enable SPI bus (SPE=1) |
177 | // Enable SPI bus (SPE=1) |
129 | // MSB transmitted first (DORD = 0) |
178 | // MSB transmitted first (DORD = 0) |
130 | // Master SPI Mode (MSTR=1) |
179 | // Master SPI Mode (MSTR=1) |
131 | // Clock polarity low when idle (CPOL=0) |
180 | // Clock polarity low when idle (CPOL=0) |
132 | // Clock phase sample at leading edge (CPHA=0) |
181 | // Clock phase sample at leading edge (CPHA=0) |
133 | // Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz |
182 | // Clock rate = SYSCLK/128 (SPI2X=0, SPR1=1, SPR0=1) 20MHz/128 = 156.25kHz |
134 | SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0); |
183 | SPCR = (1<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(1<<SPR0); |
135 | SPSR &= ~(1<<SPI2X); |
184 | SPSR &= ~(1<<SPI2X); |
136 | 185 | ||
137 | // Init Statemachine |
186 | // Init Statemachine |
138 | MM3.AXIS = MM3_X_AXIS; |
187 | MM3.AXIS = MM3_X_AXIS; |
139 | MM3.STATE = MM3_STATE_RESET; |
188 | MM3.STATE = MM3_STATE_RESET; |
140 | 189 | ||
141 | // Read calibration from EEprom |
190 | // Read calibration from EEprom |
142 | MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF); |
191 | MM3_calib.X_off = (int8_t)GetParamByte(PID_MM3_X_OFF); |
143 | MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF); |
192 | MM3_calib.Y_off = (int8_t)GetParamByte(PID_MM3_Y_OFF); |
144 | MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
193 | MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
145 | MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
194 | MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
146 | MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
195 | MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
147 | MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
196 | MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
148 | 197 | ||
149 | MM3_Timeout = 0; |
198 | MM3_Timeout = 0; |
150 | 199 | ||
151 | SREG = sreg; |
200 | SREG = sreg; |
152 | } |
201 | } |
153 | 202 | ||
154 | 203 | ||
155 | /*********************************************/ |
204 | /*********************************************/ |
156 | /* Get Data from MM3 */ |
205 | /* Get Data from MM3 */ |
157 | /*********************************************/ |
206 | /*********************************************/ |
158 | void MM3_Update(void) // called every 102.4 µs by timer 0 ISR |
207 | void MM3_Update(void) // called every 102.4 µs by timer 0 ISR |
159 | { |
208 | { |
160 | switch (MM3.STATE) |
209 | switch (MM3.STATE) |
161 | { |
210 | { |
162 | case MM3_STATE_RESET: |
211 | case MM3_STATE_RESET: |
163 | MM3_SS_ON // select slave |
212 | MM3_SS_ON // select slave |
164 | MM3_RESET_ON // RESET to High, MM3 Reset |
213 | MM3_RESET_ON // RESET to High, MM3 Reset |
165 | MM3.STATE = MM3_STATE_START_TRANSFER; |
214 | MM3.STATE = MM3_STATE_START_TRANSFER; |
166 | return; |
215 | return; |
167 | 216 | ||
168 | case MM3_STATE_START_TRANSFER: |
217 | case MM3_STATE_START_TRANSFER: |
169 | MM3_RESET_OFF // RESET auf Low (was 102.4 µs at high level) |
218 | MM3_RESET_OFF // RESET auf Low (was 102.4 µs at high level) |
170 | // write to SPDR triggers automatically the transfer MOSI MISO |
219 | // write to SPDR triggers automatically the transfer MOSI MISO |
171 | // MM3 Period, + AXIS code |
220 | // MM3 Period, + AXIS code |
172 | switch(MM3.AXIS) |
221 | switch(MM3.AXIS) |
173 | { |
222 | { |
174 | case MM3_X_AXIS: |
223 | case MM3_X_AXIS: |
175 | SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
224 | SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
176 | break; |
225 | break; |
177 | case MM3_Y_AXIS: |
226 | case MM3_Y_AXIS: |
178 | SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
227 | SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
179 | break; |
228 | break; |
180 | case MM3_Z_AXIS: |
229 | case MM3_Z_AXIS: |
181 | SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; |
230 | SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; |
182 | break; |
231 | break; |
183 | default: |
232 | default: |
184 | MM3.AXIS = MM3_X_AXIS; |
233 | MM3.AXIS = MM3_X_AXIS; |
185 | MM3.STATE = MM3_STATE_RESET; |
234 | MM3.STATE = MM3_STATE_RESET; |
186 | return; |
235 | return; |
187 | } |
236 | } |
188 | 237 | ||
189 | // DRDY line is not connected, therefore |
238 | // DRDY line is not connected, therefore |
190 | // wait before reading data back |
239 | // wait before reading data back |
191 | MM3.DRDY = SetDelay(8); // wait 8ms for data ready |
240 | MM3.DRDY = SetDelay(8); // wait 8ms for data ready |
192 | MM3.STATE = MM3_STATE_WAIT_DRDY; |
241 | MM3.STATE = MM3_STATE_WAIT_DRDY; |
193 | return; |
242 | return; |
194 | 243 | ||
195 | case MM3_STATE_WAIT_DRDY: |
244 | case MM3_STATE_WAIT_DRDY: |
196 | if (CheckDelay(MM3.DRDY)) |
245 | if (CheckDelay(MM3.DRDY)) |
197 | { |
246 | { |
198 | // write something into SPDR to trigger data reading |
247 | // write something into SPDR to trigger data reading |
199 | SPDR = 0x00; |
248 | SPDR = 0x00; |
200 | MM3.STATE = MM3_STATE_DRDY; |
249 | MM3.STATE = MM3_STATE_DRDY; |
201 | } |
250 | } |
202 | return; |
251 | return; |
203 | } |
252 | } |
204 | } |
253 | } |
205 | 254 | ||
206 | 255 | ||
207 | /*********************************************/ |
256 | /*********************************************/ |
208 | /* Interrupt SPI transfer complete */ |
257 | /* Interrupt SPI transfer complete */ |
209 | /*********************************************/ |
258 | /*********************************************/ |
210 | ISR(SPI_STC_vect) |
259 | ISR(SPI_STC_vect) |
211 | { |
260 | { |
212 | static int8_t tmp; |
261 | static int8_t tmp; |
213 | int16_t value; |
262 | int16_t value; |
214 | 263 | ||
215 | switch (MM3.STATE) |
264 | switch (MM3.STATE) |
216 | { |
265 | { |
217 | // 1st byte received |
266 | // 1st byte received |
218 | case MM3_STATE_DRDY: |
267 | case MM3_STATE_DRDY: |
219 | tmp = SPDR; // store 1st byte |
268 | tmp = SPDR; // store 1st byte |
220 | SPDR = 0x00; // trigger transfer of 2nd byte |
269 | SPDR = 0x00; // trigger transfer of 2nd byte |
221 | MM3.STATE = MM3_STATE_BYTE2; |
270 | MM3.STATE = MM3_STATE_BYTE2; |
222 | return; |
271 | return; |
223 | 272 | ||
224 | case MM3_STATE_BYTE2: // 2nd byte received |
273 | case MM3_STATE_BYTE2: // 2nd byte received |
225 | value = (int16_t)tmp; // combine the 1st and 2nd byte to a word |
274 | value = (int16_t)tmp; // combine the 1st and 2nd byte to a word |
226 | value <<= 8; // shift 1st byte to MSB-Position |
275 | value <<= 8; // shift 1st byte to MSB-Position |
227 | value |= (int16_t)SPDR; // add 2nd byte |
276 | value |= (int16_t)SPDR; // add 2nd byte |
228 | 277 | ||
229 | if(abs(value) < MAX_AXIS_VALUE) // ignore spikes |
278 | if(abs(value) < MAX_AXIS_VALUE) // ignore spikes |
230 | { |
279 | { |
231 | switch (MM3.AXIS) |
280 | switch (MM3.AXIS) |
232 | { |
281 | { |
233 | case MM3_X_AXIS: |
282 | case MM3_X_AXIS: |
234 | MM3.x_axis = value; |
283 | MM3.x_axis = value; |
235 | MM3.AXIS = MM3_Y_AXIS; |
284 | MM3.AXIS = MM3_Y_AXIS; |
236 | break; |
285 | break; |
237 | case MM3_Y_AXIS: |
286 | case MM3_Y_AXIS: |
238 | MM3.y_axis = value; |
287 | MM3.y_axis = value; |
239 | MM3.AXIS = MM3_Z_AXIS; |
288 | MM3.AXIS = MM3_Z_AXIS; |
240 | break; |
289 | break; |
241 | case MM3_Z_AXIS: |
290 | case MM3_Z_AXIS: |
242 | MM3.z_axis = value; |
291 | MM3.z_axis = value; |
243 | MM3.AXIS = MM3_X_AXIS; |
292 | MM3.AXIS = MM3_X_AXIS; |
244 | break; |
293 | break; |
245 | default: |
294 | default: |
246 | MM3.AXIS = MM3_X_AXIS; |
295 | MM3.AXIS = MM3_X_AXIS; |
247 | break; |
296 | break; |
248 | } |
297 | } |
249 | } |
298 | } |
250 | MM3_SS_OFF // deselect slave |
299 | MM3_SS_OFF // deselect slave |
251 | MM3.STATE = MM3_STATE_RESET; |
300 | MM3.STATE = MM3_STATE_RESET; |
252 | // Update timeout is called every 102.4 µs. |
301 | // Update timeout is called every 102.4 µs. |
253 | // It takes 2 cycles to write a measurement data request for one axis and |
302 | // It takes 2 cycles to write a measurement data request for one axis and |
254 | // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back. |
303 | // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back. |
255 | // I.e. 81 cycles * 102.4 µs = 8.3ms per axis. |
304 | // I.e. 81 cycles * 102.4 µs = 8.3ms per axis. |
256 | // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() - |
305 | // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() - |
257 | // decremtent the MM3_Timeout every 100 ms. |
306 | // decremtent the MM3_Timeout every 100 ms. |
258 | // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout. |
307 | // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout. |
259 | if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis)) |
308 | if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis)) |
260 | { // if all axis measurements give diffrent readings the data should be valid |
309 | { // if all axis measurements give diffrent readings the data should be valid |
261 | if(MM3_Timeout < 20) MM3_Timeout++; |
310 | if(MM3_Timeout < 20) MM3_Timeout++; |
262 | } |
311 | } |
263 | else // something is very strange here |
312 | else // something is very strange here |
264 | { |
313 | { |
265 | if(MM3_Timeout ) MM3_Timeout--; |
314 | if(MM3_Timeout ) MM3_Timeout--; |
266 | } |
315 | } |
267 | return; |
316 | return; |
268 | 317 | ||
269 | default: |
318 | default: |
270 | return; |
319 | return; |
271 | } |
320 | } |
272 | } |
321 | } |
273 | 322 | ||
274 | 323 | ||
275 | /*********************************************/ |
324 | /*********************************************/ |
276 | /* Calibrate Compass */ |
325 | /* Calibrate Compass */ |
277 | /*********************************************/ |
326 | /*********************************************/ |
278 | void MM3_Calibrate(void) |
327 | void MM3_Calibrate(void) |
279 | { |
328 | { |
280 | static int16_t x_min, x_max, y_min, y_max, z_min, z_max; |
329 | static int16_t x_min, x_max, y_min, y_max, z_min, z_max; |
281 | 330 | ||
282 | switch(CompassCalState) |
331 | switch(CompassCalState) |
283 | { |
332 | { |
284 | case 1: // change to x-y axis |
333 | case 1: // change to x-y axis |
285 | x_min = 10000; |
334 | x_min = 10000; |
286 | x_max = -10000; |
335 | x_max = -10000; |
287 | y_min = 10000; |
336 | y_min = 10000; |
288 | y_max = -10000; |
337 | y_max = -10000; |
289 | z_min = 10000; |
338 | z_min = 10000; |
290 | z_max = -10000; |
339 | z_max = -10000; |
291 | break; |
340 | break; |
292 | case 2: |
341 | case 2: |
293 | // find Min and Max of the X- and Y-Axis |
342 | // find Min and Max of the X- and Y-Axis |
294 | if(MM3.x_axis < x_min) x_min = MM3.x_axis; |
343 | if(MM3.x_axis < x_min) x_min = MM3.x_axis; |
295 | if(MM3.x_axis > x_max) x_max = MM3.x_axis; |
344 | if(MM3.x_axis > x_max) x_max = MM3.x_axis; |
296 | if(MM3.y_axis < y_min) y_min = MM3.y_axis; |
345 | if(MM3.y_axis < y_min) y_min = MM3.y_axis; |
297 | if(MM3.y_axis > y_max) y_max = MM3.y_axis; |
346 | if(MM3.y_axis > y_max) y_max = MM3.y_axis; |
298 | break; |
347 | break; |
299 | case 3: |
348 | case 3: |
300 | // change to z-Axis |
349 | // change to z-Axis |
301 | break; |
350 | break; |
302 | case 4: |
351 | case 4: |
303 | RED_ON; // find Min and Max of the Z-axis |
352 | RED_ON; // find Min and Max of the Z-axis |
304 | if(MM3.z_axis < z_min) z_min = MM3.z_axis; |
353 | if(MM3.z_axis < z_min) z_min = MM3.z_axis; |
305 | if(MM3.z_axis > z_max) z_max = MM3.z_axis; |
354 | if(MM3.z_axis > z_max) z_max = MM3.z_axis; |
306 | break; |
355 | break; |
307 | case 5: |
356 | case 5: |
308 | // calc range of all axis |
357 | // calc range of all axis |
309 | MM3_calib.X_range = (x_max - x_min); |
358 | MM3_calib.X_range = (x_max - x_min); |
310 | MM3_calib.Y_range = (y_max - y_min); |
359 | MM3_calib.Y_range = (y_max - y_min); |
311 | MM3_calib.Z_range = (z_max - z_min); |
360 | MM3_calib.Z_range = (z_max - z_min); |
312 | 361 | ||
313 | // calc offset of all axis |
362 | // calc offset of all axis |
314 | MM3_calib.X_off = (x_max + x_min) / 2; |
363 | MM3_calib.X_off = (x_max + x_min) / 2; |
315 | MM3_calib.Y_off = (y_max + y_min) / 2; |
364 | MM3_calib.Y_off = (y_max + y_min) / 2; |
316 | MM3_calib.Z_off = (z_max + z_min) / 2; |
365 | MM3_calib.Z_off = (z_max + z_min) / 2; |
317 | 366 | ||
318 | // save to EEProm |
367 | // save to EEProm |
319 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
368 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
320 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
369 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
321 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
370 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
322 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
371 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
323 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
372 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
324 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
373 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
325 | 374 | ||
326 | CompassCalState = 0; |
375 | CompassCalState = 0; |
327 | break; |
376 | break; |
328 | default: |
377 | default: |
329 | CompassCalState = 0; |
378 | CompassCalState = 0; |
330 | break; |
379 | break; |
331 | } |
380 | } |
332 | } |
381 | } |
333 | 382 | ||
334 | 383 | ||
335 | /* |
384 | /* |
336 | void MM3_Calibrate(void) |
385 | void MM3_Calibrate(void) |
337 | { |
386 | { |
338 | static uint8_t debugcounter = 0; |
387 | static uint8_t debugcounter = 0; |
339 | int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
388 | int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
340 | uint8_t measurement = 50, beeper = 0; |
389 | uint8_t measurement = 50, beeper = 0; |
341 | uint16_t timer; |
390 | uint16_t timer; |
342 | 391 | ||
343 | GRN_ON; |
392 | GRN_ON; |
344 | RED_OFF; |
393 | RED_OFF; |
345 | 394 | ||
346 | // get maximum and minimum reading of all axis |
395 | // get maximum and minimum reading of all axis |
347 | while (measurement) |
396 | while (measurement) |
348 | { |
397 | { |
349 | // reset range markers if yawstick ist leftmost |
398 | // reset range markers if yawstick ist leftmost |
350 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100) |
399 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100) |
351 | { |
400 | { |
352 | x_min = 0; |
401 | x_min = 0; |
353 | x_max = 0; |
402 | x_max = 0; |
354 | y_min = 0; |
403 | y_min = 0; |
355 | y_max = 0; |
404 | y_max = 0; |
356 | z_min = 0; |
405 | z_min = 0; |
357 | z_max = 0; |
406 | z_max = 0; |
358 | } |
407 | } |
359 | 408 | ||
360 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
409 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
361 | else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
410 | else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
362 | 411 | ||
363 | if (MM3.y_axis > y_max) y_max = MM3.y_axis; |
412 | if (MM3.y_axis > y_max) y_max = MM3.y_axis; |
364 | else if (MM3.y_axis < y_min) y_min = MM3.y_axis; |
413 | else if (MM3.y_axis < y_min) y_min = MM3.y_axis; |
365 | 414 | ||
366 | if (MM3.z_axis > z_max) z_max = MM3.z_axis; |
415 | if (MM3.z_axis > z_max) z_max = MM3.z_axis; |
367 | else if (MM3.z_axis < z_min) z_min = MM3.z_axis; |
416 | else if (MM3.z_axis < z_min) z_min = MM3.z_axis; |
368 | 417 | ||
369 | if (!beeper) |
418 | if (!beeper) |
370 | { |
419 | { |
371 | RED_FLASH; |
420 | RED_FLASH; |
372 | GRN_FLASH; |
421 | GRN_FLASH; |
373 | BeepTime = 50; |
422 | BeepTime = 50; |
374 | beeper = 50; |
423 | beeper = 50; |
375 | } |
424 | } |
376 | beeper--; |
425 | beeper--; |
377 | // loop with period of 10 ms / 100 Hz |
426 | // loop with period of 10 ms / 100 Hz |
378 | timer = SetDelay(10); |
427 | timer = SetDelay(10); |
379 | while(!CheckDelay(timer)); |
428 | while(!CheckDelay(timer)); |
380 | 429 | ||
381 | if(debugcounter++ > 30) |
430 | if(debugcounter++ > 30) |
382 | { |
431 | { |
383 | printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max); |
432 | printf("\n\rXMin:%4d, XMax:%4d, YMin:%4d, YMax:%4d, ZMin:%4d, ZMax:%4d",x_min,x_max,y_min,y_max,z_min,z_max); |
384 | debugcounter = 0; |
433 | debugcounter = 0; |
385 | } |
434 | } |
386 | 435 | ||
387 | // If gas is less than 100, stop calibration with a delay of 0.5 seconds |
436 | // If gas is less than 100, stop calibration with a delay of 0.5 seconds |
388 | if (PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 100) measurement--; |
437 | if (PPM_in[ParamSet.ChannelAssignment[CH_GAS]] < 100) measurement--; |
389 | } |
438 | } |
390 | // Rage of all axis |
439 | // Rage of all axis |
391 | MM3_calib.X_range = (x_max - x_min); |
440 | MM3_calib.X_range = (x_max - x_min); |
392 | MM3_calib.Y_range = (y_max - y_min); |
441 | MM3_calib.Y_range = (y_max - y_min); |
393 | MM3_calib.Z_range = (z_max - z_min); |
442 | MM3_calib.Z_range = (z_max - z_min); |
394 | 443 | ||
395 | // Offset of all axis |
444 | // Offset of all axis |
396 | MM3_calib.X_off = (x_max + x_min) / 2; |
445 | MM3_calib.X_off = (x_max + x_min) / 2; |
397 | MM3_calib.Y_off = (y_max + y_min) / 2; |
446 | MM3_calib.Y_off = (y_max + y_min) / 2; |
398 | MM3_calib.Z_off = (z_max + z_min) / 2; |
447 | MM3_calib.Z_off = (z_max + z_min) / 2; |
399 | 448 | ||
400 | // save to EEProm |
449 | // save to EEProm |
401 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
450 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
402 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
451 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
403 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
452 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
404 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
453 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
405 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
454 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
406 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
455 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
407 | 456 | ||
408 | } |
457 | } |
409 | */ |
458 | */ |
410 | 459 | ||
411 | /*********************************************/ |
460 | /*********************************************/ |
412 | /* Calculate north direction (heading) */ |
461 | /* Calculate north direction (heading) */ |
413 | /*********************************************/ |
462 | /*********************************************/ |
414 | void MM3_Heading(void) |
463 | void MM3_Heading(void) |
415 | { |
464 | { |
416 | int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
465 | int32_t sin_nick, cos_nick, sin_roll, cos_roll, sin_yaw, cos_yaw; |
417 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
466 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
418 | int16_t angle; |
467 | int16_t angle; |
419 | uint16_t div_factor; |
- | |
420 | int16_t heading; |
468 | int16_t heading; |
421 | 469 | ||
422 | if (MM3_Timeout) |
470 | if (MM3_Timeout) |
423 | { |
471 | { |
424 | // Offset correction and normalization (values of H are +/- 512) |
472 | // Offset correction and normalization (values of H are +/- 512) |
425 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
473 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
426 | Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
474 | Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
427 | Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
475 | Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
428 | 476 | ||
429 | // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
477 | // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
430 | // assuming the MM3 board is mounted parallel to the frame. |
478 | // assuming the MM3 board is mounted parallel to the frame. |
431 | // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
479 | // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
432 | // in a top view counter clockwise direction. |
480 | // in a top view counter clockwise direction. |
433 | // North is in opposite direction of the small arrow on the MM3 board. |
481 | // North is in opposite direction of the small arrow on the MM3 board. |
434 | // Therefore 180 deg must be added to that angle. |
482 | // Therefore 180 deg must be added to that angle. |
435 | angle = ((int16_t)ParamSet.UserParam4 + 180); |
483 | angle = ((int16_t)ParamSet.UserParam4 + 180); |
436 | // wrap angle to interval of 0°- 359° |
484 | // wrap angle to interval of 0°- 359° |
437 | angle += 360; |
485 | angle += 360; |
438 | angle %= 360; |
486 | angle %= 360; |
439 | sin_yaw = (int32_t)(c_sin_8192(angle)); |
487 | sin_yaw = (int32_t)(c_sin_8192(angle)); |
440 | cos_yaw = (int32_t)(c_cos_8192(angle)); |
488 | cos_yaw = (int32_t)(c_cos_8192(angle)); |
441 | 489 | ||
442 | Hx_corr = Hx; |
490 | Hx_corr = Hx; |
443 | Hy_corr = Hy; |
491 | Hy_corr = Hy; |
444 | 492 | ||
445 | // rotate |
493 | // rotate |
446 | Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
494 | Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
447 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
495 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
448 | 496 | ||
449 | 497 | ||
450 | // tilt compensation |
498 | // tilt compensation |
451 | - | ||
452 | // calibration factor for transforming Gyro Integrals to angular degrees |
- | |
453 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
- | |
454 | 499 | ||
455 | // calculate sinus cosinus of nick and tilt angle |
500 | // calculate sinus cosinus of nick and tilt angle |
456 | angle = (IntegralNick/div_factor); |
501 | angle = (int16_t)(IntegralGyroNick/GYRO_DEG_FACTOR); |
457 | sin_nick = (int32_t)(c_sin_8192(angle)); |
502 | sin_nick = (int32_t)(c_sin_8192(angle)); |
458 | cos_nick = (int32_t)(c_cos_8192(angle)); |
503 | cos_nick = (int32_t)(c_cos_8192(angle)); |
459 | 504 | ||
460 | angle = (IntegralRoll/div_factor); |
505 | angle = (int16_t)(IntegralGyroRoll/GYRO_DEG_FACTOR); |
461 | sin_roll = (int32_t)(c_sin_8192(angle)); |
506 | sin_roll = (int32_t)(c_sin_8192(angle)); |
462 | cos_roll = (int32_t)(c_cos_8192(angle)); |
507 | cos_roll = (int32_t)(c_cos_8192(angle)); |
463 | 508 | ||
464 | Hx_corr = Hx * cos_nick; |
509 | Hx_corr = Hx * cos_nick; |
465 | Hx_corr -= Hz * sin_nick; |
510 | Hx_corr -= Hz * sin_nick; |
466 | Hx_corr /= 8192; |
511 | Hx_corr /= 8192; |
467 | 512 | ||
468 | Hy_corr = Hy * cos_roll; |
513 | Hy_corr = Hy * cos_roll; |
469 | Hy_corr += Hz * sin_roll; |
514 | Hy_corr += Hz * sin_roll; |
470 | Hy_corr /= 8192; |
515 | Hy_corr /= 8192; |
471 | 516 | ||
472 | // calculate Heading |
517 | // calculate Heading |
473 | heading = c_atan2(Hy_corr, Hx_corr); |
518 | heading = c_atan2(Hy_corr, Hx_corr); |
474 | 519 | ||
475 | // atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
520 | // atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
476 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
521 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
477 | if (heading < 0) heading = -heading; |
522 | if (heading < 0) heading = -heading; |
478 | else heading = 360 - heading; |
523 | else heading = 360 - heading; |
479 | } |
524 | } |
480 | else // MM3_Timeout = 0 i.e now new data from external board |
525 | else // MM3_Timeout = 0 i.e now new data from external board |
481 | { |
526 | { |
482 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
527 | if(!BeepTime) BeepTime = 100; // make noise to signal the compass problem |
483 | heading = -1; |
528 | heading = -1; |
484 | } |
529 | } |
485 | // update compass values in fc variables |
530 | // update compass values in fc variables |
486 | CompassHeading = heading; |
531 | CompassHeading = heading; |
487 | if (CompassHeading < 0) CompassOffCourse = 0; |
532 | if (CompassHeading < 0) CompassOffCourse = 0; |
488 | else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
533 | else CompassOffCourse = ((540 + CompassHeading - CompassCourse) % 360) - 180; |
489 | } |
534 | } |
490 | 535 |