/trunk/compass.c |
---|
71,6 → 71,11 |
volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation |
volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation |
volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation |
s16 Hx = 0, Hy = 0; |
s32 EarthMagneticField = 100; |
s32 EarthMagneticFieldFiltered = 100; |
s32 EarthMagneticInclination = 0; |
u8 ErrorDisturbedEarthMagnetField = 0; |
#define COMPASS_NONE 0 |
#define COMPASS_MK3MAG 1 |
120,9 → 125,8 |
{ |
// calculate attitude correction |
// a float implementation takes too long |
s16 tmp, Hx, Hy; |
s16 tmp; |
s32 sinnick, cosnick, sinroll, cosroll; |
tmp = FromFlightCtrl.AngleNick/10; // in deg |
sinnick = (s32)c_sin_8192(tmp); |
cosnick = (s32)c_cos_8192(tmp); |
140,6 → 144,7 |
} |
} |
void Compass_Update(void) |
{ |
static s16vec_t old; |
/trunk/compass.h |
---|
11,6 → 11,11 |
extern volatile s16vec_t MagVector; // current magnetic field vector |
extern volatile s16 Compass_Heading; // current heading direction |
extern volatile u8 Compass_CalState; // current calibration state |
extern s32 EarthMagneticField; |
extern s32 EarthMagneticInclination; |
extern s32 EarthMagneticFieldFiltered; |
extern u8 ErrorDisturbedEarthMagnetField; |
extern s16 Hx, Hy; |
#define COMPASS_NONE 0 |
#define COMPASS_MK3MAG 1 |
/trunk/main.c |
---|
285,6 → 285,12 |
sprintf(ErrorMSG,"GPS Fix lost "); |
newErrorCode = 21; |
} |
else if(ErrorDisturbedEarthMagnetField) |
{ |
LED_RED_ON; |
sprintf(ErrorMSG,"Magnet error "); |
newErrorCode = 22; |
} |
else // no error occured |
{ |
StopNavigation = 0; |
/trunk/main.h |
---|
14,7 → 14,7 |
#define VERSION_MAJOR 0 |
#define VERSION_MINOR 25 |
#define VERSION_PATCH 8 |
#define VERSION_PATCH 10 |
// 0 = A |
// 1 = B |
// 2 = C |
35,7 → 35,7 |
#define VERSION_SERIAL_MINOR 0 |
#ifndef FOLLOW_ME |
#define FC_SPI_COMPATIBLE 23 |
#define FC_SPI_COMPATIBLE 24 |
#else |
#define FC_SPI_COMPATIBLE 0xFF |
#endif |
/trunk/menu.c |
---|
395,10 → 395,15 |
} |
else |
{ |
if(GeoMagDec < 0) sign = '-'; |
else sign = '+'; |
LCD_printfxy(0,0,"Magnetic Field"); |
LCD_printfxy(0,1,"X:%5i",MagVector.X); |
LCD_printfxy(0,2,"Y:%5i",MagVector.Y); |
LCD_printfxy(0,3,"Z:%5i",MagVector.Z); |
LCD_printfxy(8,1,"Field:%3i",EarthMagneticField/5); |
LCD_printfxy(8,2,"Dec:%c%i.%1i", sign, abs(GeoMagDec)/10,abs(GeoMagDec)%10); |
LCD_printfxy(8,3,"Inc:%2i", EarthMagneticInclination); |
LCD_printfxy(15,3,"(CAL)"); |
} |
if(Keys & KEY4) // next step |
/trunk/mymath.c |
---|
5,6 → 5,8 |
// sinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
const s16 sinlookup[91] = {0, 143, 286, 429, 571, 714, 856, 998, 1140, 1282, 1423, 1563, 1703, 1843, 1982, 2120, 2258, 2395, 2531, 2667, 2802, 2936, 3069, 3201, 3332, 3462, 3591, 3719, 3846, 3972, 4096, 4219, 4341, 4462, 4581, 4699, 4815, 4930, 5043, 5155, 5266, 5374, 5482, 5587, 5691, 5793, 5893, 5991, 6088, 6183, 6275, 6366, 6455, 6542, 6627, 6710, 6791, 6870, 6947, 7022, 7094, 7165, 7233, 7299, 7363, 7424, 7484, 7541, 7595, 7648, 7698, 7746, 7791, 7834, 7875, 7913, 7949, 7982, 8013, 8041, 8068, 8091, 8112, 8131, 8147, 8161, 8172, 8181, 8187, 8191, 8192}; |
//0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 38 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 |
const s16 arccos64[65] = {90,89,88,87,86, 85, 84, 83, 83, 82, 81, 80, 79, 78, 77, 76, 75, 74, 73, 72, 71, 71, 70, 69, 68, 67, 66, 65, 64, 63, 62, 61, 60, 59, 58, 57, 56, 55, 54, 53, 51, 50, 49, 48, 47, 45, 44, 43, 41, 40, 39, 37, 36, 34, 32, 31, 29, 27, 25, 23, 20, 18, 14, 10, 0}; |
s16 c_sin_8192(s16 angle) |
{ |
33,6 → 35,12 |
return (sinus * m * n); |
} |
s16 c_arccos2(s32 a,s32 b) |
{ |
if(a>b) return(0); |
return(arccos64[64 * a / b]); |
} |
// cosinus with argument in degree at an angular resolution of 1 degree and a discretisation of 13 bit. |
s16 c_cos_8192(s16 angle) |
{ |
/trunk/mymath.h |
---|
12,4 → 12,7 |
// fast arctan implementation |
s32 c_atan2_546(s32 y, s32 x); |
// fast arccos implementation |
s16 c_arccos2(s32 a,s32 b); |
#endif // _MYMATH_H |
/trunk/ncmag.c |
---|
288,6 → 288,7 |
void NCMAG_Calibrate(void) |
{ |
u8 msg[64]; |
static s16 Xmin = 0, Xmax = 0, Ymin = 0, Ymax = 0, Zmin = 0, Zmax = 0; |
static s16 X = 0, Y = 0, Z = 0; |
static u8 OldCalState = 0; |
343,17 → 344,24 |
{ |
NCMAG_IsCalibrated = NCMag_CalibrationWrite(); |
BeepTime = 2500; |
UART1_PutString("\r\n Calibration okay"); |
UART1_PutString("\r\n Calibration okay\n\r"); |
} |
else |
{ |
UART1_PutString("\r\n Calibration FAILED - Values too low"); |
if(Calibration.MagX.Range < MIN_CALIBRATION) UART1_PutString("X! "); |
if(Calibration.MagY.Range < MIN_CALIBRATION) UART1_PutString("y! "); |
if(Calibration.MagZ.Range < MIN_CALIBRATION) UART1_PutString("Z! "); |
UART1_PutString("\r\n"); |
// restore old calibration data from eeprom |
NCMAG_IsCalibrated = NCMag_CalibrationRead(); |
UART1_PutString("\r\n Calibration FAILED - Values too low: "); |
if(Calibration.MagX.Range < MIN_CALIBRATION) UART1_PutString("X! "); |
if(Calibration.MagY.Range < MIN_CALIBRATION) UART1_PutString("Y! "); |
if(Calibration.MagZ.Range < MIN_CALIBRATION) UART1_PutString("Z! "); |
} |
sprintf(msg, "X: (%i - %i = %i)\r\n",Xmax,Xmin,Xmax - Xmin); |
UART1_PutString(msg); |
sprintf(msg, "Y: (%i - %i = %i)\r\n",Ymax,Ymin,Ymax - Ymin); |
UART1_PutString(msg); |
sprintf(msg, "Z: (%i - %i = %i)\r\n",Zmax,Zmin,Zmax - Zmin); |
UART1_PutString(msg); |
} |
break; |
393,10 → 401,19 |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.X = raw; |
raw = pRxBuffer[2]<<8; |
raw+= pRxBuffer[3]; |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Y = raw; |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
{ |
if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Z = raw; // here Z and Y are exchanged |
else MagRawVector.Y = raw; |
} |
raw = pRxBuffer[4]<<8; |
raw+= pRxBuffer[5]; |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) MagRawVector.Z = raw; |
if(raw >= NCMAG_MIN_RAWVALUE && raw <= NCMAG_MAX_RAWVALUE) |
{ |
if(NCMAG_Identification2.Sub == 0x3c) MagRawVector.Y = raw; // here Z and Y are exchanged |
else MagRawVector.Z = raw; |
} |
} |
if(Compass_CalState || !NCMAG_IsCalibrated) |
{ // mark out data invalid |
577,6 → 594,7 |
// try to catch the I2C buffer within 0 ms |
if(I2C_LockBuffer(0)) |
{ |
// s16 tmp; |
u16 TxBytes = 0; |
// set register pointer |
I2C_Buffer[TxBytes++] = REG_MAG_DATAX_MSB; |
599,6 → 617,42 |
} |
} |
//---------------------------------------------------------------- |
void InitNC_MagnetSensor(void) |
{ |
s16 xscale, yscale, zscale; |
u8 crb_gain, cra_rate; |
u8 i = 0, retval = 1; |
switch(NCMAG_MagType) |
{ |
case MAG_TYPE_HMC5843: |
crb_gain = HMC5843_CRB_GAIN_10GA; |
cra_rate = HMC5843_CRA_RATE_50HZ; |
xscale = HMC5843_TEST_XSCALE; |
yscale = HMC5843_TEST_YSCALE; |
zscale = HMC5843_TEST_ZSCALE; |
break; |
case MAG_TYPE_LSM303DLH: |
crb_gain = LSM303DLH_CRB_GAIN_13GA; |
cra_rate = LSM303DLH_CRA_RATE_75HZ; |
xscale = LSM303DLH_TEST_XSCALE; |
yscale = LSM303DLH_TEST_YSCALE; |
zscale = LSM303DLH_TEST_ZSCALE; |
break; |
default: |
return(0); |
} |
MagConfig.cra = cra_rate|CRA_MODE_NORMAL; |
MagConfig.crb = crb_gain; |
MagConfig.mode = MODE_CONTINUOUS; |
NCMAG_SetMagConfig(); |
} |
// -------------------------------------------------------- |
void NCMAG_Update(void) |
{ |
617,9 → 671,7 |
if(++send_config == 25) // 500ms |
{ |
send_config = 0; |
MagConfig.mode = MODE_CONTINUOUS; |
// activate positive bias field |
NCMAG_SetMagConfig(); |
InitNC_MagnetSensor(); |
TimerUpdate = SetDelay(15); // back into the old time-slot |
} |
else |
634,6 → 686,7 |
} |
} |
// -------------------------------------------------------- |
u8 NCMAG_SelfTest(void) |
{ |
817,7 → 870,7 |
NCMAG_IsCalibrated = 0; |
} else UART1_PutString("\r\n Selftest ok"); |
} |
else InitNC_MagnetSensor(); |
} |
else |
{ |
/trunk/spi_slave.c |
---|
96,6 → 96,7 |
s32 Kalman_K = 32; |
s32 Kalman_MaxDrift = 5 * 16; |
s32 Kalman_MaxFusion = 64; |
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}; |
109,6 → 110,7 |
u8 DisableFC_Sticks = 0; |
u8 NC_GPS_ModeCharacter = ' '; |
u8 FCCalibActive = 0; |
u8 FC_is_Calibrated = 0; |
SPI_Version_t FC_Version; |
303,7 → 305,7 |
ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K; |
ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion; |
ToFlightCtrl.Param.sByte[2] = (s8) Kalman_MaxDrift; |
ToFlightCtrl.Param.Byte[3] = (u8) SerialLinkOkay; |
ToFlightCtrl.Param.Byte[3] = (u8) Kalman_Kompass; |
ToFlightCtrl.Param.sByte[4] = (s8) ToFcGpsZ; |
ToFlightCtrl.Param.Byte[5] = (s8) ToFC_Rotate_C; |
ToFlightCtrl.Param.Byte[6] = (s8) ToFC_Rotate_S; |
339,6 → 341,7 |
ToFlightCtrl.Param.Byte[6] = DebugOut.Status[1]; |
ToFlightCtrl.Param.Byte[7] = ErrorCode; |
ToFlightCtrl.Param.Byte[8] = NC_GPS_ModeCharacter; |
ToFlightCtrl.Param.Byte[9] = SerialLinkOkay; |
break; |
case SPI_NCCMD_GPSINFO: |
366,12 → 369,7 |
{ |
ToFlightCtrl.Param.sInt[5] = (s16)ToFC_AltitudeSetpoint; |
} |
// DebugOut.Analog[25] = (s16)ToFlightCtrl.Param.Byte[9]; |
// DebugOut.Analog[20] = ToFlightCtrl.Param.sInt[5]; |
break; |
case SPI_NCCMD_HOTT_INFO: |
switch(hott_index++) |
{ |
/* |
typedef struct |
{ |
400,6 → 398,39 |
unsigned char EndByte; // 0x7D |
} GPSPacket_t; |
*/ |
/* |
typedef struct |
{ |
unsigned char StartByte; //0 0x7C |
unsigned char Packet_ID; //1 HOTT_GENERAL_PACKET_ID |
unsigned char WarnBeep; //2 Anzahl der Töne 0..36 |
unsigned char VoltageCell1; //3 208 = 4,16V (Voltage * 50 = Wert) |
unsigned char VoltageCell2; //4 |
unsigned char VoltageCell3; //5 |
unsigned char VoltageCell4; //6 |
unsigned char VoltageCell5; //7 |
unsigned char VoltageCell6; //8 |
unsigned int Battery1; //9 51 = 5,1V |
unsigned int Battery2; //11 51 = 5,1V |
unsigned char Temperature1; //13 44 = 24°C, 0 = -20°C |
unsigned char Temperature2; //14 44 = 24°C, 0 = -20°C |
unsigned char FuelPercent; //15 |
signed int FuelCapacity; //16 |
unsigned int Rpm; |
unsigned int Altitude; |
unsigned int m_sec; // 3000 = 0 |
unsigned char m_3sec; // 120 = 0 |
unsigned int Current; // 1 = 0.1A |
unsigned int InputVoltage; // 66 = 6,6V |
unsigned int Capacity; // 1 = 10mAh |
unsigned char NullByte1; // 0x00 |
unsigned char NullByte2; // 0x00 |
unsigned char EndByte; // 0x7D |
} HoTTGeneral_t; |
*/ |
case SPI_NCCMD_HOTT_INFO: |
switch(hott_index++) |
{ |
case 0: |
//Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden |
//53.285788 7.4847269 --> N53° 17.14728 E7° 29.08362 --> N53° 17' 8.837" E7° 29' 5.017" |
426,7 → 457,6 |
ToFlightCtrl.Param.Byte[7] = i1 / 256; |
ToFlightCtrl.Param.Byte[8] = i2 % 256; |
ToFlightCtrl.Param.Byte[9] = i2 / 256; |
break; |
case 1: |
ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID; |
449,12 → 479,19 |
i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m |
ToFlightCtrl.Param.Byte[7] = i1 % 256; |
ToFlightCtrl.Param.Byte[8] = i1 / 256; |
break; |
case 2: |
ToFlightCtrl.Param.Byte[11] = HOTT_GENERAL_PACKET_ID; |
ToFlightCtrl.Param.Byte[0] = 5; // index |
ToFlightCtrl.Param.Byte[1] = 2; // how many |
ToFlightCtrl.Param.Byte[2] = EarthMagneticField / (5 * 2); |
ToFlightCtrl.Param.Byte[3] = EarthMagneticInclination / 2; |
hott_index = 0; |
break; |
default: |
ToFlightCtrl.Param.Byte[0] = 255; |
hott_index = 0; |
break; |
default: |
ToFlightCtrl.Param.Byte[0] = 255; |
hott_index = 0; |
break; |
} |
break; |
default: |
489,11 → 526,11 |
{ |
HeadFreeStartAngle = Compass_Heading * 10; |
Compass_Init(); |
FCCalibActive = 1; |
FCCalibActive = 10; |
} |
else |
{ |
FCCalibActive = 0; |
if(FCCalibActive) if(--FCCalibActive == 0) FC_is_Calibrated = 1; |
} |
if(FC.StatusFlags & FC_STATUS_START) |
{ |
/trunk/spi_slave.h |
---|
16,6 → 16,7 |
#define SPI_FCCMD_ACCU 16 |
extern s32 Kalman_K; |
extern s32 Kalman_Kompass ; |
extern s32 Kalman_MaxDrift; |
extern s32 Kalman_MaxFusion; |
extern s32 ToFcGpsZ; |
25,6 → 26,9 |
extern u32 ToFC_AltitudeRate; |
extern s32 ToFC_AltitudeSetpoint; |
extern u8 NC_GPS_ModeCharacter; |
extern u8 FC_is_Calibrated; |
extern u8 FCCalibActive; |
typedef struct |
{ |
u8 Command; |
61,6 → 65,7 |
#define HOTT_VARIO_PACKET_ID 0x89 |
#define HOTT_GPS_PACKET_ID 0x8A |
#define HOTT_ELECTRIC_AIR_PACKET_ID 0x8E |
#define HOTT_GENERAL_PACKET_ID 0x8D |
typedef struct |
{ |
/trunk/uart1.c |
---|
147,11 → 147,11 |
"SPI Okay ", |
"I2C Error ", |
"I2C Okay ", //15 |
"16 ",// "Kalman_K ", |
"16 KompassKalm ",// "Kalman_K ", |
"17 ", |
"18 ", |
"20 ", |
"23 ",//20 |
"18 Horizontal ", |
"19 Normal ", |
"EarthMagnet [%] ", //20 |
"Magnet X ", |
"Magnet Y ", |
"Magnet Z ", |