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