Subversion Repositories NaviCtrl

Compare Revisions

Ignore whitespace Rev 328 → Rev 329

/trunk/compass.c
157,9 → 157,9
default:
break;
}
DebugOut.Analog[24] = MagVector.X;
DebugOut.Analog[25] = MagVector.Y;
DebugOut.Analog[26] = MagVector.Z;
DebugOut.Analog[21] = MagVector.X;
DebugOut.Analog[22] = MagVector.Y;
DebugOut.Analog[23] = MagVector.Z;
if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normally changing
 
if(check_value_counter > 5000)
/trunk/main.c
134,7 → 134,7
 
void CheckErrors(void)
{
static no_error_delay = 0;
static s32 no_error_delay = 0;
s32 newErrorCode = 0;
UART_VersionInfo.HardwareError[0] = 0;
 
/trunk/main.h
14,7 → 14,7
 
#define VERSION_MAJOR 0
#define VERSION_MINOR 25
#define VERSION_PATCH 6
#define VERSION_PATCH 8
// 0 = A
// 1 = B
// 2 = C
35,7 → 35,7
#define VERSION_SERIAL_MINOR 0
 
#ifndef FOLLOW_ME
#define FC_SPI_COMPATIBLE 22
#define FC_SPI_COMPATIBLE 23
#else
#define FC_SPI_COMPATIBLE 0xFF
#endif
/trunk/ncmag.c
113,9 → 113,11
#define REG_MAG_DATAZ_MSB 0x07
#define REG_MAG_DATAZ_LSB 0x08
#define REG_MAG_STATUS 0x09
 
#define REG_MAG_IDA 0x0A
#define REG_MAG_IDB 0x0B
#define REG_MAG_IDC 0x0C
#define REG_MAG_IDF 0x0F
 
// bit mask for configuration mode
#define CRA_MODE_MASK 0x03
215,11 → 217,16
u8 B;
u8 C;
} __attribute__((packed)) Identification_t;
 
volatile Identification_t NCMAG_Identification;
 
typedef struct
{
u8 Sub;
} __attribute__((packed)) Identification2_t;
volatile Identification2_t NCMAG_Identification2;
 
typedef struct
{
u8 cra;
u8 crb;
u8 mode;
366,6 → 373,15
memcpy((u8 *)&NCMAG_Identification, pRxBuffer, sizeof(NCMAG_Identification));
}
}
 
void NCMAG_UpdateIdentification_Sub(u8* pRxBuffer, u8 RxBufferSize)
{ // if number of bytes are matching
if(RxBufferSize == sizeof(NCMAG_Identification2))
{
memcpy((u8 *)&NCMAG_Identification2, pRxBuffer, sizeof(NCMAG_Identification2));
}
}
 
// rx data handler for magnetic sensor raw data
void NCMAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
{ // if number of bytes are matching
533,6 → 549,28
return(retval);
}
 
