/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software Nutzungsbedingungen (english version: see below)
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
// + des Mitverschuldens offen.
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
// + Hinweis: Informationen über erweiterte Nutzungsrechte (wie z.B. Nutzung für nicht-private Zwecke) sind auf Anfrage per Email an info(@)hisystems.de verfügbar.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Software LICENSING TERMS
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
// + The Licensor grants the customer a non-exclusive license to use the microcontroller firmware of the Flight-Ctrl, Navi-Ctrl, BL-Ctrl, and MK3Mag hardware
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
// + The Software may only be used with the Licensor's products.
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
// + agreement shall be the property of the Licensor.
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
// + features that can be used to identify the program may not be altered or defaced by the customer.
// + The customer shall be responsible for taking reasonable precautions
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
// + #### END OF LICENSING TERMS ####
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <string.h>
#include <math.h>
#include <stdlib.h>
#include "91x_lib.h"
#include "led.h"
#include "gps.h"
#include "uart1.h"
#include "spi_slave.h"
#include "compass.h"
#include "timer1.h"
#include "timer2.h"
#include "config.h"
#include "main.h"
#include "params.h"
#include "settings.h"
#include "triggerlog.h"
#include "camctrl.h"
#define SPI_FCSYNCBYTE1 0xAA
#define SPI_FCSYNCBYTE2 0x85
#define SPI_FCSYNCBYTE_HB1 0xA2 // for the huge Block
#define SPI_FCSYNCBYTE_HB2 0x48 // for the huge Block
#define SPI_NCSYNCBYTE1 0x82
#define SPI_NCSYNCBYTE2 0x55
#define SPI_NCSYNCBYTE_HB2 0xA5 // for the huge Block
//communication packets
FromFlightCtrl_t FromFlightCtrl
;
ToFlightCtrl_t ToFlightCtrl
;
paramset_t EE_Parameter
;
#define SPI0_TIMEOUT 2500 //ms
volatile u32 SPI0_Timeout
= 0;
u8 Logging_FCStatusFlags1
= 0,Logging_FCStatusFlags2
= 0;
u8 SpeakHoTT
= 0;
u32 LoggingGasFilter
= 0, LoggingGasCnt
= 0;
u8 Out1TriggerUpdateBlocked
= 0;
u8 Out1TriggerUpdateNewData
= 0;
u8 CntSpiErrorPerSecond
= 0;
// tx packet buffer
volatile u8 SPI_TxBuffer
[sizeof(HugeBlockFromFC
) + 10];
volatile u8 SPI_TxBufferIndex
= 0;
u8
*Ptr_TxChksum
= NULL
; // pointer to checksum in TxBuffer
u16 SPI_Tx_Datasize
= sizeof(ToFlightCtrl
) + 2;
// rx packet buffer
volatile u8 SPI_RxBuffer
[sizeof(HugeBlockFromFC
)+10];
volatile u8 SPI_RxBufferIndex
= 0;
volatile u8 SPI_RxBuffer_Request
= 0;
#define SPI_COMMAND_INDEX 0
s32 Kalman_K
= 32;
s32 Kalman_MaxDrift
= 5 * 16;
s32 Kalman_MaxFusion
= 64;
s32 Kalman_Kompass
= 32;
s32 ToFcGpsZ
= 0;
u8 CompassCalState
= 0;
u8 RequestConfigFromFC
= 0;
u8 SendOemName
= 0;
u8 SPI_CommandSequence
[] = { SPI_NCCMD_KALMAN
, SPI_NCCMD_GPSINFO
, SPI_SERIAL_CH
,SPI_OEM_NAME
, // Achtung: SPI_SERIAL_CH & SPI_OEM_NAME darf nicht am Ende des Arrays stehen (wird gescipped)
SPI_NCCMD_KALMAN
, SPI_NCCMD_HOTT_INFO
,
SPI_NCCMD_KALMAN
, SPI_MISC
,
SPI_NCCMD_KALMAN
, SPI_NCCMD_VERSION
};
u8 SPI_CommandCounter
= 0;
s32 ToFC_Rotate_C
= 64, ToFC_Rotate_S
= 0;
s32 HeadFreeStartAngle
= 0; // in 0,1°
s32 CompassDirectionAtMotorStart
= 0; // in 0,1°
s16 FC_WP_EventChannel
= 0, LogFC_WP_EventChannel
= 0, FC_WP_EventChannel_Processed
= 0; // gibt einen Schaltkanal an die FC weiter, wenn der Wegpunkt erreicht wurde
u32 ToFC_AltitudeRate
= 0;
s32 ToFC_AltitudeSetpoint_dm
= 0;
u8 FromFC_VarioCharacter
= ' ';
s16 GPS_Aid_StickMultiplikator
= 0;
u8 NC_GPS_ModeCharacter
= ' ';
u8 FCCalibActive
= 0;
u8 FC_is_Calibrated
= 0;
Motor_t Motor
[MAX_MOTORS
];
u8 Motor_Version
[MAX_MOTORS
]; // das kann nicht in die struct, weil der PC die Struktur bekommt
u8 NC_To_FC_Flags
= 0;
u8 BL_MinOfMaxPWM
= 255; // indication if all BL-controllers run on full power
u8 Logging_BL_MinOfMaxPWM
= 255;
u8 ErrorCheck_BL_MinOfMaxPWM
= 255;
u32 FC_I2C_ErrorConter
;
SPI_Version_t FC_Version
;
s16 POI_KameraNick
= 0;
u8 NC_Wait_for_LED
= 0;
s16 GyroCompassCorrected
= 0; // corrected with the magnetic declination
s16 CompassSetpointCorrected
= 0; // The compass setpoint that the FC tries to keep - corrected with the magnetic declination
s16 CompassSetpoint
= 0; // in 0,1°
s16 SimulatedDirection
= 0; // only for flight simulation
u8 AmountOfMotors
= 0;
u16 FlugMinutenGesamt
;
u8 HoverGas
= 0;
u8 LowVoltageLandingActive
= 0;
s8 PPM_In
[MAX_RC_IN
] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
s16 FromFlightCtrl_AccNick
= 0,FromFlightCtrl_AccRoll
= 0,FromFlightCtrl_GyroNick
= 0,FromFlightCtrl_GyroRoll
= 0;
str_HugeBlockFromFC HugeBlockFromFC
;
str_HugeBlockToFC HugeBlockToFC
;
u8 CamCtrlCharacter
=' ';
//--------------------------------------------------------------
void SSP0_IRQHandler
(void)
{
static u8 rxchksum
= 0;
u8 rxdata
;
#define SPI_SYNC1 0
#define SPI_SYNC2 1
#define SPI_DATA 2
#define SPI_SYNC_HB 3
#define SPI_DATA_HB 4
static u8 SPI_State
= SPI_SYNC1
;
//IENABLE;
// clear pending bits
SSP_ClearITPendingBit
(SSP0
, SSP_IT_RxTimeOut
);
SSP_ClearITPendingBit
(SSP0
, SSP_IT_RxFifo
);
// while RxFIFO not empty
while(SSP_GetFlagStatus
(SSP0
, SSP_FLAG_RxFifoNotEmpty
) == SET
)
{
rxdata
= SSP0
->DR
; // catch the received byte
switch (SPI_State
)
{
case SPI_SYNC1
:
SPI_RxBufferIndex
= 0; // reset buffer index
SPI_TxBufferIndex
= 0; // reset buffer index
rxchksum
= rxdata
; // init checksum
if (rxdata
== SPI_FCSYNCBYTE1
)
{ // 1st syncbyte ok
SPI_State
= SPI_SYNC2
; // step to sync2
ToFlightCtrl.
Chksum = 0;
memcpy((u8
*) &(SPI_TxBuffer
[2]), (u8
*) &ToFlightCtrl
, sizeof(ToFlightCtrl
));
SPI_TxBuffer
[0] = SPI_NCSYNCBYTE1
;
SPI_TxBuffer
[1] = SPI_NCSYNCBYTE2
;
SPI_Tx_Datasize
= sizeof(ToFlightCtrl
) + 2; // +2 weil die Syncbytes nicht in dem Block stehen
Ptr_TxChksum
= (u8
*) &(((ToFlightCtrl_t
*) &(SPI_TxBuffer
[2]))->Chksum
);
}
else
if (rxdata
== SPI_FCSYNCBYTE_HB1
)
{ // 1st syncbyte ok
SPI_State
= SPI_SYNC_HB
; // step to syncHB
SPI_RxBuffer
[SPI_RxBufferIndex
++]= rxdata
; // sync1
HugeBlockToFC.
Sync1 = SPI_NCSYNCBYTE1
;
HugeBlockToFC.
Sync2 = SPI_NCSYNCBYTE_HB2
;
HugeBlockToFC.
Chksum = 0;
memcpy((u8
*) &(SPI_TxBuffer
[0]), (u8
*) &HugeBlockToFC
, sizeof(HugeBlockToFC
));
SPI_Tx_Datasize
= sizeof(HugeBlockToFC
);
Ptr_TxChksum
= (u8
*) &(((str_HugeBlockToFC
*) &(SPI_TxBuffer
[0]))->Chksum
);
*Ptr_TxChksum
= 0;
}
break;
case SPI_SYNC2
:
if (rxdata
== SPI_FCSYNCBYTE2
)
{ // 2nd Syncbyte ok
rxchksum
+= rxdata
;
SPI_State
= SPI_DATA
;
} // 2nd Syncbyte does not match
else
{
SPI_State
= SPI_SYNC1
; //jump back to sync1
}
break;
case SPI_SYNC_HB
:
if (rxdata
== SPI_FCSYNCBYTE_HB2
)
{ // 2nd Syncbyte ok
rxchksum
+= rxdata
;
SPI_State
= SPI_DATA_HB
;
SPI_RxBuffer
[SPI_RxBufferIndex
++]= rxdata
; // sync2
} // 2nd Syncbyte does not match
else
{
SPI_State
= SPI_SYNC1
; //jump back to sync1
}
break;
case SPI_DATA
:
SPI_RxBuffer
[SPI_RxBufferIndex
++]= rxdata
; // copy databyte to rx buffer
if(SPI_RxBufferIndex
>= sizeof(ToFlightCtrl
)) // end of packet is reached
{
if (rxdata
== rxchksum
) // verify checksum byte
{
// copy SPI_RxBuffer -> FromFlightCtrl
if(!SPI_RxBuffer_Request
) // block writing to FromFlightCtrl on reading access
{
memcpy((u8
*) &FromFlightCtrl
, (u8
*) SPI_RxBuffer
, sizeof(FromFlightCtrl
));
SPI_RxBuffer_Request
= 1;
}
// reset timeout counter on good packet
SPI0_Timeout
= SetDelay
(SPI0_TIMEOUT
);
// DebugOut.Analog[13]++;
}
else // bad checksum byte
{
DebugOut.
Analog[12]++; // increase SPI chksum error counter
CntSpiErrorPerSecond
++;
}
SPI_State
= SPI_SYNC1
; // reset state
}
else // end of packet not reached
{
rxchksum
+= rxdata
; // update checksum
}
break;
case SPI_DATA_HB
:
SPI_RxBuffer
[SPI_RxBufferIndex
++]= rxdata
; // copy databyte to rx buffer
if(SPI_RxBufferIndex
>= sizeof(HugeBlockFromFC
)) // end of packet is reached
{
if (rxdata
== rxchksum
) // verify checksum byte
{
// copy SPI_RxBuffer -> FromFlightCtrl
memcpy((u8
*) &HugeBlockFromFC
, (u8
*) SPI_RxBuffer
, sizeof(HugeBlockFromFC
));
// reset timeout counter on good packet
SPI0_Timeout
= SetDelay
(SPI0_TIMEOUT
);
}
else // bad checksum byte
{
DebugOut.
Analog[12]++; // increase SPI chksum error counter
CntSpiErrorPerSecond
++;
}
SPI_State
= SPI_SYNC1
; // reset state
}
else // end of packet not reached
{
rxchksum
+= rxdata
; // update checksum
}
break;
default:
SPI_State
= SPI_SYNC1
;
break;
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Ouput Data
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Fill TxFIFO while its not full or end of packet is reached
while((SSP_GetFlagStatus
(SSP0
, SSP_FLAG_TxFifoNotFull
) == SET
) && (SPI_TxBufferIndex
< SPI_Tx_Datasize
))
{
SSP0
->DR
= SPI_TxBuffer
[SPI_TxBufferIndex
]; // send a byte
*Ptr_TxChksum
+= SPI_TxBuffer
[SPI_TxBufferIndex
]; // update checksum
if(SPIWatchDog
== 0) *Ptr_TxChksum
+= 1; // disturbe this packet to stop the communication!
SPI_TxBufferIndex
++; // pointer to next byte
}
}
VIC1
->VAR
= 0xFF; // write any value to VIC0 Vector address register
}
//--------------------------------------------------------------
void SPI0_Init
(void)
{
GPIO_InitTypeDef GPIO_InitStructure
;
SSP_InitTypeDef SSP_InitStructure
;
UART1_PutString
("\r\n SPI init...");
SCU_APBPeriphClockConfig
(__GPIO2
,ENABLE
);
SCU_APBPeriphClockConfig
(__SSP0
,ENABLE
);
GPIO_DeInit
(GPIO2
);
//SSP0_CLK, SSP0_MOSI, SSP0_NSS pins
GPIO_StructInit
(&GPIO_InitStructure
);
GPIO_InitStructure.
GPIO_Direction = GPIO_PinInput
;
GPIO_InitStructure.
GPIO_Pin = GPIO_Pin_4
| GPIO_Pin_5
| GPIO_Pin_7
;
GPIO_InitStructure.
GPIO_Type = GPIO_Type_PushPull
;
GPIO_InitStructure.
GPIO_IPInputConnected = GPIO_IPInputConnected_Enable
;
GPIO_InitStructure.
GPIO_Alternate = GPIO_InputAlt1
; //SSP0_SCLK, SSP0_MOSI, SSP0_NSS
GPIO_Init
(GPIO2
, &GPIO_InitStructure
);
// SSP0_MISO pin GPIO2.6
GPIO_StructInit
(&GPIO_InitStructure
);
GPIO_InitStructure.
GPIO_Direction = GPIO_PinOutput
;
GPIO_InitStructure.
GPIO_Pin = GPIO_Pin_6
;
GPIO_InitStructure.
GPIO_Type = GPIO_Type_PushPull
;
GPIO_InitStructure.
GPIO_IPInputConnected = GPIO_IPInputConnected_Enable
;
GPIO_InitStructure.
GPIO_Alternate = GPIO_OutputAlt2
; //SSP0_MISO
GPIO_Init
(GPIO2
, &GPIO_InitStructure
);
GPIO_WriteBit
(GPIO2
, GPIO_Pin_6
, Bit_RESET
); // switch MISO to low
SSP_DeInit
(SSP0
);
SSP_StructInit
(&SSP_InitStructure
);
SSP_InitStructure.
SSP_FrameFormat = SSP_FrameFormat_Motorola
;
SSP_InitStructure.
SSP_Mode = SSP_Mode_Slave
;
SSP_InitStructure.
SSP_SlaveOutput = SSP_SlaveOutput_Enable
;
SSP_InitStructure.
SSP_CPHA = SSP_CPHA_1Edge
;
SSP_InitStructure.
SSP_CPOL = SSP_CPOL_Low
;
SSP_InitStructure.
SSP_ClockRate = 0;
SSP_Init
(SSP0
, &SSP_InitStructure
);
SSP_ITConfig
(SSP0
, SSP_IT_RxFifo
| SSP_IT_RxTimeOut
, ENABLE
);
SSP_Cmd
(SSP0
, ENABLE
);
// initialize the syncbytes in the tx buffer
SPI_TxBuffer
[0] = SPI_NCSYNCBYTE1
;
SPI_TxBuffer
[1] = SPI_NCSYNCBYTE2
;
// set the pointer to the checksum byte in the tx buffer
Ptr_TxChksum
= (u8
*) &(((ToFlightCtrl_t
*) &(SPI_TxBuffer
[2]))->Chksum
);
ToFlightCtrl.
GPSStick.
Nick = 0;
ToFlightCtrl.
GPSStick.
Roll = 0;
// ToFlightCtrl.GPSStick.Yaw = 0;
VIC_Config
(SSP0_ITLine
, VIC_IRQ
, PRIORITY_SPI0
);
VIC_ITCmd
(SSP0_ITLine
, ENABLE
);
SPI0_Timeout
= SetDelay
(4*SPI0_TIMEOUT
);
EE_Parameter.
Revision = 0;
sprintf(EE_Parameter.
Name,"???%c",0);
UART1_PutString
("ok");
}
//------------------------------------------------------
void SPI0_UpdateBuffer
(void)
{
static u32 timeout
= 0;
static u8 counter
= 50,hott_index
= 0, last_error_code
= 0, enable_injecting
= 0;
static s16 last_wp_event
= 0;
u8 index
;
s16 tmp
;
s32 i1
,i2
;
/*
union
{
unsigned char Byte[4];
unsigned int Int[2];
unsigned long Long;
} Temp;
*/
SPIWatchDog
= 3500; // stop communication to FC after this timeout
if(SPI_RxBuffer_Request
)
{
// avoid sending data via SPI during the update of the ToFlightCtrl structure
VIC_ITCmd
(SSP0_ITLine
, DISABLE
); // disable SPI interrupt
ToFlightCtrl.
CompassHeading = Compass_Heading
;
DebugOut.
Analog[10] = ToFlightCtrl.
CompassHeading;
GyroCompassCorrected
= (3600 + FromFlightCtrl.
GyroHeading + FC.
FromFC_CompassOffset + GeoMagDec
) % 3600;
if(ToFlightCtrl.
CompassHeading >= 0) ToFlightCtrl.
CompassHeading = (360 + ToFlightCtrl.
CompassHeading + FromFlightCtrl.
GyroYaw / 12) % 360;
// ToFlightCtrl.MagVecX = MagVector.X;
// ToFlightCtrl.MagVecY = MagVector.Y;
// ToFlightCtrl.MagVecZ = MagVector.Z;
// ToFlightCtrl.NCStatus = 0;
// ++++++++++++++++++++++++++++++++++++++++++++++++
// ++++++++++++++++++++++++++++++++++++++++++++++++
// cycle spi commands
if(ErrorCode
!= last_error_code
&& enable_injecting
)
{
ToFlightCtrl.
Command = SPI_NCCMD_VERSION
;
last_error_code
= ErrorCode
;
enable_injecting
= 0;
}
else
if(FC_WP_EventChannel
!= last_wp_event
&& enable_injecting
)
{
ToFlightCtrl.
Command = SPI_NCCMD_GPSINFO
;
last_wp_event
= FC_WP_EventChannel
;
enable_injecting
= 0;
}
else
{
ToFlightCtrl.
Command = SPI_CommandSequence
[SPI_CommandCounter
++];
if((ToFlightCtrl.
Command == SPI_SERIAL_CH
) && !NewSerialChannelFrame
) ToFlightCtrl.
Command = SPI_CommandSequence
[SPI_CommandCounter
++];
if((ToFlightCtrl.
Command == SPI_OEM_NAME
) && !SendOemName
) ToFlightCtrl.
Command = SPI_CommandSequence
[SPI_CommandCounter
++];
// restart command cycle at the end
if(SPI_CommandCounter
>= sizeof(SPI_CommandSequence
)) SPI_CommandCounter
= 0;
if(ToFlightCtrl.
Command == SPI_NCCMD_KALMAN
) enable_injecting
= 1;
}
// ++++++++++++++++++++++++++++++++++++++++++++++++
#define FLAG_GPS_AID 0x01
switch (ToFlightCtrl.
Command)
{
case SPI_NCCMD_KALMAN
: // wird am häufigsten betätigt
#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
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
) Kalman_Kompass
;
ToFlightCtrl.
Param.
Byte[4] = 0; // siehe bitcodiert unten
if(DebugUART
== UART2
) ToFlightCtrl.
Param.
Byte[4] = KM_BIT_UART
; // informs the FC to listen to the UART
if(IO1_Function
== IO1FUNC_PARACHUTE
)
{
if(ToFC_Parachute_Off
> 5) ToFlightCtrl.
Param.
Byte[4] |= KM_BIT_SLOW
; // informs the FC to listen to the UART
if(ToFC_Parachute_Off
> 300) ToFlightCtrl.
Param.
Byte[4] |= KM_BIT_OFF
; // informs the FC to listen to the UART
}
ToFlightCtrl.
Param.
Byte[5] = (s8
) ToFC_Rotate_C
;
ToFlightCtrl.
Param.
Byte[6] = (s8
) ToFC_Rotate_S
;
ToFlightCtrl.
Param.
Byte[7] = GPS_Aid_StickMultiplikator
;
if(CAM_Orientation.
UpdateMask & CAM_UPDATE_AZIMUTH
)
{
// if(CAM_Orientation.Azimuth != -1) ToFlightCtrl.Param.sInt[4] = (CAM_Orientation.Azimuth + (3*360) - (FC.FromFC_CompassOffset / 10 + GeoMagDec/10 + Parameter.OrientationAngle * 15)) % 360; // the FC uses the uncorrected comnpass value
if(CAM_Orientation.
Azimuth != -1) ToFlightCtrl.
Param.
sInt[4] = (CAM_Orientation.
Azimuth + (3*360) - (FC.
FromFC_CompassOffset / 10 + GeoMagDec
/10 + Parameter.
CamOrientation * 15)) % 360; // the FC uses the uncorrected comnpass value
else CAM_Orientation.
Azimuth = -1;
if(CAM_Orientation.
UpdateMask & FORCE_AZIMUTH_ROTATION
) ToFlightCtrl.
Param.
Byte[4] |= KM_BIT_YAW
; // allows Yawing without CareFree (Yawing at Coming Home)
CAM_Orientation.
UpdateMask &= ~
(CAM_UPDATE_AZIMUTH
| FORCE_AZIMUTH_ROTATION
);
}
else
{
ToFlightCtrl.
Param.
sInt[4] = -1;
}
if(NCRARAM_STATE_VALID
== NCParams_GetValue
(NCPARAMS_NEW_CAMERA_ELEVATION
, &tmp
)) // Elevation set via 'j' command
{
POI_KameraNick
= tmp
;
}
else
{
//if(FC.StatusFlags2 & FC_STATUS2_CAREFREE) // only, if carefree is active
POI_KameraNick
= CAM_Orientation.
Elevation;
//else ToFlightCtrl.Param.sInt[5] = 0;
}
ToFlightCtrl.
Param.
sInt[5] = POI_KameraNick
;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(BeepTime
> 255 * 16) BeepTime
= 255 * 16;
ToFlightCtrl.
Param.
Byte[12] = BeepTime
/16; // set beeptime
BeepTime
= 0; // reset local beeptime
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
if(NewExternalControlFrame
)
{
NewExternalControlFrame
= 0;
ToFlightCtrl.
Param.
Byte[4] |= KM_BIT_EXTCNTRL
; // Bit: there is External Control in the Data
memcpy((u8
*) &(ToFlightCtrl.
Param.
Byte[13]), (u8
*) &ExternControl
, 7); // 6 Bytes External Control
}
break;
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++
case SPI_SERIAL_CH
:
memcpy((u8
*) &(ToFlightCtrl.
Param.
Byte[0]), (u8
*) &SerialChannel
, 12); // 12 Bytes Serial Channels
NewSerialChannelFrame
= 0;
break;
case SPI_OEM_NAME
:
memcpy((u8
*) &(ToFlightCtrl.
Param.
Byte[0]), (u8
*) &OEM_String
, 16); // 16 Bytes
SendOemName
= 0;
break;
case SPI_NCCMD_VERSION
:
//+++++++++++++++++++++++++++++++++++++++++++++++++++
//+ higher than the maximum allowed altitude
//+++++++++++++++++++++++++++++++++++++++++++++++++++
ToFlightCtrl.
Param.
Byte[0] = VERSION_MAJOR
;
ToFlightCtrl.
Param.
Byte[1] = VERSION_MINOR
;
ToFlightCtrl.
Param.
Byte[2] = VERSION_PATCH
;
ToFlightCtrl.
Param.
Byte[3] = FC_SPI_COMPATIBLE
;
ToFlightCtrl.
Param.
Byte[4] = Version_HW
;
ToFlightCtrl.
Param.
Byte[5] = DebugOut.
StatusGreen;
ToFlightCtrl.
Param.
Byte[6] = DebugOut.
StatusRed;
ToFlightCtrl.
Param.
Byte[7] = ErrorCode
; // muss in SPI_NCCMD_VERSION bleiben! (siehe oben)
ToFlightCtrl.
Param.
Byte[8] = NC_GPS_ModeCharacter
;
ToFlightCtrl.
Param.
Byte[9] = SerialLinkOkay
;
ToFlightCtrl.
Param.
Byte[10] = NC_To_FC_Flags
;
ToFlightCtrl.
Param.
Byte[11] = SpeakHoTT
;
ToFlightCtrl.
Param.
Byte[12] = RequestConfigFromFC
& 0x01;
SpeakHoTT
= 0;
ToFlightCtrl.
Param.
Byte[13] = Partner.
ErrorCode;
ToFlightCtrl.
Param.
Byte[14] = Partner.
StatusFlags;
ToFlightCtrl.
Param.
Byte[15] = Partner.
StatusFlags2;
ToFlightCtrl.
Param.
Byte[16] = Partner.
StatusFlags3;
// if(AbsoluteFlyingAltitude > 255) ToFlightCtrl.Param.Byte[11] = 0; // then the limitation of the FC doesn't work
// else ToFlightCtrl.Param.Byte[11] = AbsoluteFlyingAltitude;
break;
case SPI_MISC
:
ToFlightCtrl.
Param.
Byte[0] = EarthMagneticFieldFiltered
/5;
ToFlightCtrl.
Param.
Byte[1] = EarthMagneticInclination
;
ToFlightCtrl.
Param.
Byte[2] = EarthMagneticInclinationTheoretic
;
ToFlightCtrl.
Param.
Byte[3] = 0;
ToFlightCtrl.
Param.
Byte[4] = NaviData.
WaypointIndex; // index of current waypoints running from 0 to WaypointNumber-1
ToFlightCtrl.
Param.
Byte[5] = NaviData.
WaypointNumber; // number of stored waypoints
ToFlightCtrl.
Param.
Int[3] = NaviData.
TargetPositionDeviation.
Distance_dm / 10;
ToFlightCtrl.
Param.
Byte[8] = NaviData.
TargetHoldTime; // time in s to stay at the given target, counts down to 0 if target has been reached
ToFlightCtrl.
Param.
Byte[9] = ToFC_MaxWpListIndex
;
ToFlightCtrl.
Param.
sInt[5] = GyroCompassCorrected
/ 10; // Bytes 10 & 11
ToFlightCtrl.
Param.
Byte[12] = CamCtrlCharacter
;
ToFlightCtrl.
Param.
Byte[13] = BaroCalState
;
ToFlightCtrl.
Param.
sInt[7] = LuftdruckTemperaturOffset
; // Bytes 14 & 15
ToFlightCtrl.
Param.
Int[8] = FromLaserCtrl.
Distance; // Bytes 16 & 17
ToFlightCtrl.
Param.
Byte[18] = FlyzonePointCnt
;
ToFlightCtrl.
Param.
Byte[19] = 0;
break;
case SPI_NCCMD_GPSINFO
:
ToFlightCtrl.
Param.
Byte[0] = GPSData.
Flags;
ToFlightCtrl.
Param.
Byte[1] = GPSData.
NumOfSats;
ToFlightCtrl.
Param.
Byte[2] = GPSData.
SatFix;
ToFlightCtrl.
Param.
Byte[3] = GPSData.
Speed_Ground / 100; // m/s
ToFlightCtrl.
Param.
Int[2] = NaviData.
HomePositionDeviation.
Distance_dm; // dm //4&5
ToFlightCtrl.
Param.
sInt[3] = NaviData.
HomePositionDeviation.
Bearing; // deg //6&7
if(FC_WP_EventChannel
> 254) FC_WP_EventChannel
= 254; // Muss in SPI_NCCMD_GPSINFO bleiben! (siehe oben)
if(FC_WP_EventChannel
)
{
LogFC_WP_EventChannel
= FC_WP_EventChannel
; // to make sure that it will be logged
NaviData_WP.
WP_Eventchannel = FC_WP_EventChannel
; // to make sure that it will be logged
Out1TriggerUpdateBlocked
= 3; // makes sure that the right trigger-pos is sent in command 18
NaviData_Out1Trigger.
Longitude = NaviData.
CurrentPosition.
Longitude;
NaviData_Out1Trigger.
Latitude = NaviData.
CurrentPosition.
Latitude;
}
FC_WP_EventChannel_Processed
= 1;
//DebugOut.Analog[] = FC_WP_EventChannel;
// ++++++++++++++++++++++++++++++++++
// Waypoint event +++++++++++++++++++
// ++++++++++++++++++++++++++++++++++
if(NCRARAM_STATE_VALID
== NCParams_GetValue
(NCPARAMS_WP_EVENT_ONCE
, &tmp
))
{
ToFlightCtrl.
Param.
Byte[8] = (s8
)(tmp
- 127);
NCParams_ClearValue
(NCPARAMS_WP_EVENT_ONCE
);
NCParams_ClearValue
(NCPARAMS_WP_EVENT_FOREVER
);
}
else
if(NCRARAM_STATE_VALID
== NCParams_GetValue
(NCPARAMS_WP_EVENT_FOREVER
, &tmp
))
{
ToFlightCtrl.
Param.
Byte[8] = (s8
)(tmp
- 127);
if(tmp
== 0) NCParams_ClearValue
(NCPARAMS_WP_EVENT_FOREVER
);
}
else ToFlightCtrl.
Param.
Byte[8] = (s8
)(FC_WP_EventChannel
- 127);
// FC_WP_EventChannel = 0; // the GPS-Routine will set it again
// ++++++++++++++++++++++++++++++++++
if(NCRARAM_STATE_VALID
== NCParams_GetValue
(NCPARAMS_ALTITUDE_RATE
, &tmp
))
{
ToFlightCtrl.
Param.
Byte[9] = (u8
)tmp
;
}
else
{
ToFlightCtrl.
Param.
Byte[9] = (u8
)ToFC_AltitudeRate
;
}
if(NCRARAM_STATE_VALID
== NCParams_GetValue
(NCPARAMS_ALTITUDE_SETPOINT
, &tmp
))
{
ToFlightCtrl.
Param.
sInt[5] = tmp
;
}
else
{
ToFlightCtrl.
Param.
sInt[5] = (s16
)ToFC_AltitudeSetpoint_dm
;
}
//DebugOut.Analog[] = ToFlightCtrl.Param.Byte[8];
if(BlitzSchuhConnected
) ToFlightCtrl.
Param.
sInt[6] = TrigLogging.
CountExternal + 1;
else ToFlightCtrl.
Param.
sInt[6] = 0;
break;
case SPI_NCCMD_HOTT_INFO
:
if(NewWPL_Name
) hott_index
= 100;
switch(hott_index
++)
{
case 0:
//Dezimalgrad --> Grad mit Dezimalminuten --> Grad, Minuten, Sekunden
//53.28 5788 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+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
ToFlightCtrl.
Param.
Byte[1] = 9-1; // how many
//-----------------------------
ToFlightCtrl.
Param.
Byte[2] = GyroCompassCorrected
/ 20;//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[5] = 1; // 1 = S
else ToFlightCtrl.
Param.
Byte[5] = 0; // 1 = S
i1
= abs(GPSData.
Position.
Latitude)/10000000L;
i2
= abs(GPSData.
Position.
Latitude)%10000000L
;
if(!(NCFlags
& NC_FLAG_GPS_OK
)) {i1
= 0; i2
= 0;}
i1
*= 100;
// Minuten
i2
*= 6;
i2
/= 10;
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+3; // index +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
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
;
if(!(NCFlags
& NC_FLAG_GPS_OK
)) {i1
= 0; i2
= 0;}
i1
*= 100;
// Minuten
i2
*= 6;
i2
/= 10;
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_dm / 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+3; // index // +3, weil bei HoTT V4 3 Bytes eingeschoben wurden
ToFlightCtrl.
Param.
Byte[1] = 2; // how many
ToFlightCtrl.
Param.
Byte[2] = EarthMagneticField
/ (5 * 2);
ToFlightCtrl.
Param.
Byte[3] = EarthMagneticInclination
/ 2;
break;
case 3:
ToFlightCtrl.
Param.
Byte[11] = JETI_GPS_PACKET_ID1
;
ToFlightCtrl.
Param.
Byte[0] = 0; // index
ToFlightCtrl.
Param.
Byte[1] = 4; // how many
//JetiExData[14].Value = 53 * 0x10000 + 23467;
if(GPSData.
Position.
Latitude < 0) ToFlightCtrl.
Param.
Byte[5] = 0x40;
else ToFlightCtrl.
Param.
Byte[5] = 0x00;
ToFlightCtrl.
Param.
Byte[4] = abs(GPSData.
Position.
Latitude)/10000000L;
i2
= abs(GPSData.
Position.
Latitude)%10000000L
;
i2
*= 6;
i2
/= 1000;
ToFlightCtrl.
Param.
Byte[3] = i2
/ 256;
ToFlightCtrl.
Param.
Byte[2] = i2
% 256;
break;
case 4:
ToFlightCtrl.
Param.
Byte[11] = JETI_GPS_PACKET_ID2
;
ToFlightCtrl.
Param.
Byte[0] = 0; // index
ToFlightCtrl.
Param.
Byte[1] = 4; // how many
if(GPSData.
Position.
Latitude < 0) ToFlightCtrl.
Param.
Byte[5] = 0x60;
else ToFlightCtrl.
Param.
Byte[5] = 0x20;
ToFlightCtrl.
Param.
Byte[4] = abs(GPSData.
Position.
Longitude)/10000000L;
i2
= abs(GPSData.
Position.
Longitude)%10000000L
;
i2
*= 6;
i2
/= 1000;
ToFlightCtrl.
Param.
Byte[3] = i2
/ 256;
ToFlightCtrl.
Param.
Byte[2] = i2
% 256;
hott_index
= 0;
break;
case 100:
ToFlightCtrl.
Param.
Byte[11] = HOTT_WPL_NAME
;
ToFlightCtrl.
Param.
Byte[0] = 0; // index
ToFlightCtrl.
Param.
Byte[2] = WPL_Store.
Name[0];
ToFlightCtrl.
Param.
Byte[3] = WPL_Store.
Name[1];
ToFlightCtrl.
Param.
Byte[4] = WPL_Store.
Name[2];
ToFlightCtrl.
Param.
Byte[5] = WPL_Store.
Name[3];
ToFlightCtrl.
Param.
Byte[6] = WPL_Store.
Name[4];
ToFlightCtrl.
Param.
Byte[7] = WPL_Store.
Name[5];
ToFlightCtrl.
Param.
Byte[8] = WPL_Store.
Name[6];
ToFlightCtrl.
Param.
Byte[9] = WPL_Store.
Name[7];
ToFlightCtrl.
Param.
Byte[10]= WPL_Store.
Name[8];
ToFlightCtrl.
Param.
Byte[1] = 9; // how many
NewWPL_Name
= 0;
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
// 3 = 6,7
// 4 = 8,9
// 5 = 10,11
// 6 = 12,13
// 7 = 14,15
// 8 = 16,17
// 9 = 18,19
}
VIC_ITCmd
(SSP0_ITLine
, ENABLE
); // enable SPI interrupt
switch(FromFlightCtrl.
Command)
{
case SPI_FCCMD_USER
:
Parameter.
User1 = FromFlightCtrl.
Param.
Byte[0];
Parameter.
User2 = FromFlightCtrl.
Param.
Byte[1];
Parameter.
User3 = FromFlightCtrl.
Param.
Byte[2];
Parameter.
User4 = FromFlightCtrl.
Param.
Byte[3];
Parameter.
User5 = FromFlightCtrl.
Param.
Byte[4];
Parameter.
User6 = FromFlightCtrl.
Param.
Byte[5];
Parameter.
User7 = FromFlightCtrl.
Param.
Byte[6];
Parameter.
User8 = FromFlightCtrl.
Param.
Byte[7];
FC.
RealStatusFlags = FromFlightCtrl.
Param.
Byte[8];
if(FC.
RealStatusFlags & FC_STATUS_MOTOR_RUN
) SimulationFlags
= 0; // stop the simulation if the motors would really start
if(!(SimulationFlags
& SIMULATION_ACTIVE
))
{
if(ClearFCStatusFlags
)
{
FC.
StatusFlags = 0;
ClearFCStatusFlags
= 0;
}
}
FC.
StatusFlags |= FC.
RealStatusFlags;
if(FC.
StatusFlags & FC_STATUS_CALIBRATE
&& !FCCalibActive
)
{
HeadFreeStartAngle
= (3600 + Compass_Heading
* 10 + GeoMagDec
) % 3600;
CompassDirectionAtMotorStart
= HeadFreeStartAngle
;
Compass_Init
();
FCCalibActive
= 15;
FC_is_Calibrated
= 0;
FreqNewGpsData
= 50;
}
else
{
if(FCCalibActive
)
{
if(--FCCalibActive
== 0)
{
FC_is_Calibrated
= 1;
Compass_Check
();
}
}
}
if(FC.
StatusFlags & FC_STATUS_START
)
{
if(Compass_Heading
!= -1) HeadFreeStartAngle
= (3600 + Compass_Heading
* 10 + GeoMagDec
) % 3600;
else HeadFreeStartAngle
= GyroCompassCorrected
;
CompassDirectionAtMotorStart
= HeadFreeStartAngle
;
}
if((Parameter.
ExtraConfig & CFG_TEACHABLE_CAREFREE
))
{
if(!(FC.
StatusFlags2 & FC_STATUS2_CAREFREE
)) // CF ist jetzt ausgeschaltet -> neue Richtung lernen
{
if((NaviData.
HomePositionDeviation.
Distance_dm > 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
= GyroCompassCorrected
; // in 0.1°
}
}
NaviData.
FCStatusFlags = FC.
StatusFlags;
if(FC.
StatusFlags2 & FC_STATUS2_WAIT_FOR_TAKEOFF
) NaviData.
FCStatusFlags &= ~FC_STATUS_FLY
;
FC.
StatusFlags2 = FromFlightCtrl.
Param.
Byte[9];
NaviData.
FCStatusFlags2 = (NaviData.
FCStatusFlags2 & (FC_STATUS2_OUT1_ACTIVE
| FC_STATUS2_OUT2_ACTIVE
)) | (FC.
StatusFlags2 & (0xff - (FC_STATUS2_OUT1_ACTIVE
| FC_STATUS2_OUT2_ACTIVE
)));
if((!(LastTransmittedFCStatusFlags2
& FC_STATUS2_OUT1_ACTIVE
)) && (FC.
StatusFlags2 & FC_STATUS2_OUT1_ACTIVE
)) NaviData.
FCStatusFlags2 |= FC_STATUS2_OUT1_ACTIVE
;
else
if(((LastTransmittedFCStatusFlags2
& FC_STATUS2_OUT1_ACTIVE
)) && !(FC.
StatusFlags2 & FC_STATUS2_OUT1_ACTIVE
)) NaviData.
FCStatusFlags2 &= ~FC_STATUS2_OUT1_ACTIVE
;
if((!(LastTransmittedFCStatusFlags2
& FC_STATUS2_OUT2_ACTIVE
)) && (FC.
StatusFlags2 & FC_STATUS2_OUT2_ACTIVE
)) NaviData.
FCStatusFlags2 |= FC_STATUS2_OUT2_ACTIVE
;
else
if(((LastTransmittedFCStatusFlags2
& FC_STATUS2_OUT2_ACTIVE
)) && !(FC.
StatusFlags2 & FC_STATUS2_OUT2_ACTIVE
)) NaviData.
FCStatusFlags2 &= ~FC_STATUS2_OUT2_ACTIVE
;
//DebugOut.Analog[] = (NaviData.FCStatusFlags2 & FC_STATUS2_OUT1_ACTIVE);
Logging_FCStatusFlags1
|= FC.
StatusFlags;
Logging_FCStatusFlags2
|= FC.
StatusFlags2;
FC.
BAT_Voltage = FromFlightCtrl.
Param.
Int[5]; // 10 & 11
DebugOut.
Analog[7] = FC.
BAT_Voltage;
DebugOut.
Analog[5] = FC.
StatusFlags;
NaviData.
UBat = (u8
) FC.
BAT_Voltage; // Achtung: die (u8) NaviData.UBat kann überlaufen -> das KopterTool müsste dann 25,5V drauf rechnen
break;
case SPI_FCCMD_BL_ACCU
:
FC.
BAT_Current = FromFlightCtrl.
Param.
Int[0];
DebugOut.
Analog[8] = FC.
BAT_Current;
BL_MinOfMaxPWM
= FromFlightCtrl.
Param.
Byte[3];
if(BL_MinOfMaxPWM
< Logging_BL_MinOfMaxPWM
) Logging_BL_MinOfMaxPWM
= BL_MinOfMaxPWM
; // hold the value until logged
if(BL_MinOfMaxPWM
< ErrorCheck_BL_MinOfMaxPWM
) ErrorCheck_BL_MinOfMaxPWM
= BL_MinOfMaxPWM
; // hold the value until Error processed
Parameter.
NaviGpsModeControl = FromFlightCtrl.
Param.
Byte[4];
FromFC_VarioCharacter
= FromFlightCtrl.
Param.
Byte[5];
//0x40
//0x20
//0x10
if(FromFlightCtrl.
Param.
Byte[2] & 0x80) // this Flag marks a changed Out1
{
Out1TriggerUpdateNewData
= 1;
if(!Out1TriggerUpdateBlocked
)
{
NaviData_Out1Trigger.
Longitude = NaviData.
CurrentPosition.
Longitude;
NaviData_Out1Trigger.
Latitude = NaviData.
CurrentPosition.
Latitude;
}
Out1TriggerUpdateBlocked
= 0;
}
if(Out1TriggerUpdateBlocked
) Out1TriggerUpdateBlocked
--;
index
= FromFlightCtrl.
Param.
Byte[2] & 0x0f; //MAX_MOTORS
if(AmountOfMotors
< index
+1) AmountOfMotors
= index
+1;
Motor
[index
].
NotReadyCnt = FromFlightCtrl.
Param.
Byte[6];
Motor_Version
[index
] = FromFlightCtrl.
Param.
Byte[7];
Motor
[index
].
MaxPWM = FromFlightCtrl.
Param.
Byte[8];
Motor
[index
].
State = FromFlightCtrl.
Param.
Byte[9];
Motor
[index
].
Temperature = FromFlightCtrl.
Param.
Byte[10];
Motor
[index
].
Current = FromFlightCtrl.
Param.
Byte[11];
if(FromFC_VarioCharacter
== '+' || FromFC_VarioCharacter
== '-') // manual setpoint clears the NC-Parameter command
{
NCParams_ClearValue
(NCPARAMS_ALTITUDE_RATE
);
}
if(!(SimulationFlags
& SIMULATION_ACTIVE
)) NaviData.
Current = FC.
BAT_Current;
NaviData.
UsedCapacity = FC.
BAT_UsedCapacity;
FromFlightCtrl_AccNick
= FromFlightCtrl.
Param.
sInt[6];
FromFlightCtrl_AccRoll
= FromFlightCtrl.
Param.
sInt[7];
DebugOut.
Analog[2] = FromFlightCtrl_AccNick
;
DebugOut.
Analog[3] = FromFlightCtrl_AccRoll
;
//DebugOut.Analog[] = FromFlightCtrl.Param.sInt[8];
//DebugOut.Analog[] = FromFlightCtrl.Param.sInt[9];
break;
case SPI_FCCMD_PARAMETER1
:
Parameter.
LowVoltageWarning = FromFlightCtrl.
Param.
Byte[0];
CHK_POTI_MM
(Parameter.
NaviGpsGain,FromFlightCtrl.
Param.
Byte[1],0,255);
CHK_POTI_MM
(Parameter.
NaviGpsP,FromFlightCtrl.
Param.
Byte[2],0,255);
CHK_POTI_MM
(Parameter.
NaviGpsI,FromFlightCtrl.
Param.
Byte[3],0,255);
CHK_POTI_MM
(Parameter.
NaviGpsD,FromFlightCtrl.
Param.
Byte[4],0,255);
CHK_POTI_MM
(Parameter.
NaviGpsACC,FromFlightCtrl.
Param.
Byte[5],0,255);
Parameter.
NaviGpsMinSat = FromFlightCtrl.
Param.
Byte[6];
Parameter.
NaviStickThreshold = FromFlightCtrl.
Param.
Byte[7];
// CHK_POTI_MM(Parameter.NaviOperatingRadius,FromFlightCtrl.Param.Byte[8],0,255);
Parameter.
NaviMaxFlyingRange = FromFlightCtrl.
Param.
Byte[8];
CHK_POTI_MM
(Parameter.
NaviWindCorrection,FromFlightCtrl.
Param.
Byte[9],0,255);
CHK_POTI_MM
(Parameter.
NaviAccCompensation,FromFlightCtrl.
Param.
Byte[10],0,255);
CHK_POTI_MM
(Parameter.
NaviAngleLimitation,FromFlightCtrl.
Param.
Byte[11],0,255);
break;
case SPI_FCCMD_PARAMETER2
:
CHK_POTI_MM
(FC.
AutoPhotoDistance,FromFlightCtrl.
Param.
Byte[0],0,255);
if(FromFlightCtrl.
Param.
Byte[1] == SPEAK_ERR_CALIBARTION
) ShowCalibrationErrorMessage
= 8;
if(FromFlightCtrl.
Param.
Byte[1])
{
FC.
FromFC_SpeakHoTT = FromFlightCtrl.
Param.
Byte[1]; // will be cleared in the SD-Logging
}
if(NaviData_Flags_SpeakHoTT_Processed
)
{
NaviData_Flags.
SpeakHoTT = FromFlightCtrl.
Param.
Byte[1]; // will be cleared after the Uart-Trasmitting
NaviData_Flags_SpeakHoTT_Processed
= 0;
}
FC.
BAT_UsedCapacity = FromFlightCtrl.
Param.
Int[1]; // 2 & 3
Parameter.
FromFC_LowVoltageHomeActive = FromFlightCtrl.
Param.
Byte[4];
if(FromFlightCtrl.
Param.
Byte[5]) FromFC_LoadWP_List
= FromFlightCtrl.
Param.
Byte[5];
if(FromFlightCtrl.
Param.
Byte[6]) FromFC_Load_SinglePoint
= FromFlightCtrl.
Param.
Byte[6];
if(FromFlightCtrl.
Param.
Byte[7]) FromFC_Save_SinglePoint
= FromFlightCtrl.
Param.
Byte[7];
CompassSetpoint
= FromFlightCtrl.
Param.
sInt[4] * 10; // 8 & 9
CompassSetpointCorrected
= (3600 + CompassSetpoint
+ FC.
FromFC_CompassOffset + GeoMagDec
) % 3600;
FC.
StatusFlags3 = FromFlightCtrl.
Param.
Byte[10];
Parameter.
SingleWpSpeed = FromFlightCtrl.
Param.
Byte[11];
break;
case SPI_FCCMD_STICK
:
FC.
StickGas = FromFlightCtrl.
Param.
sByte[0];
FC.
StickYaw = FromFlightCtrl.
Param.
sByte[1];
FC.
StickRoll = FromFlightCtrl.
Param.
sByte[2];
FC.
StickNick = FromFlightCtrl.
Param.
sByte[3];
FC.
Poti[0] = FromFlightCtrl.
Param.
Byte[4];
FC.
Poti[1] = FromFlightCtrl.
Param.
Byte[5];
FC.
Poti[2] = FromFlightCtrl.
Param.
Byte[6];
FC.
Poti[3] = FromFlightCtrl.
Param.
Byte[7];
FC.
Poti[4] = FromFlightCtrl.
Param.
Byte[8];
FC.
Poti[5] = FromFlightCtrl.
Param.
Byte[9];
FC.
Poti[6] = FromFlightCtrl.
Param.
Byte[10];
FC.
Poti[7] = FromFlightCtrl.
Param.
Byte[11];
CHK_POTI_MM
(WaypointAcceleration
,WaypointAccelerationSetting
,0,255); // that could be a Poti-Value
break;
case SPI_FCCMD_STICK2
:
FC.
StickGas = FromFlightCtrl.
Param.
sByte[0];
FC.
StickYaw = FromFlightCtrl.
Param.
sByte[1];
FC.
StickRoll = FromFlightCtrl.
Param.
sByte[2];
FC.
StickNick = FromFlightCtrl.
Param.
sByte[3];
memcpy((u8
*) &(PPM_In
[1]), (u8
*) &FromFlightCtrl.
Param.
sByte[4], 16); // 16 Bytes PPM-Data
PPM_In
[0] = 0;
break;
case SPI_FCCMD_MISC
:
if(CompassCalState
!= FromFlightCtrl.
Param.
Byte[0])
{ // put only new CompassCalState into queue to send via I2C
// if(FromFlightCtrl.Param.Byte[0] == CompassCalState+1 || FromFlightCtrl.Param.Byte[0] == 0)
{
CompassCalState
= FromFlightCtrl.
Param.
Byte[0];
Compass_SetCalState
(CompassCalState
);
}
// else CompassCalState = 0;
}
HoverGas
= FromFlightCtrl.
Param.
Byte[1];
NaviData.
Variometer = (NaviData.
Variometer + 2 * (FromFlightCtrl.
Param.
sInt[1] - NaviData.
Altimeter_5cm)) / 2; // provisorisch
FC.
Altimeter_5cm = FromFlightCtrl.
Param.
sInt[1]; // in 5cm
if(!(SimulationFlags
& SIMULATION_ACTIVE
)) NaviData.
SetpointAltitude = FromFlightCtrl.
Param.
sInt[2]; // in 5cm
FC.
Error[0] |= FromFlightCtrl.
Param.
Byte[6];
FC.
Error[1] |= FromFlightCtrl.
Param.
Byte[7];
DebugOut.
StatusGreen |= AMPEL_FC
; // status of FC Present
DebugOut.
StatusGreen |= AMPEL_BL
; // status of BL Present
if(FC.
Error[0] || FC.
Error[1] /* || FC.Error[2] || FC.Error[3] || FC.Error[4]*/) DebugOut.
StatusRed |= AMPEL_FC
;
else DebugOut.
StatusRed &= ~AMPEL_FC
;
FC_I2C_ErrorConter
= FromFlightCtrl.
Param.
Byte[8];
FC.
RC_Quality = FromFlightCtrl.
Param.
Byte[9];
if(FC.
RC_Quality > 160) NaviData.
RC_Quality = 200; else NaviData.
RC_Quality = FC.
RC_Quality;
NC_Wait_for_LED
= FromFlightCtrl.
Param.
Byte[10];
//DebugOut.Analog[] = NC_Wait_for_LED;
NaviData.
Gas = (FC.
BAT_Voltage * (u32
) FromFlightCtrl.
Param.
Byte[11]) / (u32
) Parameter.
LowVoltageWarning;
if(LoggingGasCnt
== 0) LoggingGasFilter
= 0;
LoggingGasFilter
+= (u32
) FromFlightCtrl.
Param.
Byte[11];
LoggingGasCnt
++;
break;
case SPI_FCCMD_VERSION
: // slow!
FC_Version.
Major = FromFlightCtrl.
Param.
Byte[0];
FC_Version.
Minor = FromFlightCtrl.
Param.
Byte[1];
FC_Version.
Patch = FromFlightCtrl.
Param.
Byte[2];
FC_Version.
Compatible = FromFlightCtrl.
Param.
Byte[3];
FC_Version.
Hardware = FromFlightCtrl.
Param.
Byte[4];
Parameter.
FromFC_LandingSpeed = FromFlightCtrl.
Param.
Byte[5];
Parameter.
ComingHomeAltitude = FromFlightCtrl.
Param.
Byte[6];
CHK_POTI_MM
(Parameter.
AutoPhotoAltitudes,FromFlightCtrl.
Param.
Byte[7],0,255);
UART_VersionInfo.
BL_Firmware = FromFlightCtrl.
Param.
Byte[8];
Parameter.
ActiveSetting = FromFlightCtrl.
Param.
Byte[9];
FlugMinutenGesamt
= FromFlightCtrl.
Param.
Int[5]; // 10 & 11
IamMaster
= FromFlightCtrl.
Param.
Byte[12];
break;
case SPI_FCCMD_NEUTRAL
: // slow!
FC.
AdNeutralNick = FromFlightCtrl.
Param.
Int[0];
FC.
AdNeutralRoll = FromFlightCtrl.
Param.
Int[1];
FC.
AdNeutralYaw = FromFlightCtrl.
Param.
Int[2];
Parameter.
Driftkomp = FromFlightCtrl.
Param.
Byte[6];
Parameter.
NaviPH_LoginTime = FromFlightCtrl.
Param.
Byte[7];
Parameter.
ReceiverType = FromFlightCtrl.
Param.
Byte[8];
CHK_POTI_MM
(Parameter.
NaviGpsPLimit,FromFlightCtrl.
Param.
Byte[9],0,255);
CHK_POTI_MM
(Parameter.
NaviGpsILimit,FromFlightCtrl.
Param.
Byte[10],0,255);
CHK_POTI_MM
(Parameter.
NaviGpsDLimit,FromFlightCtrl.
Param.
Byte[11],0,255);
break;
case SPI_FCCMD_SLOW2
: // slow!
FC.
BoatNeutralNick = FromFlightCtrl.
Param.
Int[0]; // 0 & 1
FC.
BoatNeutralRoll = FromFlightCtrl.
Param.
Int[1]; // 2 & 3
FC.
BoatNeutralYaw = FromFlightCtrl.
Param.
Int[2]; // 4 & 5
Parameter.
CamOrientation = FromFlightCtrl.
Param.
Byte[6];
if(FromFlightCtrl.
Param.
Byte[7] >= 68 && FromFlightCtrl.
Param.
Byte[7] <= 188)
{
FC.
FromFC_DisableDeclination = 1;
FC.
FromFC_CompassOffset = 10 * (s8
) (FromFlightCtrl.
Param.
Byte[7] - 128);
GeoMagDec
= 0;
}
else
{
FC.
FromFC_DisableDeclination = 0;
FC.
FromFC_CompassOffset = 10 * FromFlightCtrl.
Param.
sByte[7];
}
Parameter.
GlobalConfig = FromFlightCtrl.
Param.
Byte[8];
Parameter.
ExtraConfig = FromFlightCtrl.
Param.
Byte[9];
Parameter.
OrientationAngle = FromFlightCtrl.
Param.
Byte[10];
Parameter.
GlobalConfig3 = FromFlightCtrl.
Param.
Byte[11];
break;
case SPI_FCCMD_SLOW3
: // slow!
ServoParams.
NickControl = FromFlightCtrl.
Param.
Byte[0];
ServoParams.
RollControl = FromFlightCtrl.
Param.
Byte[1];
Parameter.
DescendRange = FromFlightCtrl.
Param.
Byte[2];
Parameter.
MaximumAltitude = FromFlightCtrl.
Param.
Byte[3];
ServoParams.
CompInvert = FromFlightCtrl.
Param.
Byte[4];
Parameter.
HomeYawMode = ((ServoParams.
CompInvert & 0x18) >> 3);
NaviData_Home.
LipoCellCount = FromFlightCtrl.
Param.
Byte[5];
if(UART_VersionInfo.
HWMajor >= 30) NaviData_Volatile.
ShutterCounter = TrigLogging.
Count;
else NaviData_Volatile.
ShutterCounter = FromFlightCtrl.
Param.
Int[3]; // 6 & 7
LowVoltageLandingActive
= FromFlightCtrl.
Param.
Byte[8];
Parameter.
FailSafeTime = FromFlightCtrl.
Param.
Byte[9];
// DebugOut.Analog[] = NaviData_Volatile.ShutterCounter;
// 8
// 9
// 10
// 11
break;
default:
break;
}
DebugOut.
Analog[0] = FromFlightCtrl.
AngleNick;
DebugOut.
Analog[1] = FromFlightCtrl.
AngleRoll;
DebugOut.
Analog[11] = FromFlightCtrl.
GyroHeading/10;// in deg
// every time we got new data from the FC via SPI call the navigation routine
// and update GPSStick that are returned to FC
SPI_RxBuffer_Request
= 0;
GPS_Navigation
(&GPSData
, &(ToFlightCtrl.
GPSStick));
ClearFCStatusFlags
= 1;
if(counter
)
{
counter
--; // count down to enable servo
if(!counter
) TIMER2_Init
(); // enable Servo Output
}
timeout
= SetDelay
(80); // 80 ms, new data are send every 20 ms
} // EOF if(SPI_RxBuffer_Request)
else // no new SPI data
{
if(CheckDelay
(timeout
) && (counter
== 0))
{
TIMER2_Deinit
(); // disable Servo Output
counter
= 50; // reset counter for enabling Servo Output
}
}
}
//------------------------------------------------------
void SPI0_GetFlightCtrlVersion
(void)
{
u32 timeout
;
u8 repeat
;
u8 msg
[64];
UART1_PutString
("\r\n Looking for FlightControl");
FC_Version.
Major = 0xFF;
FC_Version.
Minor = 0xFF;
FC_Version.
Patch = 0xFF;
FC_Version.
Compatible = 0xFF;
// polling FC version info
repeat
= 0;
do
{
timeout
= SetDelay
(250);
do
{
SPI0_UpdateBuffer
();
if (FC_Version.
Major != 0xFF) break;
}while (!CheckDelay
(timeout
));
UART1_PutString
(".");
repeat
++;
FCCalibActive
= 0;
}while((FC_Version.
Major == 0xFF) && (repeat
< 40)); // 40*250ms = 10s
// if we got it
if (FC_Version.
Major != 0xFF)
{
sprintf(msg
, " FC V%d.%02d%c HW:%d.%d", FC_Version.
Major, FC_Version.
Minor, 'a'+FC_Version.
Patch, FC_Version.
Hardware/10,FC_Version.
Hardware%10);
UART1_PutString
(msg
);
}
else UART1_PutString
("\r\n not found!");
//sprintf(msg, "\r\n size: HugeBlockFromFC %i",sizeof(HugeBlockFromFC));
//UART1_PutString(msg);
}
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + extended Current measurement -> 200 = 20A 201 = 21A 255 = 75A (20+55)
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
u16 BL3_Current
(u8 who
) // in 0,1A
{
if(Motor
[who
].
Current <= 200) return((u16
) Motor
[who
].
Current);
else
{
if(Motor_Version
[who
] & MOTOR_STATE_BL30
) return(200 + 10 * (u16
) (Motor
[who
].
Current - 200));
else return((u16
) Motor
[who
].
Current);
}
}