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) |