/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()) |
{ |
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,9 → 369,96 |
// 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 |
// 1 = 2,3 |
// 2 = 4,5 |
380,7 → 468,6 |
} |
VIC_ITCmd(SSP0_ITLine, ENABLE); // enable SPI interrupt |
switch(FromFlightCtrl.Command) |
{ |
case SPI_FCCMD_USER: |
409,7 → 496,10 |
FCCalibActive = 0; |
} |
if(FC.StatusFlags & FC_STATUS_START) |
{ if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else HeadFreeStartAngle = FromFlightCtrl.GyroHeading; } |
{ |
if(Compass_Heading != -1) HeadFreeStartAngle = Compass_Heading * 10; else |
HeadFreeStartAngle = FromFlightCtrl.GyroHeading; |
} |
if((Parameter.ExtraConfig & CFG_TEACHABLE_CAREFREE)) |
{ |
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 ", |