68,6 → 68,7 |
|
|
AttitudeSource_t AttitudeSource = ATTITUDE_SOURCE_ACC; |
Orientation_t Orientation = ORIENTATION_FC; |
|
uint16_t Led_Timer = 0; |
|
95,16 → 96,16 |
int16_t RawMagnet2a, RawMagnet2b; |
int16_t RawMagnet3a, RawMagnet3b; |
int16_t UncalMagX, UncalMagY, UncalMagZ; // sensor signal difference without Scaling |
int16_t MagX, MagY, MagZ; // rescaled magnetic field readings |
// acc sensor variables |
int16_t RawAccX, RawAccY, RawAccZ; // raw acceleration readings |
int16_t AccX, AccY, AccZ; // rescaled acceleration readings |
uint8_t AccPresent = 0; |
uint8_t PC_Connected = 0; |
int16_t MagX = 0, MagY = 0, MagZ = 0; // rescaled magnetic field readings |
|
int16_t Heading = -1; |
// acceleration sensor variables |
int16_t RawAccX = 0, RawAccY = 0, RawAccZ = 0; // raw acceleration readings |
int16_t AccX = 0, AccY = 0, AccZ = 0; // rescaled acceleration readings |
int16_t AccAttitudeNick = 0, AccAttitudeRoll = 0; // nick and roll angle from acc |
|
int16_t Heading = -1; // the current compass heading in deg |
|
|
void CalcFields(void) |
{ |
UncalMagX = (RawMagnet1a - RawMagnet1b); |
117,12 → 118,46 |
else MagY = 0; |
if(Calibration.MagY.Range != 0) MagZ = (1024L * (int32_t)(UncalMagZ - Calibration.MagZ.Offset)) / (Calibration.MagZ.Range); |
else MagZ = 0; |
|
if(AccPresent) |
{ |
AccX = (RawAccX - Calibration.AccX.Offset); |
AccY = (RawAccY - Calibration.AccY.Offset); |
AccZ = (Calibration.AccZ.Offset - RawAccZ); |
|
#if (BOARD == 10) // the hardware 1.0 has the LIS3L02AL |
// acc mode assumes orientation like FC |
if(AccX > 136) AccAttitudeNick = -800; |
else |
if(AccX < -136) AccAttitudeNick = 800; |
else AccAttitudeNick = (int16_t)(-1800.0 * asin((double) AccX / 138.0) / M_PI); |
|
|
if(AccY > 136) AccAttitudeRoll = 800; |
else |
if(AccY < -136) AccAttitudeRoll = -800; |
else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI); |
|
#else // the hardware 1.1 has the LIS344ALH with a different axis definition (X -> -Y, Y -> X, Z -> Z) |
// acc mode assumes orientation like FC |
if(AccY > 136) AccAttitudeNick = 800; |
else |
if(AccY < -136) AccAttitudeNick = -800; |
else AccAttitudeNick = (int16_t)( 1800.0 * asin((double) AccY / 138.0) / M_PI); |
|
|
if(AccX > 136) AccAttitudeRoll = 800; |
else |
if(AccX < -136) AccAttitudeRoll = -800; |
else AccAttitudeRoll = (int16_t)( 1800.0 * asin((double) AccX / 138.0) / M_PI); |
#endif |
} |
} |
|
|
void CalcHeading(void) |
{ |
double nick_rad, roll_rad, Hx, Hy, Cx, Cy, Cz; |
double nick_rad, roll_rad, Hx, Hy, Cx = 0.0, Cy = 0.0, Cz = 0.0; |
int16_t heading = -1; |
|
// blink code for normal operation |
132,15 → 167,20 |
Led_Timer = SetDelay(500); |
} |
|
switch(Orientation) |
{ |
case ORIENTATION_NC: |
Cx = MagX; |
Cy = MagY; |
Cz = MagZ; |
break; |
|
if(ExternData.Orientation == 1) |
{ |
Cx = MagX; |
Cy = -MagY; |
case ORIENTATION_FC: |
// rotation of 90 deg compared to NC setup |
Cx = MagY; |
Cy = -MagX; |
Cz = MagZ; |
break; |
} |
|
// calculate nick and roll angle in rad |
157,21 → 197,13 |
break; |
|
case ATTITUDE_SOURCE_ACC: |
if(AccX > 125) nick_rad = M_PI / 2; |
else |
if(AccX < -125) nick_rad = -M_PI / 2; |
else |
{ |
nick_rad = asin((double) AccX / 125.0); |
} |
nick_rad = ((double)AccAttitudeNick) * M_PI / (double)(1800.0); |
roll_rad = ((double)AccAttitudeRoll) * M_PI / (double)(1800.0); |
break; |
|
if(AccY > 125) roll_rad = M_PI / 2; |
else |
if(AccY < -125) roll_rad = -M_PI / 2; |
else |
{ |
roll_rad = asin((double) AccY / 125.0); |
} |
default: |
nick_rad = 0; |
roll_rad = 0; |
break; |
} |
|
179,6 → 211,10 |
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; |
|
|
// 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 |
269,7 → 305,7 |
// Save values |
if(cal != calold) // avoid continously writing of eeprom! |
{ |
Calibration.MagY.Range = Xmax - Xmin; |
Calibration.MagX.Range = Xmax - Xmin; |
Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
Calibration.MagY.Range = Ymax - Ymin; |
Calibration.MagY.Offset = (Ymin + Ymax) / 2; |
280,12 → 316,9 |
// indicate write process by setting the led |
LED_GRN_ON; |
eeprom_write_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
Delay_ms(2000); |
// reset led state |
LED_GRN_OFF; |
Led_Timer = SetDelay(2000); |
// reset blinkcode |
blinkcount = 0; |
Led_Timer = SetDelay(1000); |
} |
} |
break; |
308,7 → 341,8 |
switch(AttitudeSource) |
{ |
case ATTITUDE_SOURCE_ACC: |
|
DebugOut.Analog[6] = AccAttitudeNick; |
DebugOut.Analog[7] = AccAttitudeRoll; |
break; |
|
case ATTITUDE_SOURCE_UART: |
341,6 → 375,7 |
DebugOut.Analog[24] = Calibration.AccX.Offset; |
DebugOut.Analog[25] = Calibration.AccY.Offset; |
DebugOut.Analog[26] = Calibration.AccZ.Offset; |
DebugOut.Analog[29] = AttitudeSource; |
} |
|
void AccMeasurement(void) |
347,9 → 382,9 |
{ |
if(AccPresent) |
{ |
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ADC2))/2; |
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ADC3))/2; |
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ADC6))/2; |
RawAccX = (RawAccX + (int16_t)ADC_GetValue(ACC_X))/2; |
RawAccY = (RawAccY + (int16_t)ADC_GetValue(ACC_Y))/2; |
RawAccZ = (RawAccZ + (int16_t)ADC_GetValue(ACC_Z))/2; |
} |
else |
{ |
357,13 → 392,8 |
RawAccY = 0; |
RawAccZ = 0; |
} |
AccX = ((RawAccX - Calibration.AccX.Offset) + AccX * 7) / 8; |
AccY = ((RawAccY - Calibration.AccY.Offset) + AccY * 7) / 8; |
AccZ = ((Calibration.AccZ.Offset - RawAccZ) + AccZ * 7) / 8; |
} |
|
|
|
int main (void) |
{ |
// reset input pullup |
378,6 → 408,11 |
|
sei(); // enable globale interrupts |
|
if(AccPresent) |
{ |
USART0_Print("ACC present\n"); |
} |
|
LED_GRN_ON; |
|
Debug_Timer = SetDelay(200); |
386,7 → 421,6 |
// read calibration info from eeprom |
eeprom_read_block(&Calibration, &eeCalibration, sizeof(Calibration)); |
|
ExternData.Orientation = 0; |
ExternData.CalState = 0; |
I2C_WriteCal.CalByte = 0; |
|
396,17 → 430,17 |
{ |
FLIP_LOW; |
Delay_ms(2); |
RawMagnet1a = ADC_GetValue(ADC0); |
RawMagnet2a = -ADC_GetValue(ADC1); |
RawMagnet3a = ADC_GetValue(ADC7); |
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(ADC0); |
RawMagnet2b = -ADC_GetValue(ADC1); |
RawMagnet3b = ADC_GetValue(ADC7); |
RawMagnet1b = ADC_GetValue(MAG_X); |
RawMagnet2b = -ADC_GetValue(MAG_Y); |
RawMagnet3b = ADC_GetValue(MAG_Z); |
AccMeasurement(); |
Delay_ms(1); |
|
417,8 → 451,16 |
|
// check data from USART |
USART0_ProcessRxData(); |
USART0_TransmitTxData(); |
|
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(!NC_Connected && ! NC_Connected) |
{ |
AttitudeSource = ATTITUDE_SOURCE_ACC; |
Orientation = ORIENTATION_FC; |
} |
|
if(PC_Connected) |
{ |
USART0_EnableTXD(); |