/trunk/compass.c |
---|
78,6 → 78,7 |
s32 EarthMagneticInclinationFiltered = 0; |
s32 EarthMagneticInclinationTheoretic = 0; |
u8 ErrorDisturbedEarthMagnetField = 0; |
s16 EarthMagneticStrengthTheoretic = 0; // in mT (50 in Germany - 22 in Brazil) |
#define COMPASS_NONE 0 |
#define COMPASS_MK3MAG 1 |
90,7 → 91,10 |
{ |
case COMPASS_NONE: |
UART1_PutString("\r\n Looking for compass"); |
if(Version_HW > 11) |
if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG; |
else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
/* if(Version_HW > 11) |
{ |
if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
else if( MK3MAG_Init()) Compass_Device = COMPASS_MK3MAG; |
100,6 → 104,7 |
if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG; |
else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG; |
} |
*/ |
break; |
case COMPASS_NCMAG: |
/trunk/compass.h |
---|
17,6 → 17,7 |
extern s32 EarthMagneticInclinationTheoretic; |
extern s32 EarthMagneticFieldFiltered; |
extern u8 ErrorDisturbedEarthMagnetField; |
extern s16 EarthMagneticStrengthTheoretic; |
extern s16 Hx, Hy; |
#define COMPASS_NONE 0 |
/trunk/main.c |
---|
510,4 → 510,4 |
} |
} |
// DebugOut.Analog[] |
/trunk/main.h |
---|
14,7 → 14,7 |
#define VERSION_MAJOR 0 |
#define VERSION_MINOR 26 |
#define VERSION_PATCH 1 |
#define VERSION_PATCH 4 |
// 0 = A |
// 1 = B |
// 2 = C |
39,7 → 39,7 |
#define VERSION_SERIAL_MINOR 0 |
#ifndef FOLLOW_ME |
#define FC_SPI_COMPATIBLE 26 |
#define FC_SPI_COMPATIBLE 27 |
#else |
#define FC_SPI_COMPATIBLE 0xFF |
#endif |
/trunk/mk3mag.c |
---|
233,7 → 233,9 |
do |
{ |
MK3MAG_SendCommand(MK3MAG_CMD_VERSION); |
timeout = SetDelay(250); |
if(Version_HW > 11) timeout = SetDelay(100); |
else timeout = SetDelay(250); |
do |
{ |
if (MK3MAG_Version.Major != 0xFF) break; // break loop on success |
291,8 → 293,14 |
if(last_state != Compass_CalState) |
{ |
UART1_PutString("\r\nMK3Mag calibration\n\r"); |
if(EarthMagneticStrengthTheoretic) |
{ |
MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50; |
sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic); |
UART1_PutString(msg); |
} |
else UART1_PutString("without GPS\n\r"); |
} |
x_max = -30000; y_max = -30000; z_max = -30000; |
x_min = 30000; y_min = 30000; z_min = 30000; |
break; |
313,16 → 321,15 |
if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration)) |
{ |
BeepTime = 2500; |
UART1_PutString("\r\n Calibration okay\n\r"); |
UART1_PutString("\r\n-> Calibration okay <-\n\r"); |
} |
else |
{ |
UART1_PutString("\r\nCalibration FAILED - Values too low: "); |
if((x_max - x_min) < MinCaclibration) UART1_PutString("X! "); |
if((y_max - y_min) < MinCaclibration) UART1_PutString("y! "); |
if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! "); |
if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! "); |
UART1_PutString("\r\n"); |
sprintf(msg, "Minimum is: %i \r\n",MinCaclibration); |
} |
UART1_PutString(msg); |
sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min); |
331,7 → 338,8 |
UART1_PutString(msg); |
sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min); |
UART1_PutString(msg); |
sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration); |
UART1_PutString(msg); |
break; |
default: |
break; |
/trunk/ncmag.c |
---|
170,7 → 170,7 |
#define HMC5843_TEST_YSCALE 555 |
#define HMC5843_TEST_ZSCALE 555 |
// clibration range |
#define HMC5843_CALIBRATION_RANGE 550 |
#define HMC5843_CALIBRATION_RANGE 600 |
// the special LSM302DLH interface |
// bit mask for rate |
196,7 → 196,7 |
#define LSM303DLH_TEST_YSCALE 495 |
#define LSM303DLH_TEST_ZSCALE 470 |
// clibration range |
#define LSM303_CALIBRATION_RANGE 500 |
#define LSM303_CALIBRATION_RANGE 550 |
// the i2c ACC interface |
#define ACC_SLAVE_ADDRESS 0x30 // i2c slave for acc. sensor registers |
348,6 → 348,13 |
UART1_PutString("\r\n\r\nLSM303 calibration\n\r"); |
MinCaclibration =LSM303_CALIBRATION_RANGE; |
} |
if(EarthMagneticStrengthTheoretic) |
{ |
MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50; |
sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic); |
UART1_PutString(msg); |
} |
else UART1_PutString("without GPS\n\r"); |
Calibration.MagX.Range = Xmax - Xmin; |
Calibration.MagX.Offset = (Xmin + Xmax) / 2; |
359,17 → 366,15 |
{ |
NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
BeepTime = 2500; |
UART1_PutString("\r\n Calibration okay\n\r"); |
UART1_PutString("\r\n-> Calibration okay <-\n\r"); |
} |
else |
{ |
UART1_PutString("\r\nCalibration FAILED - Values too low: "); |
if(Calibration.MagX.Range < MinCaclibration) UART1_PutString("X! "); |
if(Calibration.MagY.Range < MinCaclibration) UART1_PutString("y! "); |
if(Calibration.MagY.Range < MinCaclibration) UART1_PutString("Y! "); |
if(Calibration.MagZ.Range < MinCaclibration) UART1_PutString("Z! "); |
UART1_PutString("\r\n"); |
sprintf(msg, "Minimum is: %i \r\n",MinCaclibration); |
UART1_PutString(msg); |
// restore old calibration data from eeprom |
NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
380,6 → 385,8 |
UART1_PutString(msg); |
sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin); |
UART1_PutString(msg); |
sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration); |
UART1_PutString(msg); |
} |
break; |
431,7 → 438,7 |
if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Y = raw; // here Z and Y are exchanged |
else MagRawVector.Z = raw; |
} |
//MagRawVector.X += 2 * ((s32) FC.Poti[7] - 128); |
} |
if(Compass_CalState || !NCMAG_IsCalibrated) |
{ // mark out data invalid |
/trunk/spi_slave.c |
---|
101,7 → 101,7 |
s32 Kalman_Kompass = 32; |
s32 ToFcGpsZ = 0; |
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN}; |
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, SPI_NCCMD_HOTT_INFO, SPI_NCCMD_KALMAN, SPI_MISC, SPI_NCCMD_KALMAN }; |
u8 SPI_CommandCounter = 0; |
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0; |
s32 HeadFreeStartAngle = 0; |
292,6 → 292,8 |
// avoid sending data via SPI during the update of the ToFlightCtrl structure |
VIC_ITCmd(SSP0_ITLine, DISABLE); // disable SPI interrupt |
ToFlightCtrl.CompassHeading = Compass_Heading; |
//ToFlightCtrl.CompassHeading += 360 + ((s32) FC.Poti[7] - 100); |
//ToFlightCtrl.CompassHeading %= 360; |
DebugOut.Analog[10] = ToFlightCtrl.CompassHeading; |
if(ToFlightCtrl.CompassHeading >= 0) ToFlightCtrl.CompassHeading = (360 + ToFlightCtrl.CompassHeading + FromFlightCtrl.GyroYaw / 12) % 360; |
// ToFlightCtrl.MagVecX = MagVector.X; |
347,7 → 349,23 |
ToFlightCtrl.Param.Byte[7] = ErrorCode; |
ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter; |
ToFlightCtrl.Param.Byte[9] = SerialLinkOkay; |
ToFlightCtrl.Param.Byte[10] = 0; |
ToFlightCtrl.Param.Byte[11] = 0; |
break; |
case SPI_MISC: |
ToFlightCtrl.Param.Byte[0] = EarthMagneticFieldFiltered/5; |
ToFlightCtrl.Param.Byte[1] = EarthMagneticInclination; |
ToFlightCtrl.Param.Byte[2] = EarthMagneticInclinationTheoretic; |
ToFlightCtrl.Param.Byte[3] = 0; |
ToFlightCtrl.Param.Byte[4] = 0; |
ToFlightCtrl.Param.Byte[5] = 0; |
ToFlightCtrl.Param.Byte[6] = 0; |
ToFlightCtrl.Param.Byte[7] = 0; |
ToFlightCtrl.Param.Byte[8] = 0; |
ToFlightCtrl.Param.Byte[9] = 0; |
ToFlightCtrl.Param.Byte[10] = 0; |
ToFlightCtrl.Param.Byte[11] = 0; |
break; |
case SPI_NCCMD_GPSINFO: |
ToFlightCtrl.Param.Byte[0] = GPSData.Flags; |
/trunk/spi_slave.h |
---|
67,6 → 67,7 |
#define SPI_NCCMD_VERSION 104 |
#define SPI_NCCMD_GPSINFO 105 |
#define SPI_NCCMD_HOTT_INFO 106 |
#define SPI_MISC 107 |
#define HOTT_VARIO_PACKET_ID 0x89 |
#define HOTT_GPS_PACKET_ID 0x8A |