24,7 → 24,7 |
#include "timer0.h" |
#include "rc.h" |
#include "eeprom.h" |
#include "uart.h" |
#include "printf_P.h" |
|
#define MAX_AXIS_VALUE 500 |
|
264,6 → 264,7 |
/*********************************************/ |
void MM3_Calibrate(void) |
{ |
static uint8_t debugcounter = 0; |
int16_t x_min = 0, x_max = 0, y_min = 0, y_max = 0, z_min = 0, z_max = 0; |
uint8_t measurement = 50, beeper = 0; |
uint16_t timer; |
274,6 → 275,17 |
// get maximum and minimum reading of all axis |
while (measurement) |
{ |
// reset range markers if yawstick ist leftmost |
if(PPM_in[ParamSet.ChannelAssignment[CH_YAW]] > 100) |
{ |
x_min = 0; |
x_max = 0; |
y_min = 0; |
y_max = 0; |
z_min = 0; |
z_max = 0; |
} |
|
if (MM3.x_axis > x_max) x_max = MM3.x_axis; |
else if (MM3.x_axis < x_min) x_min = MM3.x_axis; |
|
295,6 → 307,12 |
timer = SetDelay(10); |
while(!CheckDelay(timer)); |
|
if(debugcounter++ > 30) |
{ |
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); |
debugcounter = 0; |
} |
|
// If thrust is less than 100, stop calibration with a delay of 0.5 seconds |
if (PPM_in[ParamSet.ChannelAssignment[CH_THRUST]] < 100) measurement--; |
} |
330,8 → 348,6 |
uint16_t div_factor; |
int16_t heading; |
|
DebugOut.Analog[11] = MM3_Timeout; |
|
if (MM3_Timeout) |
{ |
// Offset correction and normalization (values of H are +/- 512) |