Rev 782 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 782 | Rev 790 | ||
|---|---|---|---|
| Line 22... | Line 22... | ||
| 22 | #include "mymath.h" |
22 | #include "mymath.h" |
| 23 | #include "fc.h" |
23 | #include "fc.h" |
| 24 | #include "timer0.h" |
24 | #include "timer0.h" |
| 25 | #include "rc.h" |
25 | #include "rc.h" |
| 26 | #include "eeprom.h" |
26 | #include "eeprom.h" |
| 27 | #include "uart.h" |
27 | #include "printf_P.h" |
| Line 28... | Line 28... | ||
| 28 | 28 | ||
| Line 29... | Line 29... | ||
| 29 | #define MAX_AXIS_VALUE 500 |
29 | #define MAX_AXIS_VALUE 500 |
| Line 262... | Line 262... | ||
| 262 | /*********************************************/ |
262 | /*********************************************/ |
| 263 | /* Calibrate Compass */ |
263 | /* Calibrate Compass */ |
| 264 | /*********************************************/ |
264 | /*********************************************/ |
| 265 | void MM3_Calibrate(void) |
265 | void MM3_Calibrate(void) |
| 266 | { |
266 | { |
| - | 267 | static uint8_t debugcounter = 0; |
|
| 267 | int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
268 | int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
| 268 | uint8_t measurement = 50, beeper = 0; |
269 | uint8_t measurement = 50, beeper = 0; |
| 269 | uint16_t timer; |
270 | uint16_t timer; |
| Line 270... | Line 271... | ||
| 270 | 271 | ||
| 271 | GRN_ON; |
272 | GRN_ON; |
| Line 272... | Line 273... | ||
| 272 | ROT_OFF; |
273 | ROT_OFF; |
| 273 | 274 | ||
| 274 | // get maximum and minimum reading of all axis |
275 | // get maximum and minimum reading of all axis |
| - | 276 | while (measurement) |
|
| - | 277 | { |
|
| - | 278 | // reset range markers if yawstick ist leftmost |
|
| - | 279 | if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100) |
|
| - | 280 | { |
|
| - | 281 | x_min = 0; |
|
| - | 282 | x_max = 0; |
|
| - | 283 | y_min = 0; |
|
| - | 284 | y_max = 0; |
|
| - | 285 | z_min = 0; |
|
| - | 286 | z_max = 0; |
|
| 275 | while (measurement) |
287 | } |
| 276 | { |
288 | |
| Line 277... | Line 289... | ||
| 277 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
289 | if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
| 278 | else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
290 | else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
| Line 293... | Line 305... | ||
| 293 | beeper--; |
305 | beeper--; |
| 294 | // loop with period of 10 ms / 100 Hz |
306 | // loop with period of 10 ms / 100 Hz |
| 295 | timer = SetDelay(10); |
307 | timer = SetDelay(10); |
| 296 | while(!CheckDelay(timer)); |
308 | while(!CheckDelay(timer)); |
| Line -... | Line 309... | ||
| - | 309 | ||
| - | 310 | if(debugcounter++ > 30) |
|
| - | 311 | { |
|
| - | 312 | 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); |
|
| - | 313 | debugcounter = 0; |
|
| - | 314 | } |
|
| 297 | 315 | ||
| 298 | // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
316 | // If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
| 299 | if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
317 | if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
| 300 | } |
318 | } |
| 301 | // Rage of all axis |
319 | // Rage of all axis |
| Line 328... | Line 346... | ||
| 328 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
346 | int32_t Hx, Hy, Hz, Hx_corr, Hy_corr; |
| 329 | int16_t angle; |
347 | int16_t angle; |
| 330 | uint16_t div_factor; |
348 | uint16_t div_factor; |
| 331 | int16_t heading; |
349 | int16_t heading; |
| Line 332... | Line -... | ||
| 332 | - | ||
| 333 | DebugOut.Analog[11] = MM3_Timeout; |
- | |
| 334 | 350 | ||
| 335 | if (MM3_Timeout) |
351 | if (MM3_Timeout) |
| 336 | { |
352 | { |
| 337 | // Offset correction and normalization (values of H are +/- 512) |
353 | // Offset correction and normalization (values of H are +/- 512) |
| 338 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |
354 | Hx = (((int32_t)(MM3.x_axis - MM3_calib.X_off)) * 1024) / (int32_t)MM3_calib.X_range; |