u8 NCMAG_GetIdentification_Sub(void)
{
u8 retval = 0;
// try to catch the i2c buffer within 100 ms timeout
if(I2C_LockBuffer(100))
{
u16 TxBytes = 0;
NCMAG_Identification2.Sub = 0xFF;
I2C_Buffer[TxBytes++] = REG_MAG_IDF;
// initiate transmission
if(I2C_Transmission(MAG_SLAVE_ADDRESS, TxBytes, &NCMAG_UpdateIdentification_Sub, sizeof(NCMAG_Identification2)))
{
if(I2C_WaitForEndOfTransmission(100))
{
if(I2C_Error == I2C_ERROR_NONE) retval = 1;
}
}
}
return(retval);
}
 
 
// ----------------------------------------------------------------------------------------
void NCMAG_GetMagVector(void)
{
726,20 → 764,36
repeat = 0;
do
{
retval = NCMAG_GetIdentification_Sub();
if(retval) break; // break loop on success
UART1_PutString(".");
repeat++;
}while(repeat < 12);
retval = 0;
do
{
retval = NCMAG_GetIdentification();
if(retval) break; // break loop on success
UART1_PutString(".");
repeat++;
}while(repeat < 12);
 
// if we got an answer to id request
if(retval)
{
u8 n1[] = "HMC5843";
u8 n2[] = "LSM303DLH";
u8 n1[] = "\n\r HMC5843";
u8 n2[] = "\n\r LSM303DLH";
u8 n3[] = "\n\r LSM303DLM";
u8* pn;
if(NCMAG_MagType == MAG_TYPE_LSM303DLH) pn = n2;
else pn = n1;
sprintf(msg, " %s ID%d/%d/%d", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C);
pn = n1;
if(NCMAG_MagType == MAG_TYPE_LSM303DLH)
{
if(NCMAG_Identification2.Sub == 0x3c) pn = n3;
else pn = n2;
}
 
sprintf(msg, " %s ID 0x%02x/%02x/%02x-%02x", pn, NCMAG_Identification.A, NCMAG_Identification.B, NCMAG_Identification.C,NCMAG_Identification2.Sub);
UART1_PutString(msg);
if ( (NCMAG_Identification.A == MAG_IDA)
&& (NCMAG_Identification.B == MAG_IDB)
746,21 → 800,24
&& (NCMAG_Identification.C == MAG_IDC))
{
NCMAG_Present = 1;
if(!NCMAG_SelfTest())
{
UART1_PutString(" Selftest failed!!!!!!!!!!!!!!!!!!!!");
LED_RED_ON;
NCMAG_IsCalibrated = 0;
}
else
{
if(EEPROM_Init())
 
if(EEPROM_Init())
{
NCMAG_IsCalibrated = NCMag_CalibrationRead();
if(!NCMAG_IsCalibrated) UART1_PutString("\r\n Not calibrated!");
}
else UART1_PutString("\r\n Calibration data not available!!!!!!!!!!!!!!!");
else UART1_PutString("\r\n EEPROM data not available!!!!!!!!!!!!!!!");
if(NCMAG_Identification2.Sub == 0x00)
{
if(!NCMAG_SelfTest())
{
UART1_PutString("\r\n Selftest failed!!!!!!!!!!!!!!!!!!!!\r\n");
LED_RED_ON;
NCMAG_IsCalibrated = 0;
} else UART1_PutString("\r\n Selftest ok");
}
 
}
else
{
/trunk/spi_slave.c
98,7 → 98,7
s32 Kalman_MaxFusion = 64;
s32 ToFcGpsZ = 0;
 
u8 SPI_CommandSequence[] = { SPI_NCCMD_VERSION, SPI_NCCMD_KALMAN, SPI_NCCMD_GPSINFO ,SPI_NCCMD_KALMAN, 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};
u8 SPI_CommandCounter = 0;
s32 ToFC_Rotate_C = 64, ToFC_Rotate_S = 0;
s32 HeadFreeStartAngle = 0;
108,6 → 108,7
u8 FromFC_VarioCharacter = ' ';
u8 DisableFC_Sticks = 0;
u8 NC_GPS_ModeCharacter = ' ';
u8 FCCalibActive = 0;
 
SPI_Version_t FC_Version;
 
274,10 → 275,10
void SPI0_UpdateBuffer(void)
{
static u32 timeout = 0;
static u8 counter = 50;
static u8 counter = 50,hott_index = 0;
static u8 CompassCalState = 0;
static u8 FCCalibActive = 0;
s16 tmp;
s32 i1,i2;
 
if (SPI_RxBuffer_Request)
{
297,7 → 298,7
#define FLAG_GPS_AID 0x01
switch (ToFlightCtrl.Command)
{
case SPI_NCCMD_KALMAN:
case SPI_NCCMD_KALMAN: // wird am häufigsten betätigt
CalcHeadFree();
ToFlightCtrl.Param.sByte[0] = (s8) Kalman_K;
ToFlightCtrl.Param.sByte[1] = (s8) Kalman_MaxFusion;
368,7 → 369,94
// 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
{
unsigned char StartByte; //0 // 0x7C
unsigned char Packet_ID; //1 // 0x89 - Vario ID
unsigned char WarnBeep; //2 // Anzahl der Töne 0..36
unsigned char Heading; //3 // 1 = 2°
unsigned int Speed; //4+5 // in km/h
unsigned char Lat_North; //6
unsigned char Lat_G; //7
unsigned char Lat_M; //8
unsigned char Lat_Sek1; //9
unsigned char Lat_Sek2; //10
unsigned char Lon_East; //11
unsigned char Lon_G; //12
unsigned char Lon_M; //13
unsigned char Lon_Sek1; //14
unsigned char Lon_Sek2; //15
unsigned int Distance; //16+17 // 9000 = 0m
unsigned int Altitude; //18+19 // 500 = 0m
unsigned int m_sec; //20+21 // 3000 = 0
unsigned int m_3sec; //22+23 // 3000 = 0
unsigned int m_10sec; //24+25 // 3000 = 0
unsigned char NullByte; // 0x00
unsigned char NullByte1; // 0x00
unsigned char EndByte; // 0x7D
} GPSPacket_t;
*/
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"
ToFlightCtrl.Param.Byte[11] = HOTT_GPS_PACKET_ID;
ToFlightCtrl.Param.Byte[0] = 3; // index
ToFlightCtrl.Param.Byte[1] = 9-1; // how many
//-----------------------------
ToFlightCtrl.Param.Byte[2] = NaviData.HomePositionDeviation.Bearing / 2;
i1 = GPSData.Speed_Ground; // in cm/sec
i1 *= 36;
i1 /= 1000;
ToFlightCtrl.Param.Byte[3] = i1 % 256;
ToFlightCtrl.Param.Byte[4] = i1 / 256;
//-----------------------------
if(GPSData.Position.Latitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = S
else ToFlightCtrl.Param.Byte[5] = 0; // 1 = S
i1 = abs(GPSData.Position.Latitude)/10000000L;
i2 = abs(GPSData.Position.Latitude)%10000000L;
i1 *= 100;
i1 += i2 / 100000;
i2 = i2 % 100000;
i2 /= 10;
ToFlightCtrl.Param.Byte[6] = i1 % 256;
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;
ToFlightCtrl.Param.Byte[0] = 11; // index
ToFlightCtrl.Param.Byte[1] = 8-1; // how many
//-----------------------------
if(GPSData.Position.Longitude < 0) ToFlightCtrl.Param.Byte[2] = 1; // 1 = E
else ToFlightCtrl.Param.Byte[2] = 0; // 1 = S
i1 = abs(GPSData.Position.Longitude)/10000000L;
i2 = abs(GPSData.Position.Longitude)%10000000L;
i1 *= 100;
i1 += i2 / 100000;
i2 = i2 % 100000;
i2 /= 10;
ToFlightCtrl.Param.Byte[3] = i1 % 256;
ToFlightCtrl.Param.Byte[4] = i1 / 256;
ToFlightCtrl.Param.Byte[5] = i2 % 256;
ToFlightCtrl.Param.Byte[6] = i2 / 256;
//-----------------------------
i1 = NaviData.HomePositionDeviation.Distance / 10; // dann in m
ToFlightCtrl.Param.Byte[7] = i1 % 256;
ToFlightCtrl.Param.Byte[8] = i1 / 256;
hott_index = 0;
break;
default:
ToFlightCtrl.Param.Byte[0] = 255;
hott_index = 0;
break;
}
break;
default:
break;
// 0 = 0,1
380,7 → 468,6
}
VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt
 
 
switch(FromFlightCtrl.Command)
{
case SPI_FCCMD_USER:
408,21 → 495,24
{
FCCalibActive = 0;
}
if(FC.StatusFlags & FC_STATUS_START)
{ if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else HeadFreeStartAngle = FromFlightCtrl.GyroHeading; }
if(FC.StatusFlags & FC_STATUS_START)
{
if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else
HeadFreeStartAngle = FromFlightCtrl.GyroHeading;
}
 
if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
{
if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
{
if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m
{
HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1°
}
else // Ansonsten die aktuelle Richtung übernehmen
HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1°
}
}
if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE))
{
if(!(FC.StatusFlags2 & FC_STATUS2_CAREFREE)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
{
if((NaviData.HomePositionDeviation.Distance > 200) && (NCFlags & NC_FLAG_GPS_OK)) // nur bei ausreichender Distance -> 20m
{
HeadFreeStartAngle = (10 * NaviData.HomePositionDeviation.Bearing + 1800 + 3600 - Parameter.OrientationAngle * 150) % 3600; // in 0.1°
}
else // Ansonsten die aktuelle Richtung übernehmen
HeadFreeStartAngle = (3600 + FromFlightCtrl.GyroHeading /*+ Parameter.OrientationAngle * 150*/) % 3600; // in 0.1°
}
}
 
//DebugOut.Analog[16] = HeadFreeStartAngle;
 
596,6 → 686,7
}while (!CheckDelay(timeout));
UART1_PutString(".");
repeat++;
FCCalibActive = 1;
}while((FC_Version.Major == 0xFF) && (repeat < 40)); // 40*250ms = 10s
// if we got it
if (FC_Version.Major != 0xFF)
/trunk/spi_slave.h
56,7 → 56,12
#define SPI_NCCMD_KALMAN 103
#define SPI_NCCMD_VERSION 104
#define SPI_NCCMD_GPSINFO 105
#define SPI_NCCMD_HOTT_INFO 106
 
#define HOTT_VARIO_PACKET_ID 0x89
#define HOTT_GPS_PACKET_ID 0x8A
#define HOTT_ELECTRIC_AIR_PACKET_ID 0x8E
 
typedef struct
{
u8 Command;
/trunk/uart1.c
124,8 → 124,6
u8 UART1_tbuffer[UART1_TX_BUFFER_LEN];
Buffer_t UART1_tx_buffer;
 
 
 
volatile u8 SerialLinkOkay = 0;
 
u8 text[200];
150,16 → 148,16
"I2C Error ",
"I2C Okay ", //15
"16 ",// "Kalman_K ",
"ACC_Speed_N ",
"ACC_Speed_E ",
"Speed_z ",
"20 ",//20
"N_Speed ",
"E_Speed ",
"23 ",
"17 ",
"18 ",
"20 ",
"23 ",//20
"Magnet X ",
"Magnet Y ",//25
"Magnet Y ",
"Magnet Z ",
"Z_Speed ",
"N_Speed ",//25
"E_Speed ",
"Distance N ",
"Distance E ",
"GPS_Nick ",