Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 329 → Rev 330

/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 ",