156,7 → 156,7 |
|
void CalcHeading(void) |
{ |
double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
double nick_rad, roll_rad, Cx = 0, Cy = 0, Cz = 0, Hx = 0, Hy = 0; |
int16_t nick, roll; |
int16_t heading = -1; |
|
167,72 → 167,73 |
else LED_GRN_ON; |
Led_Timer = SetDelay(150); |
} |
switch(Orientation) |
|
if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match |
else |
{ |
case ORIENTATION_NC: |
Cx = (double)MagX; |
Cy = (double)MagY; |
Cz = (double)MagZ; |
break; |
switch(Orientation) |
{ |
case ORIENTATION_NC: |
Cx = (double)MagX; |
Cy = (double)MagY; |
Cz = (double)MagZ; |
break; |
|
case ORIENTATION_FC: |
// rotation of 90 deg compared to NC setup |
Cx = (double)MagY; |
Cy = -(double)MagX; |
Cz = (double)MagZ; |
break; |
} |
case ORIENTATION_FC: |
// rotation of 90 deg compared to NC setup |
Cx = (double)MagY; |
Cy = -(double)MagX; |
Cz = (double)MagZ; |
break; |
} |
|
// calculate nick and roll angle in rad |
switch(AttitudeSource) |
{ |
case ATTITUDE_SOURCE_I2C: |
cli(); // stop interrupts |
nick = I2C_WriteAttitude.Nick; |
roll = I2C_WriteAttitude.Roll; |
sei(); // start interrupts |
// calculate nick and roll angle in rad |
switch(AttitudeSource) |
{ |
case ATTITUDE_SOURCE_I2C: |
cli(); // stop interrupts |
nick = I2C_WriteAttitude.Nick; |
roll = I2C_WriteAttitude.Roll; |
sei(); // start interrupts |
break; |
case ATTITUDE_SOURCE_UART: |
cli(); // stop interrupts |
nick = ExternData.Attitude[NICK]; |
roll = ExternData.Attitude[ROLL]; |
sei(); // start interrupts |
break; |
case ATTITUDE_SOURCE_ACC: |
nick = AccAttitudeNick; |
roll = AccAttitudeRoll; |
break; |
default: |
nick = 0; |
roll = 0; |
break; |
case ATTITUDE_SOURCE_UART: |
cli(); // stop interrupts |
nick = ExternData.Attitude[NICK]; |
roll = ExternData.Attitude[ROLL]; |
sei(); // start interrupts |
break; |
case ATTITUDE_SOURCE_ACC: |
nick = AccAttitudeNick; |
roll = AccAttitudeRoll; |
break; |
default: |
nick = 0; |
roll = 0; |
break; |
} |
} |
|
nick_rad = ((double)nick * M_PI) / 1800.0; |
roll_rad = ((double)roll * M_PI) / 1800.0; |
nick_rad = ((double)nick * M_PI) / 1800.0; |
roll_rad = ((double)roll * M_PI) / 1800.0; |
|
// calculate attitude correction |
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
// calculate attitude correction |
Hx = Cx * cos(nick_rad) - Cz * sin(nick_rad); |
Hy = Cy * cos(roll_rad) + Cz * sin(roll_rad); |
|
DebugOut.Analog[27] = (int16_t)Hx; |
DebugOut.Analog[28] = (int16_t)Hy; |
DebugOut.Analog[27] = (int16_t)Hx; |
DebugOut.Analog[28] = (int16_t)Hy; |
|
// calculate Heading |
heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
if (heading < 0) heading = -heading; |
else heading = 360 - heading; |
|
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(Calibration.Version != CALIBRATION_VERSION) heading = -1; // Version of the calibration Data does not match |
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
cli(); // stop interrupts |
if(abs(heading) < 361) Heading = heading; |
else (Heading = -1); |
sei(); // start interrupts |
|
// calculate Heading |
heading = (int16_t)((180.0 * atan2(Hy, Hx)) / M_PI); |
// atan2 returns angular range from -180 deg to 180 deg in counter clockwise notation |
// but the compass course is defined in a range from 0 deg to 360 deg clockwise notation. |
if (heading < 0) heading = -heading; |
else heading = 360 - heading; |
} |
// stop interrrupts during heading update to avoid transmitting heading between the update of both bytes of the int16_t |
cli(); // stop interrupts |
if(abs(heading) < 361) Heading = heading; |
else (Heading = -1); |
sei(); // start interrupts |
} |
|
void Calibrate(void) |
333,6 → 334,11 |
// reset blinkcode |
blinkcount = 0; |
} |
else |
{ |
// restore old calibration data from eeprom |
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
} |
} |
invert_blinking = 0; |
break; |
411,6 → 417,8 |
|
int main (void) |
{ |
static uint8_t state = 0; |
static uint16_t statetimer; |
// reset input pullup |
DDRC &=~((1<<DDC6)); |
PORTC |= (1<<PORTC6); |
438,48 → 446,82 |
ExternData.CalState = 0; |
I2C_WriteCal.CalByte = 0; |
|
statetimer = SetDelay(0); |
|
// main loop |
while (1) |
{ |
FLIP_LOW; |
Delay_ms(2); |
RawMagnet1a = ADC_GetValue(MAG_X); |
RawMagnet2a = -ADC_GetValue(MAG_Y); |
RawMagnet3a = ADC_GetValue(MAG_Z); |
AccMeasurement(); |
Delay_ms(1); |
|
FLIP_HIGH; |
Delay_ms(2); |
RawMagnet1b = ADC_GetValue(MAG_X); |
RawMagnet2b = -ADC_GetValue(MAG_Y); |
RawMagnet3b = ADC_GetValue(MAG_Z); |
AccMeasurement(); |
Delay_ms(1); |
switch(state) |
{ |
case 0: |
if(CheckDelay(statetimer)) |
{ |
FLIP_LOW; |
statetimer = SetDelay(2); |
state = 1; |
} |
break; |
|
CalcFields(); |
case 1: |
if(CheckDelay(statetimer)) |
{ |
RawMagnet1a = ADC_GetValue(MAG_X); |
RawMagnet2a = -ADC_GetValue(MAG_Y); |
RawMagnet3a = ADC_GetValue(MAG_Z); |
AccMeasurement(); |
statetimer = SetDelay(1); |
state = 2; |
} |
break; |
|
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
else CalcHeading(); |
case 2: |
if(CheckDelay(statetimer)) |
{ |
FLIP_HIGH; |
statetimer = SetDelay(2); |
state = 3; |
} |
break; |
|
case 3: |
if(CheckDelay(statetimer)) |
{ |
RawMagnet1b = ADC_GetValue(MAG_X); |
RawMagnet2b = -ADC_GetValue(MAG_Y); |
RawMagnet3b = ADC_GetValue(MAG_Z); |
AccMeasurement(); |
statetimer = SetDelay(1); |
state = 0; |
|
CalcFields(); |
if(ExternData.CalState || I2C_WriteCal.CalByte) Calibrate(); |
else CalcHeading(); |
if(NC_Connected) NC_Connected--; |
if(FC_Connected) FC_Connected--; |
if(PC_Connected) PC_Connected--; |
// fall back to attitude estimation from onboard acc sensor if NC or FC does'nt send attitude data |
if(!FC_Connected && ! NC_Connected) |
{ |
AttitudeSource = ATTITUDE_SOURCE_ACC; |
Orientation = ORIENTATION_FC; |
} |
} |
break; |
|
default: // should never happen |
statetimer = SetDelay(0); |
state = 0; |
break; |
} // one full state cylce is 6 ms |
|
// check data from USART |
USART0_ProcessRxData(); |
|
if(NC_Connected) NC_Connected--; |
if(FC_Connected) FC_Connected--; |
// fall back to attitude estimation from acc sensor if NC or FC does'nt send attittude data |
if(!FC_Connected && ! NC_Connected) |
{ |
AttitudeSource = ATTITUDE_SOURCE_ACC; |
Orientation = ORIENTATION_FC; |
} |
|
if(PC_Connected) |
{ |
USART0_EnableTXD(); |
USART0_TransmitTxData(); |
PC_Connected--; |
} |
else |
{ |