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