/*#######################################################################################*/
/* !!! 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 <stdio.h>
#include <string.h>
#include "91x_lib.h"
#include "mk3mag.h"
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "main.h"
#include "uart1.h"
#include "compass.h"
#include "spi_slave.h"
#define MK3MAG_SLAVE_ADDRESS 0x50 // i2C slave address
u8 MK3MAG_Present
= 0;
typedef struct
{
s16 Nick
;
s16 Roll
;
} __attribute__
((packed
)) MK3MAG_WriteAttitude_t
;
typedef struct
{
u8 Major
;
u8 Minor
;
u8 Patch
;
u8 Compatible
;
} __attribute__
((packed
)) MK3MAG_Version_t
;
typedef struct
{
u8 CalByte
;
u8 Dummy1
;
u8 Dummy2
;
} __attribute__
((packed
)) MK3MAG_Cal_t
;
// Transfer buffers
volatile MK3MAG_WriteAttitude_t MK3MAG_WriteAttitude
;
volatile MK3MAG_Version_t MK3MAG_Version
;
volatile MK3MAG_Cal_t MK3MAG_WriteCal
;
volatile MK3MAG_Cal_t MK3MAG_ReadCal
;
// -------------------------------------------------------------------------------
// the calculation of the MK3MAG packet checksum
// calculate the crc of bytecount bytes in buffer
u8 MK3MAG_CalcCRC
(u8
* pBuffer
, u8 bytecount
)
{
u8 i
, crc
= 0;
for(i
=0; i
< bytecount
; i
++)
{
crc
+= pBuffer
[i
];
}
crc
= ~crc
;
return crc
;
}
// assuming the last byte in buffer is the crc byte
u8 MK3MAG_CheckCRC
(u8
* pBuffer
, u8 BuffLen
)
{
u8 crc
= 0, retval
= 0;
if(BuffLen
== 0) return(retval
);
crc
= MK3MAG_CalcCRC
(pBuffer
, BuffLen
-1);
if(crc
== pBuffer
[BuffLen
-1])
{
retval
= 1;
DebugOut.
Analog[15]++; // count mk3mag ok
}
else
{
retval
= 0;
DebugOut.
Analog[14]++; // count mk3mag crc error
}
return(retval
);
}
// -------------------------------------------------------------------------------
// the I2C rx data handler functions
// rx data handler for version info request
void MK3MAG_UpdateVersion
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if crc is ok and number of byte are matching
if(MK3MAG_CheckCRC
(pRxBuffer
, RxBufferSize
) && (RxBufferSize
== (sizeof(MK3MAG_Version
)+1)) )
{
memcpy((u8
*)&MK3MAG_Version
, pRxBuffer
, sizeof(MK3MAG_Version
));
}
}
// rx data handler for calibration request
void MK3MAG_UpdateCalibration
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if crc is ok and number of byte are matching
if(MK3MAG_CheckCRC
(pRxBuffer
, RxBufferSize
) && (RxBufferSize
== (sizeof(MK3MAG_ReadCal
)+1)) )
{
memcpy((u8
*)&MK3MAG_ReadCal
, pRxBuffer
, sizeof(MK3MAG_ReadCal
));
}
}
// rx data handler for magnetic vector request
void MK3MAG_UpdateMagVector
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if crc is ok and number of byte are matching
if(MK3MAG_CheckCRC
(pRxBuffer
, RxBufferSize
) && (RxBufferSize
== (sizeof(MagVector
)+1)) )
{
s16vec_t
*pMagVector
= (s16vec_t
*)pRxBuffer
;
MagVector.
X = pMagVector
->Y
;
MagVector.
Y = pMagVector
->X
;
MagVector.
Z = -pMagVector
->Z
;
Compass_CalcHeading
();
}
}
//----------------------------------------------------------------
#define MK3MAG_CMD_VERSION 0x01
#define MK3MAG_CMD_READ_MAGVECT 0x02
#define MK3MAG_CMD_WRITE_CAL 0x04
// use I2C1 for communication
void MK3MAG_SendCommand
(u8 command
)
{
// try to catch the I2C buffer
if(I2CBus_LockBuffer
(I2C1
, 0))
{
u8 TxData
[100];
u16 TxBytes
= 0;
u16 RxBytes
= 0;
I2C_pRxHandler_t pRxHandlerFunc
= NULL
;
// update current command id
TxData
[TxBytes
++] = command
;
// set pointers to data area with respect to the command id
switch (command
)
{
case MK3MAG_CMD_VERSION
:
RxBytes
= sizeof(MK3MAG_Version
)+1;
pRxHandlerFunc
= &MK3MAG_UpdateVersion
;
break;
case MK3MAG_CMD_WRITE_CAL
:
RxBytes
= sizeof(MK3MAG_ReadCal
)+1;
pRxHandlerFunc
= &MK3MAG_UpdateCalibration
;
memcpy(TxData
+TxBytes
, (u8
*)&MK3MAG_WriteCal
, sizeof(MK3MAG_WriteCal
));
TxBytes
+= sizeof(MK3MAG_WriteCal
);
break;
case MK3MAG_CMD_READ_MAGVECT
:
RxBytes
= sizeof(MagVector
)+1;
pRxHandlerFunc
= &MK3MAG_UpdateMagVector
;
break;
default: // unknown command id
RxBytes
= 0;
pRxHandlerFunc
= NULL
;
break;
}
// update packet checksum
TxData
[TxBytes
] = MK3MAG_CalcCRC
(TxData
, TxBytes
);
TxBytes
++;
// initiate I2C transmission
I2CBus_Transmission
(I2C1
, MK3MAG_SLAVE_ADDRESS
, TxData
, TxBytes
, pRxHandlerFunc
, RxBytes
);
} // EOF I2C_State == I2C_IDLE
}
//----------------------------------------------------------------
u8 MK3MAG_Init
(void)
{
if(MK3MAG_Present
) // do only short init ! , full init was called before
{
// try reconnect by reseting the I2C bus
I2CBus_Deinit
(I2C1
);
I2CBus_Init
(I2C1
);
}
else // full init
{
u8 msg
[64];
u8 repeat
;
u32 timeout
;
Compass_I2CPort
= I2C1
;
MK3MAG_Present
= 0;
MK3MAG_Version.
Major = 0xFF;
MK3MAG_Version.
Minor = 0xFF;
MK3MAG_Version.
Patch = 0xFF;
MK3MAG_Version.
Compatible = 0xFF;
// polling of version info
repeat
= 0;
do
{
MK3MAG_SendCommand
(MK3MAG_CMD_VERSION
);
if(UART_VersionInfo.
HWMajor > 11) timeout
= SetDelay
(100);
else timeout
= SetDelay
(250);
do
{
if (MK3MAG_Version.
Major != 0xFF) break; // break loop on success
}while (!CheckDelay
(timeout
));
UART1_PutString
(".");
repeat
++;
}while ((MK3MAG_Version.
Major == 0xFF) && (repeat
< 12)); // 12*250ms=3s
// if we got it
if (MK3MAG_Version.
Major != 0xFF)
{
sprintf(msg
, " MK3MAG V%d.%d%c", MK3MAG_Version.
Major, MK3MAG_Version.
Minor, 'a' + MK3MAG_Version.
Patch);
UART1_PutString
(msg
);
if(MK3MAG_Version.
Compatible != MK3MAG_I2C_COMPATIBLE
)
{
UART1_PutString
("\r\n MK3MAG not compatible!");
UART_VersionInfo.
HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE
;
LED_RED_ON
;
}
else
{ // version ok
MK3MAG_Present
= 1;
}
}
}
return(MK3MAG_Present
);
}
//----------------------------------------------------------------
void MK3MAG_Update
(void)
{
static u32 TimerUpdate
= 0;
static s16 x_max
,y_max
,z_max
,x_min
,y_min
,z_min
;
static u8 last_state
,speak
= 0;
u8 msg
[64];
u16 MinCaclibration
= 500;
if( (I2CBus
(I2C1
)->State
== I2C_STATE_UNDEF
) || !MK3MAG_Present
) return;
if(CheckDelay
(TimerUpdate
))
{
// check for incomming compass calibration request
Compass_UpdateCalState
();
MK3MAG_WriteCal.
CalByte = Compass_CalState
;
if(MK3MAG_ReadCal.
CalByte != MK3MAG_WriteCal.
CalByte)
{ // send new calibration state
MK3MAG_SendCommand
(MK3MAG_CMD_WRITE_CAL
);
}
else
{ // calibration state matches
MK3MAG_SendCommand
(MK3MAG_CMD_READ_MAGVECT
); // initiate magvector transfer
switch(Compass_CalState
)
{
case 1:
if(last_state
!= Compass_CalState
)
{
UART1_PutString
("\r\nMK3Mag calibration\r\n");
if(EarthMagneticStrengthTheoretic
)
{
MinCaclibration
= (MinCaclibration
* EarthMagneticStrengthTheoretic
) / 50;
sprintf(msg
, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic
);
UART1_PutString
(msg
);
}
else UART1_PutString
("without GPS\r\n");
}
x_max
= -30000; y_max
= -30000; z_max
= -30000;
x_min
= 30000; y_min
= 30000; z_min
= 30000;
speak
= 1;
break;
case 2:
if(speak
) SpeakHoTT
= SPEAK_CALIBRATE
; speak
= 0;
if(MagVector.
X > x_max
) { x_max
= MagVector.
X; BeepTime
= 60; }
else if(MagVector.
X < x_min
) { x_min
= MagVector.
X; BeepTime
= 20; }
if(MagVector.
Y > y_max
) { y_max
= MagVector.
Y; BeepTime
= 60; }
else if(MagVector.
Y < y_min
) { y_min
= MagVector.
Y; BeepTime
= 20; }
break;
case 3:
speak
= 1;
break;
case 4:
if(speak
) SpeakHoTT
= SPEAK_CALIBRATE
; speak
= 0;
if(MagVector.
Z > z_max
) { z_max
= MagVector.
Z; BeepTime
= 80; }
else if(MagVector.
Z < z_min
) { z_min
= MagVector.
Z; BeepTime
= 80; }
break;
case 5:
if(last_state
== Compass_CalState
) break;
if(((x_max
- x_min
) > MinCaclibration
) && ((y_max
- y_min
) > MinCaclibration
) && ((z_max
- z_min
) > MinCaclibration
))
{
BeepTime
= 2500;
UART1_PutString
("\r\n-> Calibration okay <-\r\n");
SpeakHoTT
= SPEAK_MIKROKOPTER
;
}
else
{
UART1_PutString
("\r\nCalibration FAILED - Values too low: ");
if((x_max
- x_min
) < MinCaclibration
) UART1_PutString
("X! ");
if((y_max
- y_min
) < MinCaclibration
) UART1_PutString
("Y! ");
if((z_max
- z_min
) < MinCaclibration
) UART1_PutString
("Z! ");
UART1_PutString
("\r\n");
SpeakHoTT
= SPEAK_ERR_CALIBARTION
;
}
UART1_PutString
(msg
);
sprintf(msg
, "\r\nX: (%i - %i = %i)\r\n",x_max
,x_min
,x_max
- x_min
);
UART1_PutString
(msg
);
sprintf(msg
, "Y: (%i - %i = %i)\r\n",y_max
,y_min
,y_max
- y_min
);
UART1_PutString
(msg
);
sprintf(msg
, "Z: (%i - %i = %i)\r\n",z_max
,z_min
,z_max
- z_min
);
UART1_PutString
(msg
);
sprintf(msg
, "(Minimum ampilitude is: %i)\r\n",MinCaclibration
);
UART1_PutString
(msg
);
break;
default:
break;
}
last_state
= Compass_CalState
;
}
TimerUpdate
= SetDelay
(20); // every 20 ms are 50 Hz
}
}