Rev 727 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 727 | Rev 741 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | /* |
1 | /* |
| Line 2... | Line 2... | ||
| 2 | 2 | ||
| Line 3... | Line 3... | ||
| 3 | Copyright 2007, Niklas Nold |
3 | Copyright 2008, by Killagreg |
| 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 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 |
| Line -... | Line 10... | ||
| - | 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: 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 |
| 14 | */ |
15 | */ |
| Line 59... | Line 60... | ||
| 59 | #define MM3_PERIOD_2048 0x60 |
60 | #define MM3_PERIOD_2048 0x60 |
| 60 | #define MM3_PERIOD_4096 0x70 |
61 | #define MM3_PERIOD_4096 0x70 |
| Line 61... | Line 62... | ||
| 61 | 62 | ||
| 62 | MM3_calib_t MM3_calib; |
63 | MM3_calib_t MM3_calib; |
| - | 64 | volatile MM3_working_t MM3; |
|
| Line 63... | Line 65... | ||
| 63 | volatile MM3_working_t MM3; |
65 | static volatile uint8_t MM3_Timeout = 0; |
| 64 | 66 | ||
| Line 105... | Line 107... | ||
| 105 | MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
107 | MM3_calib.Z_off = (int8_t)GetParamByte(PID_MM3_Z_OFF); |
| 106 | MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
108 | MM3_calib.X_range = (int16_t)GetParamWord(PID_MM3_X_RANGE); |
| 107 | MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
109 | MM3_calib.Y_range = (int16_t)GetParamWord(PID_MM3_Y_RANGE); |
| 108 | MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
110 | MM3_calib.Z_range = (int16_t)GetParamWord(PID_MM3_Z_RANGE); |
| Line -... | Line 111... | ||
| - | 111 | ||
| - | 112 | MM3_Timeout = 0; |
|
| 109 | 113 | ||
| 110 | SREG = sreg; |
114 | SREG = sreg; |
| Line 111... | Line 115... | ||
| 111 | } |
115 | } |
| 112 | 116 | ||
| 113 | 117 | ||
| 114 | /*********************************************/ |
118 | /*********************************************/ |
| 115 | /* Get Data from MM3 */ |
119 | /* Get Data from MM3 */ |
| 116 | /*********************************************/ |
120 | /*********************************************/ |
| 117 | void MM3_Update() // called every 102.4 ms by timer 0 ISR |
121 | void MM3_Update() // called every 102.4 µs by timer 0 ISR |
| 118 | { |
122 | { |
| 119 | switch (MM3.STATE) |
123 | switch (MM3.STATE) |
| 120 | { |
124 | { |
| 121 | case MM3_STATE_RESET: |
125 | case MM3_STATE_RESET: |
| 122 | PORTC &= ~(1<<PORTC4); // select slave |
126 | PORTC &= ~(1<<PORTC4); // select slave |
| Line 123... | Line 127... | ||
| 123 | PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset |
127 | PORTC |= (1<<PORTC5); // PC5 to High, MM3 Reset |
| 124 | MM3.STATE = MM3_STATE_START_TRANSFER; |
128 | MM3.STATE = MM3_STATE_START_TRANSFER; |
| Line 125... | Line 129... | ||
| 125 | return; |
129 | return; |
| 126 | 130 | ||
| - | 131 | case MM3_STATE_START_TRANSFER: |
|
| - | 132 | PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level) |
|
| - | 133 | ||
| 127 | case MM3_STATE_START_TRANSFER: |
134 | // write to SPDR triggers automatically the transfer MOSI MISO |
| - | 135 | // MM3 Period, + AXIS code |
|
| - | 136 | switch(MM3.AXIS) |
|
| 128 | PORTC &= ~(1<<PORTC5); // PC4 auf Low (was 102.4 µs at high level) |
137 | { |
| - | 138 | case MM3_X_AXIS: |
|
| - | 139 | SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
|
| 129 | 140 | break; |
|
| - | 141 | case MM3_Y_AXIS: |
|
| - | 142 | SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
|
| - | 143 | break; |
|
| - | 144 | case MM3_Z_AXIS: |
|
| - | 145 | SPDR = MM3_PERIOD_256 + MM3_Z_AXIS; |
|
| - | 146 | break; |
|
| Line 130... | Line 147... | ||
| 130 | // write to SPDR triggers automatically the transfer MOSI MISO |
147 | default: |
| 131 | // MM3 Period, + AXIS code |
148 | MM3.AXIS = MM3_X_AXIS; |
| 132 | if (MM3.AXIS == MM3_X_AXIS) SPDR = MM3_PERIOD_256 + MM3_X_AXIS; |
149 | MM3.STATE = MM3_STATE_RESET; |
| 133 | else if (MM3.AXIS == MM3_Y_AXIS) SPDR = MM3_PERIOD_256 + MM3_Y_AXIS; |
150 | return; |
| Line 194... | Line 211... | ||
| 194 | break; |
211 | break; |
| 195 | } |
212 | } |
| 196 | } |
213 | } |
| 197 | PORTC |= (1<<PORTC4); // deselect slave |
214 | PORTC |= (1<<PORTC4); // deselect slave |
| 198 | MM3.STATE = MM3_STATE_RESET; |
215 | MM3.STATE = MM3_STATE_RESET; |
| - | 216 | // Update timeout is called every 102.4 µs. |
|
| - | 217 | // It takes 2 cycles to write a measurement data request for one axis and |
|
| - | 218 | // at at least 8 ms / 102.4 µs = 79 cycles to read the requested data back. |
|
| - | 219 | // I.e. 81 cycles * 102.4 µs = 8.3ms per axis. |
|
| - | 220 | // The two function accessing the MM3 Data - MM3_Calibrate() and MM3_Heading() - |
|
| - | 221 | // decremtent the MM3_Timeout every 100 ms. |
|
| - | 222 | // incrementing the counter by 1 every 8.3 ms is sufficient to avoid a timeout. |
|
| - | 223 | if ((MM3.x_axis != MM3.y_axis) || (MM3.x_axis != MM3.z_axis) || (MM3.y_axis != MM3.z_axis)) |
|
| - | 224 | { // if all axis measurements give diffrent readings the data should be valid |
|
| - | 225 | if(MM3_Timeout < 20) MM3_Timeout++; |
|
| - | 226 | } |
|
| - | 227 | else // something is very strange here |
|
| - | 228 | { |
|
| - | 229 | if(MM3_Timeout ) MM3_Timeout--; |
|
| - | 230 | } |
|
| - | 231 | return; |
|
| - | 232 | ||
| - | 233 | default: |
|
| - | 234 | return; |
|
| 199 | } |
235 | } |
| 200 | } |
236 | } |
| Line 212... | Line 248... | ||
| 212 | 248 | ||
| 213 | GRN_ON; |
249 | GRN_ON; |
| Line 214... | Line 250... | ||
| 214 | ROT_OFF; |
250 | ROT_OFF; |
| 215 | 251 | ||
| 216 | // get maximum and minimum reading of all axis |
252 | // get maximum and minimum reading of all axis |
| 217 | while (measurement) |
253 | while (measurement && !MM3_Timeout) |
| 218 | { |
254 | { |
| Line 219... | Line 255... | ||
| 219 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
255 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
| Line 231... | Line 267... | ||
| 231 | GRN_FLASH; |
267 | GRN_FLASH; |
| 232 | BeepTime = 50; |
268 | BeepTime = 50; |
| 233 | beeper = 50; |
269 | beeper = 50; |
| 234 | } |
270 | } |
| 235 | beeper--; |
271 | beeper--; |
| 236 | - | ||
| 237 | // loop with period of 10 ms / 100 Hz |
272 | // loop with period of 10 ms / 100 Hz |
| 238 | timer = SetDelay(10); |
273 | timer = SetDelay(10); |
| 239 | while(!CheckDelay(timer)); |
274 | while(!CheckDelay(timer)); |
| Line 240... | Line 275... | ||
| 240 | 275 | ||
| 241 | // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
276 | // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
| 242 | if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
277 | if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
| - | 278 | } |
|
| 243 | } |
279 | if(!MM3_Timeout) |
| 244 | 280 | { |
|
| 245 | // Rage of all axis |
281 | // Rage of all axis |
| 246 | MM3_calib.X_range = (x_max - x_min); |
282 | MM3_calib.X_range = (x_max - x_min); |
| 247 | MM3_calib.Y_range = (y_max - y_min); |
283 | MM3_calib.Y_range = (y_max - y_min); |
| 248 | MM3_calib.Z_range = (z_max - z_min); |
284 | MM3_calib.Z_range = (z_max - z_min); |
| 249 | 285 | ||
| 250 | // Offset of all axis |
286 | // Offset of all axis |
| 251 | MM3_calib.X_off = (x_max + x_min) / 2; |
287 | MM3_calib.X_off = (x_max + x_min) / 2; |
| 252 | MM3_calib.Y_off = (y_max + y_min) / 2; |
288 | MM3_calib.Y_off = (y_max + y_min) / 2; |
| 253 | MM3_calib.Z_off = (z_max + z_min) / 2; |
289 | MM3_calib.Z_off = (z_max + z_min) / 2; |
| 254 | 290 | ||
| 255 | // save to EEProm |
291 | // save to EEProm |
| 256 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
292 | SetParamByte(PID_MM3_X_OFF, (uint8_t)MM3_calib.X_off); |
| 257 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
293 | SetParamByte(PID_MM3_Y_OFF, (uint8_t)MM3_calib.Y_off); |
| 258 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
294 | SetParamByte(PID_MM3_Z_OFF, (uint8_t)MM3_calib.Z_off); |
| 259 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
295 | SetParamWord(PID_MM3_X_RANGE, (uint16_t)MM3_calib.X_range); |
| 260 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
296 | SetParamWord(PID_MM3_Y_RANGE, (uint16_t)MM3_calib.Y_range); |
| - | 297 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
|
| 261 | SetParamWord(PID_MM3_Z_RANGE, (uint16_t)MM3_calib.Z_range); |
298 | } |
| Line 262... | Line 299... | ||
| 262 | } |
299 | } |
| 263 | 300 | ||
| Line 271... | Line 308... | ||
| 271 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
308 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
| 272 | int16_t angle; |
309 | int16_t angle; |
| 273 | uint16_t div_factor; |
310 | uint16_t div_factor; |
| 274 | int16_t heading; |
311 | int16_t heading; |
| Line 275... | Line -... | ||
| 275 | - | ||
| 276 | // calibration factor for transforming Gyro Integrals to angular degrees |
- | |
| 277 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
- | |
| 278 | - | ||
| 279 | // Offset correction and normalization (values of H are +/- 512) |
- | |
| 280 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
- | |
| 281 | Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
- | |
| 282 | Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
- | |
| 283 | - | ||
| 284 | // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
- | |
| 285 | // assuming the MM3 board is mounted parallel to the frame. |
- | |
| 286 | // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
- | |
| 287 | // in a top view counter clockwise direction. |
- | |
| 288 | // North is in opposite direction of the small arrow on the MM3 board. |
- | |
| 289 | // Therefore 180 deg must be added to that angle. |
- | |
| 290 | angle = ((int16_t)ParamSet.UserParam4 + 180); |
- | |
| 291 | // wrap angle to interval of 0°- 359° |
- | |
| 292 | angle += 360; |
- | |
| 293 | angle %= 360; |
- | |
| 294 | sin_yaw = (int32_t)(c_sin_8192(angle)); |
- | |
| 295 | cos_yaw = (int32_t)(c_cos_8192(angle)); |
- | |
| 296 | - | ||
| 297 | Hx_corr = Hx; |
- | |
| 298 | Hy_corr = Hy; |
- | |
| 299 | - | ||
| 300 | // rotate |
- | |
| 301 | Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
- | |
| 302 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
- | |
| 303 | - | ||
| 304 | 312 | ||
| 305 | // tilt compensation |
313 | if (MM3_Timeout) |
| 306 | 314 | { |
|
| 307 | // calibration factor for transforming Gyro Integrals to angular degrees |
315 | // calibration factor for transforming Gyro Integrals to angular degrees |
| 308 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
- | |
| 309 | - | ||
| 310 | // calculate sinus cosinus of pitch and tilt angle |
- | |
| 311 | angle = (IntegralPitch/div_factor); |
- | |
| 312 | sin_pitch = (int32_t)(c_sin_8192(angle)); |
- | |
| 313 | cos_pitch = (int32_t)(c_cos_8192(angle)); |
- | |
| 314 | - | ||
| 315 | angle = (IntegralRoll/div_factor); |
- | |
| 316 | sin_roll = (int32_t)(c_sin_8192(angle)); |
- | |
| 317 | cos_roll = (int32_t)(c_cos_8192(angle)); |
- | |
| 318 | - | ||
| 319 | Hx_corr = Hx * cos_pitch; |
- | |
| 320 | Hx_corr -= Hz * sin_pitch; |
- | |
| 321 | Hx_corr /= 8192; |
- | |
| 322 | - | ||
| 323 | Hy_corr = Hy * cos_roll; |
- | |
| 324 | Hy_corr += Hz * sin_roll; |
- | |
| 325 | Hy_corr /= 8192; |
- | |
| 326 | - | ||
| 327 | // calculate Heading |
- | |
| 328 | heading = c_atan2(Hy_corr, Hx_corr); |
- | |
| 329 | - | ||
| 330 | // atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
- | |
| 331 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
- | |
| 332 | if (heading < 0) heading = -heading; |
- | |
| Line -... | Line 316... | ||
| - | 316 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
|
| - | 317 | ||
| - | 318 | // Offset correction and normalization (values of H are +/- 512) |
|
| - | 319 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
|
| - | 320 | Hy = (((int32_t)(MM3.y_axis - MM3_calib.Y_off)) * 1024) / (int32_t)MM3_calib.Y_range; |
|
| - | 321 | Hz = (((int32_t)(MM3.z_axis - MM3_calib.Z_off)) * 1024) / (int32_t)MM3_calib.Z_range; |
|
| - | 322 | ||
| - | 323 | // Compensate the angle of the MM3-arrow to the head of the MK by a yaw rotation transformation |
|
| - | 324 | // assuming the MM3 board is mounted parallel to the frame. |
|
| - | 325 | // User Param 4 is used to define the positive angle from the MM3-arrow to the MK heading |
|
| - | 326 | // in a top view counter clockwise direction. |
|
| - | 327 | // North is in opposite direction of the small arrow on the MM3 board. |
|
| - | 328 | // Therefore 180 deg must be added to that angle. |
|
| - | 329 | angle = ((int16_t)ParamSet.UserParam4 + 180); |
|
| - | 330 | // wrap angle to interval of 0°- 359° |
|
| - | 331 | angle += 360; |
|
| - | 332 | angle %= 360; |
|
| - | 333 | sin_yaw = (int32_t)(c_sin_8192(angle)); |
|
| - | 334 | cos_yaw = (int32_t)(c_cos_8192(angle)); |
|
| - | 335 | ||
| - | 336 | Hx_corr = Hx; |
|
| - | 337 | Hy_corr = Hy; |
|
| - | 338 | ||
| - | 339 | // rotate |
|
| - | 340 | Hx = (Hx_corr * cos_yaw - Hy_corr * sin_yaw) / 8192; |
|
| - | 341 | Hy = (Hx_corr * sin_yaw + Hy_corr * cos_yaw) / 8192; |
|
| - | 342 | ||
| - | 343 | ||
| - | 344 | // tilt compensation |
|
| - | 345 | ||
| - | 346 | // calibration factor for transforming Gyro Integrals to angular degrees |
|
| - | 347 | div_factor = (uint16_t)ParamSet.UserParam3 * 8; |
|
| - | 348 | ||
| - | 349 | // calculate sinus cosinus of pitch and tilt angle |
|
| - | 350 | angle = (IntegralPitch/div_factor); |
|
| - | 351 | sin_pitch = (int32_t)(c_sin_8192(angle)); |
|
| - | 352 | cos_pitch = (int32_t)(c_cos_8192(angle)); |
|
| - | 353 | ||
| - | 354 | angle = (IntegralRoll/div_factor); |
|
| - | 355 | sin_roll = (int32_t)(c_sin_8192(angle)); |
|
| - | 356 | cos_roll = (int32_t)(c_cos_8192(angle)); |
|
| - | 357 | ||
| - | 358 | Hx_corr = Hx * cos_pitch; |
|
| - | 359 | Hx_corr -= Hz * sin_pitch; |
|
| - | 360 | Hx_corr /= 8192; |
|
| - | 361 | ||
| - | 362 | Hy_corr = Hy * cos_roll; |
|
| - | 363 | Hy_corr += Hz * sin_roll; |
|
| - | 364 | Hy_corr /= 8192; |
|
| - | 365 | ||
| - | 366 | // calculate Heading |
|
| - | 367 | heading = c_atan2(Hy_corr, Hx_corr); |
|
| - | 368 | ||
| - | 369 | // atan returns angular range from -180 deg to 180 deg in counter clockwise notation |
|
| - | 370 | // but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
|
| - | 371 | if (heading < 0) heading = -heading; |
|
| - | 372 | else heading = 360 - heading; |
|
| - | 373 | } |
|
| - | 374 | else // MM3_Timeout = 0 i.e now new data from external board |
|
| - | 375 | { |
|
| 333 | else heading = 360 - heading; |
376 | heading = -1; |
| 334 | 377 | } |
|