// ######################## SPI - FlightCtrl ###################
#include "main.h"
#include "eeprom.h"
#include "uart.h"
//struct str_ToNaviCtrl_Version ToNaviCtrl_Version;
//struct str_FromNaviCtrl_Version FromNaviCtrl_Version;
struct str_ToNaviCtrl ToNaviCtrl
;
struct str_FromNaviCtrl FromNaviCtrl
;
struct str_FromNaviCtrl_Value FromNaviCtrl_Value
;
struct str_SPI_VersionInfo NC_Version
;
struct str_GPSInfo GPSInfo
;
struct str_HugeBlockToNavi HugeBlockToNavi
;
struct str_HugeBlockFromNavi HugeBlockFromNavi
;
unsigned char SPI_BufferIndex
;
unsigned char SPI_RxBufferIndex
;
unsigned char SPI_Datasize
;
signed char FromNC_Rotate_C
= 32, FromNC_Rotate_S
= 0;
signed char FromNC_WP_EventChannel_New
= 0;
volatile unsigned char SPI_Buffer
[sizeof(HugeBlockToNavi
)];
unsigned char *SPI_TX_Buffer
;
unsigned char SPITransferCompleted
, SPI_ChkSum
;
unsigned char SPI_RxDataValid
,NaviDataOkay
= 250;
unsigned char SPI_CommandSequence
[] = { SPI_FCCMD_STICK
, SPI_FCCMD_USER
, SPI_FCCMD_SLOW
,SPI_FCCMD_BL_ACCU
,
SPI_FCCMD_STICK2
, SPI_FCCMD_MISC
, SPI_FCCMD_BL_ACCU
,
SPI_FCCMD_STICK
, SPI_FCCMD_USER
, SPI_FCCMD_BL_ACCU
,
SPI_FCCMD_STICK2
, SPI_FCCMD_PARAMETER2
, SPI_FCCMD_BL_ACCU
};
unsigned char SPI_CommandCounter
= 0;
unsigned char NC_ErrorCode
= 0, Partner_ErrorCode
= 0;
unsigned char NC_GPS_ModeCharacter
= ' ';
unsigned char EarthMagneticField
= 0;
unsigned char EarthMagneticInclination
= 0, EarthMagneticInclinationTheoretic
= 0;
unsigned char NC_To_FC_Flags
= 0;
unsigned char NCForcesNewDirection
= 0; // allows Yawing without CareFree (Yawing at Coming Home)
//unsigned char NC_To_FC_MaxAltitude = 0; // this is a Parameter on the SD-card
signed int POI_KameraNick
= 0; // in 0,1°
vector16_t MagVec
= {0,0,0};
unsigned char *PtrToChksum
;
unsigned char NC_RequestsConfig
= 0;
unsigned char CamCtrlCharacter
= ' ';
unsigned char NCParachute
= 0;
unsigned char BaroCalState
= 0;
unsigned int FromNC_Laser_cm
;
#ifdef USE_SPI_COMMUNICATION
//------------------------------------------------------
void SPI_MasterInit
(void)
{
DDR_SPI
|= (1<<DD_MOSI
)|(1<<DD_SCK
); // Set MOSI and SCK output, all others input
SLAVE_SELECT_DDR_PORT
|= (1 << SPI_SLAVE_SELECT
);
SPCR
= (1<<SPE
)|(1<<MSTR
)|(1<<SPR1
)|(0<<SPR0
)|(0<<SPIE
); // Enable SPI, Master, set clock rate fck/64
SPSR
= 0;//(1<<SPI2X);
SLAVE_SELECT_PORT
|= (1 << SPI_SLAVE_SELECT
);
SPITransferCompleted
= 1;
//SPDR = 0x00; // dummy write
ToNaviCtrl.
Sync1 = SPI_FCSYNCBYTE1
;
ToNaviCtrl.
Sync2 = SPI_FCSYNCBYTE2
;
ToNaviCtrl.
Command = SPI_FCCMD_USER
;
ToNaviCtrl.
IntegralNick = 0;
ToNaviCtrl.
IntegralRoll = 0;
FromNaviCtrl_Value.
SerialDataOkay = 0;
SPI_RxDataValid
= 0;
}
//------------------------------------------------------
unsigned char SPI_StartTransmitPacket
(void)
{
if(!SPITransferCompleted
) return(0);
SLAVE_SELECT_PORT
&= ~
(1 << SPI_SLAVE_SELECT
); // SelectSlave
SPI_TX_Buffer
= (unsigned char *) &ToNaviCtrl
;
PtrToChksum
= (unsigned char *) &ToNaviCtrl.
Chksum;
ToNaviCtrl.
Command = SPI_CommandSequence
[SPI_CommandCounter
++];
if (SPI_CommandCounter
>= sizeof(SPI_CommandSequence
)) SPI_CommandCounter
= 0;
SPITransferCompleted
= 0;
UpdateSPI_Buffer
(); // update buffer
SPI_BufferIndex
= 1;
ToNaviCtrl.
Chksum = ToNaviCtrl.
Sync1;
SPI_Datasize
= sizeof(ToNaviCtrl
);
SPDR
= ToNaviCtrl.
Sync1; // Start transmission
return(1);
}
//------------------------------------------------------
//SIaNAL(SIG_SPI)
void SPI_TransmitByte
(void)
{
static unsigned char SPI_RXState
= 0;
unsigned char rxdata
;
static unsigned char rxchksum
;
if (SPITransferCompleted
) return;
if (!(SPSR
& (1 << SPIF
))) return;
BytegapSPI
= SPI_BYTEGAP
;
// _delay_us(30);
SLAVE_SELECT_PORT
|= (1 << SPI_SLAVE_SELECT
); // DeselectSlave
rxdata
= SPDR
;
switch ( SPI_RXState
)
{
case 0:
SPI_RxBufferIndex
= 0;
rxchksum
= rxdata
;
if (rxdata
== SPI_NCSYNCBYTE1
) { SPI_RXState
= 1; } // 1. Syncbyte ok
break;
case 1:
rxchksum
+= rxdata
;
if (rxdata
== SPI_NCSYNCBYTE2
) { SPI_RXState
= 2; } // 2. Syncbyte ok
else
if (rxdata
== SPI_NCSYNCBYTE_HB2
) { SPI_RXState
= 3; } // 2. Syncbyte ok
else SPI_RXState
= 0;
break;
case 2: // get normal NC data packets
SPI_Buffer
[SPI_RxBufferIndex
++]= rxdata
; // get data
//DebugOut.Analog[19]++;
if (SPI_RxBufferIndex
>= sizeof(FromNaviCtrl
))
{
if (rxdata
== rxchksum
)
{
unsigned char *ptr
= (unsigned char *)&FromNaviCtrl
;
memcpy(ptr
, (unsigned char *) SPI_Buffer
, sizeof(FromNaviCtrl
));
SPI_RxDataValid
= 1;
}
else
{
SPI_RxDataValid
= 0;
DebugOut.
Analog[27]++;
}
SPI_RXState
= 0;
}
else rxchksum
+= rxdata
;
break;
case 3: // get huge data packet
SPI_Buffer
[SPI_RxBufferIndex
++]= rxdata
; // get data
if(SPI_RxBufferIndex
>= sizeof(HugeBlockFromNavi
))
{
if(rxdata
== rxchksum
)
{
unsigned char *ptr
= (unsigned char *)&HugeBlockFromNavi
;
memcpy(ptr
, (unsigned char *) SPI_Buffer
, sizeof(HugeBlockFromNavi
));
// DebugOut.Analog[] = HugeBlockFromNavi.Data[239];
// SPI_RxDataValid = 1;
}
else
{
SPI_RxDataValid
= 0;
DebugOut.
Analog[27]++;
}
SPI_RXState
= 0;
}
else rxchksum
+= rxdata
;
break;
}
if (SPI_BufferIndex
< SPI_Datasize
)
{
unsigned char tmp
;
SLAVE_SELECT_PORT
&= ~
(1 << SPI_SLAVE_SELECT
); // SelectSlave
tmp
= SPI_TX_Buffer
[SPI_BufferIndex
];
SPDR
= tmp
;
*PtrToChksum
+= tmp
;
}
else SPITransferCompleted
= 1;
SPI_BufferIndex
++;
}
void TransmitHugeBlock
(void)
{
// if(!SPITransferCompleted) return(0);
while(!SPITransferCompleted
) { SPI_TransmitByte
();_delay_us
(100);}; // send the remaining data
SLAVE_SELECT_PORT
&= ~
(1 << SPI_SLAVE_SELECT
); // SelectSlave
SPI_TX_Buffer
= (unsigned char *) &HugeBlockToNavi
; // pointer to the huge Block
PtrToChksum
= (unsigned char *) &HugeBlockToNavi.
Chksum;
SPITransferCompleted
= 0;
SPI_BufferIndex
= 1;
HugeBlockToNavi.
Sync1 = SPI_FCSYNCBYTE_HB1
;
HugeBlockToNavi.
Sync2 = SPI_FCSYNCBYTE_HB2
;
HugeBlockToNavi.
Chksum = SPI_FCSYNCBYTE_HB1
;
SPI_Datasize
= sizeof(HugeBlockToNavi
);
SPDR
= HugeBlockToNavi.
Sync1; // Start transmission
// +++++++++++++++++++++++++++++++
// finish I2CTransfer first
// if(I2C_TransferActive)_delay_ms(2);
// +++++++++++++++++++++++++++++++
SPI_TransmitByte
();_delay_us
(200); SPI_TransmitByte
();_delay_us
(200); // leave the NC a bit time to fill the FIFO
while(!SPITransferCompleted
) { _delay_us
(80); SPI_TransmitByte
();};
}
void SendSettingToNC
(void)
{
HugeBlockToNavi.
WhoAmI = 1; // 1 means: EE-Data
EE_Parameter.
crc = RAM_Checksum
((uint8_t*)(&EE_Parameter
), sizeof(EE_Parameter
)-1);
memcpy(HugeBlockToNavi.
Data, (unsigned char *) &EE_Parameter
, sizeof(EE_Parameter
));
TransmitHugeBlock
();
NC_RequestsConfig
= 0;
}
//------------------------------------------------------
void UpdateSPI_Buffer
(void)
{
// signed int tmp;
static unsigned char motorindex
, oldcommand
= SPI_NCCMD_VERSION
, slow_command
= 0;
ToNaviCtrl.
IntegralNick = (int) (IntegralNick
/ (long)(EE_Parameter.
GyroAccFaktor * 4)); // etwa in 0.1 Grad
ToNaviCtrl.
IntegralRoll = (int) (IntegralRoll
/ (long)(EE_Parameter.
GyroAccFaktor * 4)); // etwa in 0.1 Grad
ToNaviCtrl.
GyroCompass = (10 * ErsatzKompass
) / GIER_GRAD_FAKTOR
;
ToNaviCtrl.
GyroGier = (signed int) AdNeutralGier
- AdWertGier
;
NaviCntAcc
= 0; NaviAccNick
= 0; NaviAccRoll
= 0;
// ToNaviCtrl.User8 = Parameter_UserParam8;
// ToNaviCtrl.CalState = WinkelOut.CalcState;
switch(ToNaviCtrl.
Command) //
{
case SPI_FCCMD_USER
:
ToNaviCtrl.
Param.
Byte[0] = Parameter_UserParam1
;
ToNaviCtrl.
Param.
Byte[1] = Parameter_UserParam2
;
ToNaviCtrl.
Param.
Byte[2] = Parameter_UserParam3
;
ToNaviCtrl.
Param.
Byte[3] = Parameter_UserParam4
;
ToNaviCtrl.
Param.
Byte[4] = Parameter_UserParam5
;
ToNaviCtrl.
Param.
Byte[5] = Parameter_UserParam6
;
ToNaviCtrl.
Param.
Byte[6] = Parameter_UserParam7
;
ToNaviCtrl.
Param.
Byte[7] = Parameter_UserParam8
;
ToNaviCtrl.
Param.
Byte[8] = FC_StatusFlags
;
ToNaviCtrl.
Param.
Byte[9] = FC_StatusFlags2
;
//if(FC_StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF) ToNaviCtrl.Param.Byte[8] &= ~FC_STATUS_FLY;
FC_StatusFlags
&= ~
(FC_STATUS_CALIBRATE
| FC_STATUS_START
);
ToNaviCtrl.
Param.
Int[5] = UBat
; // 10 & 11
if(!(PORTC
& (1<<PORTC2
))) FC_StatusFlags2
&= ~FC_STATUS2_OUT1_ACTIVE
; // Out1 (J16)
// 8 = 16,17
// 9 = 18,19
break;
case SPI_FCCMD_BL_ACCU
:
ToNaviCtrl.
Param.
Int[0] = Capacity.
ActualCurrent; // 0.1A
ToNaviCtrl.
Param.
Byte[2] = motorindex
| Out1ChangedFlag
; // 0x80 this Flag marks a changed Out1
ToNaviCtrl.
Param.
Byte[3] = Capacity.
MinOfMaxPWM;
ToNaviCtrl.
Param.
Byte[4] = Parameter_GPS_Switch
; // GPS-Mode control
ToNaviCtrl.
Param.
Byte[5] = VarioCharacter
;
ToNaviCtrl.
Param.
Byte[6] = Motor
[motorindex
].
NotReadyCnt;
ToNaviCtrl.
Param.
Byte[7] = Motor
[motorindex
].
Version;
ToNaviCtrl.
Param.
Byte[8] = Motor
[motorindex
].
MaxPWM;
ToNaviCtrl.
Param.
Byte[9] = Motor
[motorindex
].
State;
ToNaviCtrl.
Param.
Byte[10] = Motor
[motorindex
].
Temperature;
ToNaviCtrl.
Param.
Byte[11] = Motor
[motorindex
].
Current;
if(Mixer.
Motor[++motorindex
][0] <= 0) // next motor is not used ?
while(Mixer.
Motor[motorindex
][0] <= 0 && motorindex
) motorindex
= (motorindex
+ 1) % 13;
Out1ChangedFlag
= 0;
ToNaviCtrl.
Param.
sInt[6] = Mittelwert_AccNick
/4; // ToNaviCtrl.AccNick
ToNaviCtrl.
Param.
sInt[7] = Mittelwert_AccRoll
/4; // ToNaviCtrl.AccRoll
//ToNaviCtrl.Param.sInt[8] = DebugOut.Analog[16];
//ToNaviCtrl.Param.sInt[9] = DebugOut.Analog[17];
// 8 = 16,17
// 9 = 18,19
break;
case SPI_FCCMD_SLOW
:
switch(slow_command
)
{
case 0:
ToNaviCtrl.
Command = SPI_FCCMD_VERSION
;
ToNaviCtrl.
Param.
Byte[0] = VERSION_MAJOR
;
ToNaviCtrl.
Param.
Byte[1] = VERSION_MINOR
;
ToNaviCtrl.
Param.
Byte[2] = VERSION_PATCH
;
ToNaviCtrl.
Param.
Byte[3] = NC_SPI_COMPATIBLE
;
ToNaviCtrl.
Param.
Byte[4] = PlatinenVersion
;
ToNaviCtrl.
Param.
Byte[5] = EE_Parameter.
LandingSpeed;
ToNaviCtrl.
Param.
Byte[6] = EE_Parameter.
ComingHomeAltitude;
ToNaviCtrl.
Param.
Byte[7] = EE_Parameter.
AutoPhotoAtitudes;
ToNaviCtrl.
Param.
Byte[8] = VersionInfo.
BL_Firmware;
ToNaviCtrl.
Param.
Byte[9] = ActiveParamSet
;
ToNaviCtrl.
Param.
Int[5] = FlugMinutenGesamt
; // 10 & 11
ToNaviCtrl.
Param.
Byte[12] = IamMaster
;
slow_command
++;
break;
case 1:
ToNaviCtrl.
Command = SPI_FCCMD_PARAMETER1
;
ToNaviCtrl.
Param.
Byte[0] = (unsigned char) BattLowVoltageWarning
; //0.1V
ToNaviCtrl.
Param.
Byte[1] = EE_Parameter.
NaviGpsGain; // Parameters for the Naviboard
ToNaviCtrl.
Param.
Byte[2] = EE_Parameter.
NaviGpsP;
ToNaviCtrl.
Param.
Byte[3] = EE_Parameter.
NaviGpsI;
ToNaviCtrl.
Param.
Byte[4] = EE_Parameter.
NaviGpsD;
ToNaviCtrl.
Param.
Byte[5] = EE_Parameter.
NaviGpsA;
ToNaviCtrl.
Param.
Byte[6] = EE_Parameter.
NaviGpsMinSat;
ToNaviCtrl.
Param.
Byte[7] = EE_Parameter.
NaviStickThreshold;
ToNaviCtrl.
Param.
Byte[8] = EE_Parameter.
NaviMaxFlyingRange;
ToNaviCtrl.
Param.
Byte[9] = EE_Parameter.
NaviWindCorrection;
ToNaviCtrl.
Param.
Byte[10] = EE_Parameter.
NaviAccCompensation;
ToNaviCtrl.
Param.
Byte[11] = EE_Parameter.
NaviAngleLimitation;
slow_command
++;
break;
case 2:
ToNaviCtrl.
Command = SPI_FCCMD_SLOW2
;
ToNaviCtrl.
Param.
Int[0] = BoatNeutralNick
; // 0 & 1
ToNaviCtrl.
Param.
Int[1] = BoatNeutralRoll
; // 2 & 3
ToNaviCtrl.
Param.
Int[2] = BoatNeutralGier
; // 4 & 5
ToNaviCtrl.
Param.
Byte[6] = EE_Parameter.
CamOrientation;
ToNaviCtrl.
Param.
Byte[7] = EE_Parameter.
CompassOffset;
ToNaviCtrl.
Param.
Byte[8] = Parameter_GlobalConfig
;
ToNaviCtrl.
Param.
Byte[9] = Parameter_ExtraConfig
;
ToNaviCtrl.
Param.
Byte[10] = EE_Parameter.
OrientationAngle;
ToNaviCtrl.
Param.
Byte[11] = EE_Parameter.
GlobalConfig3;
slow_command
++;
break;
case 3:
ToNaviCtrl.
Command = SPI_FCCMD_SLOW3
;
ToNaviCtrl.
Param.
Byte[0] = Parameter_ServoNickControl
;
ToNaviCtrl.
Param.
Byte[1] = Parameter_ServoRollControl
;
ToNaviCtrl.
Param.
Byte[2] = EE_Parameter.
NaviDescendRange; // in 10m
ToNaviCtrl.
Param.
Byte[3] = Parameter_MaximumAltitude
;
ToNaviCtrl.
Param.
Byte[4] = EE_Parameter.
ServoCompInvert;
ToNaviCtrl.
Param.
Byte[5] = LipoCells
;
ToNaviCtrl.
Param.
Int[3] = ShutterCounter
; // 6 & 7
ToNaviCtrl.
Param.
Byte[8] = LowVoltageLandingActive
;
ToNaviCtrl.
Param.
Byte[9] = EE_Parameter.
FailSafeTime;
ToNaviCtrl.
Param.
Byte[10] = 0;
ToNaviCtrl.
Param.
Byte[11] = 0;
//ToNaviCtrl.Param.Byte[12]
//ToNaviCtrl.Param.Byte[13]
//ToNaviCtrl.Param.Byte[14]
//ToNaviCtrl.Param.Byte[15]
//ToNaviCtrl.Param.Byte[16]
//ToNaviCtrl.Param.Byte[17]
//ToNaviCtrl.Param.Byte[18]
//ToNaviCtrl.Param.Byte[19]
slow_command
++;
break;
default:
ToNaviCtrl.
Command = SPI_FCCMD_NEUTRAL
;
ToNaviCtrl.
Param.
Int[0] = AdNeutralNick
; // 0 & 1
ToNaviCtrl.
Param.
Int[1] = AdNeutralRoll
; // 2 & 3
ToNaviCtrl.
Param.
Int[2] = AdNeutralGier
; // 4 & 5
ToNaviCtrl.
Param.
Byte[6] = EE_Parameter.
Driftkomp;
ToNaviCtrl.
Param.
Byte[7] = EE_Parameter.
NaviPH_LoginTime;
ToNaviCtrl.
Param.
Byte[8] = EE_Parameter.
Receiver;
ToNaviCtrl.
Param.
Byte[9] = EE_Parameter.
NaviGpsPLimit;
ToNaviCtrl.
Param.
Byte[10] = EE_Parameter.
NaviGpsILimit;
ToNaviCtrl.
Param.
Byte[11] = EE_Parameter.
NaviGpsDLimit;
slow_command
= 0;
//ToNaviCtrl.Param.Byte[12]
//ToNaviCtrl.Param.Byte[13]
//ToNaviCtrl.Param.Byte[14]
//ToNaviCtrl.Param.Byte[15]
//ToNaviCtrl.Param.Byte[16]
//ToNaviCtrl.Param.Byte[17]
//ToNaviCtrl.Param.Byte[18]
//ToNaviCtrl.Param.Byte[19]
break;
}
break;
case SPI_FCCMD_PARAMETER2
:
ToNaviCtrl.
Param.
Byte[0] = EE_Parameter.
AutoPhotoDistance; // Distance between Photo releases
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
// create the ToNC_SpeakHoTT
if(EE_Parameter.
Receiver != RECEIVER_HOTT
)
{
if(JetiBeep
!= 'B') JetiBeep
= pgm_read_byte
(&JETI_CODE
[HoTT_Waring
()]);
else HoTT_Waring
();
}
ToNaviCtrl.
Param.
Byte[1] = ToNC_SpeakHoTT
;
#else
ToNaviCtrl.
Param.
Byte[1] = 0;
#endif
ToNaviCtrl.
Param.
Int[1] = Capacity.
UsedCapacity; // mAh // 2 & 3
ToNaviCtrl.
Param.
Byte[4] = LowVoltageHomeActive
;
ToNaviCtrl.
Param.
Byte[5] = ToNC_Load_WP_List
;
ToNaviCtrl.
Param.
Byte[6] = ToNC_Load_SingePoint
;
ToNaviCtrl.
Param.
Byte[7] = ToNC_Store_SingePoint
;
ToNC_Load_WP_List
= 0;
ToNC_Load_SingePoint
= 0;
ToNC_Store_SingePoint
= 0;
if(Parameter_KompassWirkung
) ToNaviCtrl.
Param.
sInt[4] = KompassSollWert
; // Pos. 8 & 9
else ToNaviCtrl.
Param.
sInt[4] = ErsatzKompassInGrad
; // answer with the compass value if the Compass effect is zero
ToNaviCtrl.
Param.
Byte[10] = FC_StatusFlags3
;
ToNaviCtrl.
Param.
Byte[11] = EE_Parameter.
SingleWpSpeed;
//ToNaviCtrl.Param.Byte[12]
//ToNaviCtrl.Param.Byte[13]
//ToNaviCtrl.Param.Byte[14]
//ToNaviCtrl.Param.Byte[15]
//ToNaviCtrl.Param.Byte[16]
//ToNaviCtrl.Param.Byte[17]
//ToNaviCtrl.Param.Byte[18]
//ToNaviCtrl.Param.Byte[19]
break;
case SPI_FCCMD_STICK
:
ToNaviCtrl.
Param.
Byte[0] = ChannelGas
;
ToNaviCtrl.
Param.
Byte[1] = ChannelYaw
;
ToNaviCtrl.
Param.
Byte[2] = ChannelRoll
;
ToNaviCtrl.
Param.
Byte[3] = ChannelNick
;
ToNaviCtrl.
Param.
Byte[4] = (unsigned char) Poti
[0];
ToNaviCtrl.
Param.
Byte[5] = (unsigned char) Poti
[1];
ToNaviCtrl.
Param.
Byte[6] = (unsigned char) Poti
[2];
ToNaviCtrl.
Param.
Byte[7] = (unsigned char) Poti
[3];
ToNaviCtrl.
Param.
Byte[8] = (unsigned char) Poti
[4];
ToNaviCtrl.
Param.
Byte[9] = (unsigned char) Poti
[5];
ToNaviCtrl.
Param.
Byte[10] = (unsigned char) Poti
[6];
ToNaviCtrl.
Param.
Byte[11] = (unsigned char) Poti
[7];
//ToNaviCtrl.Param.Byte[12]
//ToNaviCtrl.Param.Byte[13]
//ToNaviCtrl.Param.Byte[14]
//ToNaviCtrl.Param.Byte[15]
//ToNaviCtrl.Param.Byte[16]
//ToNaviCtrl.Param.Byte[17]
//ToNaviCtrl.Param.Byte[18]
//ToNaviCtrl.Param.Byte[19]
break;
case SPI_FCCMD_STICK2
:
ToNaviCtrl.
Param.
Byte[0] = ChannelGas
;
ToNaviCtrl.
Param.
Byte[1] = ChannelYaw
;
ToNaviCtrl.
Param.
Byte[2] = ChannelRoll
;
ToNaviCtrl.
Param.
Byte[3] = ChannelNick
;
ToNaviCtrl.
Param.
Byte[4] = (unsigned char) PPM_in
[1];
ToNaviCtrl.
Param.
Byte[5] = (unsigned char) PPM_in
[2];
ToNaviCtrl.
Param.
Byte[6] = (unsigned char) PPM_in
[3];
ToNaviCtrl.
Param.
Byte[7] = (unsigned char) PPM_in
[4];
ToNaviCtrl.
Param.
Byte[8] = (unsigned char) PPM_in
[5];
ToNaviCtrl.
Param.
Byte[9] = (unsigned char) PPM_in
[6];
ToNaviCtrl.
Param.
Byte[10] = (unsigned char) PPM_in
[7];
ToNaviCtrl.
Param.
Byte[11] = (unsigned char) PPM_in
[8];
ToNaviCtrl.
Param.
Byte[12] = (unsigned char) PPM_in
[9];
ToNaviCtrl.
Param.
Byte[13] = (unsigned char) PPM_in
[10];
ToNaviCtrl.
Param.
Byte[14] = (unsigned char) PPM_in
[11];
ToNaviCtrl.
Param.
Byte[15] = (unsigned char) PPM_in
[12];
ToNaviCtrl.
Param.
Byte[16] = (unsigned char) PPM_in
[13];
ToNaviCtrl.
Param.
Byte[17] = (unsigned char) PPM_in
[14];
ToNaviCtrl.
Param.
Byte[18] = (unsigned char) PPM_in
[15];
ToNaviCtrl.
Param.
Byte[19] = (unsigned char) PPM_in
[16];
break;
case SPI_FCCMD_MISC
:
if(WinkelOut.
CalcState >= 5)
{
WinkelOut.
CalcState = 0;
ToNaviCtrl.
Param.
Byte[0] = 5;
}
else ToNaviCtrl.
Param.
Byte[0] = WinkelOut.
CalcState;
ToNaviCtrl.
Param.
Byte[1] = HoverGas
/ 4;
ToNaviCtrl.
Param.
Int[1] = (int)(HoehenWert
/5); //2 & 3
ToNaviCtrl.
Param.
Int[2] = (int)(SollHoehe
/5); //4 & 5
ToNaviCtrl.
Param.
Byte[6] = VersionInfo.
HardwareError[0];
ToNaviCtrl.
Param.
Byte[7] = VersionInfo.
HardwareError[1];
VersionInfo.
HardwareError[0] &= ~FC_ERROR0_CAREFREE
; // VersionInfo.HardwareError[0] = 0;
VersionInfo.
HardwareError[1] &= (FC_ERROR1_ACC_NOT_CAL
| FC_ERROR1_MIXER
); // delete the most error-flags now
ToNaviCtrl.
Param.
Byte[8] = DebugOut.
Analog[28]; // I2C-Error counter
ToNaviCtrl.
Param.
Byte[9] = (unsigned char) ReceiverOkay
;
ToNaviCtrl.
Param.
Byte[10] = NC_Wait_for_LED
;
ToNaviCtrl.
Param.
Byte[11] = DebugOut.
Analog[7] / 4; //GasMischanteil
//ToNaviCtrl.Param.Byte[12]
//ToNaviCtrl.Param.Byte[13]
//ToNaviCtrl.Param.Byte[14]
//ToNaviCtrl.Param.Byte[15]
//ToNaviCtrl.Param.Byte[16]
//ToNaviCtrl.Param.Byte[17]
//ToNaviCtrl.Param.Byte[18]
//ToNaviCtrl.Param.Byte[19]
break;
}
if(SPI_RxDataValid
)
{
if(FromNaviCtrl.
Command != oldcommand
) NaviDataOkay
= 250;
oldcommand
= FromNaviCtrl.
Command;
CalculateCompassTimer
= 1;
if(abs(FromNaviCtrl.
GPS_Nick) < 512 && abs(FromNaviCtrl.
GPS_Roll) < 512 && (EE_Parameter.
GlobalConfig & CFG_GPS_AKTIV
))
{
GPS_Nick
= FromNaviCtrl.
GPS_Nick;
GPS_Roll
= FromNaviCtrl.
GPS_Roll;
}
if(FromNaviCtrl.
CompassValue <= 360) KompassValue
= FromNaviCtrl.
CompassValue;
switch (FromNaviCtrl.
Command)
{
case SPI_NCCMD_KALMAN
:
#define KM_BIT_YAW 0x01
#define KM_BIT_UART 0x02
#define KM_BIT_SLOW 0x04 // Fast switch off
#define KM_BIT_OFF 0x08 // Fast switch off
#define KM_BIT_EXTCNTRL 0x10
FromNaviCtrl_Value.
Kalman_K = FromNaviCtrl.
Param.
sByte[0];
FromNaviCtrl_Value.
Kalman_MaxFusion = FromNaviCtrl.
Param.
sByte[1];
FromNaviCtrl_Value.
Kalman_MaxDrift = FromNaviCtrl.
Param.
sByte[2];
KompassFusion
= FromNaviCtrl.
Param.
sByte[3];
FromNC_Rotate_C
= FromNaviCtrl.
Param.
Byte[5];
FromNC_Rotate_S
= FromNaviCtrl.
Param.
Byte[6];
GPS_Aid_StickMultiplikator
= FromNaviCtrl.
Param.
Byte[7];
if(FromNaviCtrl.
Param.
sInt[4] >= 0)
{
NC_CompassSetpoint
= FromNaviCtrl.
Param.
sInt[4]; // bei Carefree kann NC den Kompass-Sollwinkel vorgeben
}
POI_KameraNick
= (POI_KameraNick
+ FromNaviCtrl.
Param.
sInt[5]) / 2; // FromNaviCtrl.Param.sInt[5]; // Nickwinkel
//++++++++++++++++++++++++++++++++++++++++++++
if(FromNaviCtrl.
Param.
Byte[12] && !beeptime
&& !DisableRcOffBeeping
) beeptime
= FromNaviCtrl.
Param.
Byte[12] * 16;
//++++++++++++++++++++++++++++++++++++++++++++
if(FromNaviCtrl.
Param.
Byte[4] & KM_BIT_YAW
) NCForcesNewDirection
= 1;
NCParachute
= FromNaviCtrl.
Param.
Byte[4] & (KM_BIT_SLOW
| KM_BIT_OFF
);
if(FromNaviCtrl.
Param.
Byte[4] & KM_BIT_UART
) // controls the Uart-Multiplexer on the FC3.0
{
#ifdef REDUNDANT_FC_SLAVE
if(MotorenEin
) // otherwise we wouldn't read the answer of the BLs if debugging on FC is active
{
UART_MUX_TO_BL
;
SwitchMultiplexerToUpdate
= 0;
}
else
#endif
{
if(UebertragungAbgeschlossen
== 2) UART_MUX_TO_UPDATE
;
SwitchMultiplexerToUpdate
= 1;
}
}
else
{
UART_MUX_TO_BL
;
SwitchMultiplexerToUpdate
= 0;
}
if(FromNaviCtrl.
Param.
Byte[4] & KM_BIT_EXTCNTRL
) // External Control
{
memcpy((unsigned char *)&ExternalControl
, (unsigned char *)&FromNaviCtrl.
Param.
Byte[13], 7); // 7 Bytes ExternalControl
if(Parameter_ExternalControl
< 128 || (!ExternalControl.
Config & EC_VALID
)) ExternalControl.
Config = 0;
else ExternalControlTimeout
= 100; // 2 seconds timeout
}
//++++++++++++++++++++++++++++++++++++++++++++
break;
case SPI_NCCMD_VERSION
:
NC_Version.
Major = FromNaviCtrl.
Param.
Byte[0];
NC_Version.
Minor = FromNaviCtrl.
Param.
Byte[1];
NC_Version.
Patch = FromNaviCtrl.
Param.
Byte[2];
NC_Version.
Compatible = FromNaviCtrl.
Param.
Byte[3];
NC_Version.
Hardware = FromNaviCtrl.
Param.
Byte[4];
DebugOut.
Status[0] |= FromNaviCtrl.
Param.
Byte[5];
DebugOut.
Status[1] = (DebugOut.
Status[1] & (0x01|0x02)) | (FromNaviCtrl.
Param.
Byte[6] & (0x04 | 0x08));
NC_ErrorCode
= FromNaviCtrl.
Param.
Byte[7];
NC_GPS_ModeCharacter
= FromNaviCtrl.
Param.
Byte[8];
FromNaviCtrl_Value.
SerialDataOkay = FromNaviCtrl.
Param.
Byte[9];
NC_To_FC_Flags
= FromNaviCtrl.
Param.
Byte[10];
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
if(FromNaviCtrl.
Param.
Byte[11])
if(!SpeakHoTT
|| (SpeakHoTT
>= SPEAK_GPS_HOLD
&& SpeakHoTT
<= SPEAK_GPS_OFF
)) SpeakHoTT
= FromNaviCtrl.
Param.
Byte[11];
#endif
if(FromNaviCtrl.
Param.
Byte[12]) NC_RequestsConfig
= FromNaviCtrl.
Param.
Byte[12];
Partner_ErrorCode
= FromNaviCtrl.
Param.
Byte[13];
Partner_StatusFlags
= FromNaviCtrl.
Param.
Byte[14];
Partner_StatusFlags2
= FromNaviCtrl.
Param.
Byte[15];
Partner_StatusFlags3
= FromNaviCtrl.
Param.
Byte[16];
break;
case SPI_NCCMD_GPSINFO
:
GPSInfo.
Flags = FromNaviCtrl.
Param.
Byte[0];
GPSInfo.
NumOfSats = FromNaviCtrl.
Param.
Byte[1];
GPSInfo.
SatFix = FromNaviCtrl.
Param.
Byte[2];
GPSInfo.
Speed = FromNaviCtrl.
Param.
Byte[3];
GPSInfo.
HomeDistance = FromNaviCtrl.
Param.
Int[2];
GPSInfo.
HomeBearing = FromNaviCtrl.
Param.
sInt[3];
if(!FromNC_WP_EventChannel_New
) FromNC_WP_EventChannel_New
= (unsigned char) FromNaviCtrl.
Param.
Byte[8] + 127; // zwischenspeichern, damit keiner verpasst wird
PPM_in
[WP_EVENT_PPM_IN
] = (signed char) FromNaviCtrl.
Param.
Byte[8]; // WP_EVENT-Channel-Value (FromNC_WP_EventChannel)
FromNC_AltitudeSpeed
= FromNaviCtrl.
Param.
Byte[9];
FromNC_AltitudeSetpoint
= (long) FromNaviCtrl.
Param.
sInt[5] * 10; // in cm
break;
case SPI_MISC
:
EarthMagneticField
= FromNaviCtrl.
Param.
Byte[0];
EarthMagneticInclination
= FromNaviCtrl.
Param.
Byte[1];
EarthMagneticInclinationTheoretic
= FromNaviCtrl.
Param.
Byte[2];
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
NaviData_TargetDistance
= FromNaviCtrl.
Param.
Int[3];
#endif
NaviData_WaypointIndex
= FromNaviCtrl.
Param.
Byte[4];
NaviData_WaypointNumber
= FromNaviCtrl.
Param.
Byte[5];
NaviData_TargetHoldTime
= FromNaviCtrl.
Param.
Byte[8];
NaviData_MaxWpListIndex
= FromNaviCtrl.
Param.
Byte[9];
CompassCorrected
= FromNaviCtrl.
Param.
sInt[5]; // Bytes 10 & 11
CamCtrlCharacter
= FromNaviCtrl.
Param.
Byte[12];
BaroCalState
= FromNaviCtrl.
Param.
Byte[13];
LuftdruckTemperaturOffset
= FromNaviCtrl.
Param.
sInt[7]; // Bytes 14 & 15
FromNC_Laser_cm
= FromNaviCtrl.
Param.
sInt[8]; // Bytes 16 & 17
break;
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
case SPI_NCCMD_HOTT_DATA
:
//if(EE_Parameter.Receiver == RECEIVER_HOTT)
NC_Fills_HoTT_Telemety
();
break;
case SPI_SERIAL_CH
: // kommt nur, wenn NC den Datensatz empfangen hat
{
unsigned char tempchar1
;
for(tempchar1
= 0; tempchar1
< 12; tempchar1
++) PPM_in
[SERIAL_POTI_START
+ tempchar1
] = (signed char) FromNaviCtrl.
Param.
Byte[tempchar1
];
SerialChannelDataOkay
= 30;
}
break;
case SPI_OEM_NAME
:
{
unsigned char i
;
for(i
=0;i
<16;i
++) OEM_String
[i
] = FromNaviCtrl.
Param.
Byte[i
];
}
break;
#endif
// 0 = 0,1
// 1 = 2,3
// 2 = 4,5
// 3 = 6,7
// 4 = 8,9
// 5 = 10,11
// 6 = 12,13
// 7 = 14,15
// 8 = 16,17
// 9 = 18,19
default:
break;
}
}
else
{
// KompassValue = 0;
// KompassRichtung = 0;
GPS_Nick
= 0;
GPS_Roll
= 0;
}
}
#endif