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