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 | ||