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