/*#######################################################################################*/
/* !!! THIS IS NOT FREE SOFTWARE !!! */
/*#######################################################################################*/
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 2010 Ingo Busker, Holger Buss
// + Nur für den privaten Gebrauch / NON-COMMERCIAL USE ONLY
// + FOR NON COMMERCIAL USE ONLY
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// + from this software without specific prior written permission.
// + * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
// + for non-commercial use (directly or indirectly)
// + Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// + with our written permission
// + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// + clearly linked as origin
// + * porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
//
// + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// + POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
#include <string.h>
#include "91x_lib.h"
#include "hmc5843.h"
#include "i2c.h"
#include "timer1.h"
#include "led.h"
#include "spi_slave.h"
#include "uart1.h"
#include "compass.h"
#define HMC5843_SLAVE_ADDRESS 0x3C // i2C slave address
u8 HMC5843_Present
= 0;
#define REGISTER_CRA 0
#define REGISTER_CRB 1
#define REGISTER_MODE 2
#define REGISTER_DATAX_MSB 3
#define REGISTER_DATAX_LSB 4
#define REGISTER_DATAY_MSB 5
#define REGISTER_DATAY_LSB 6
#define REGISTER_DATAZ_MSB 7
#define REGISTER_DATAZ_LSB 8
#define REGISTER_STATUS 9
#define REGISTER_IDA 10
#define REGISTER_IDB 11
#define REGISTER_IDC 12
#define HMC5843_IDA 0x48
#define HMC5843_IDB 0x34
#define HMC5843_IDC 0x33
// bit mask for rate
#define HMC5843_CRA_RATE_MASK 0x1C
#define HMC5843_CRA_RATE_0_5HZ 0x00
#define HMC5843_CRA_RATE_1HZ 0x04
#define HMC5843_CRA_RATE_2HZ 0x08
#define HMC5843_CRA_RATE_5HZ 0x0C
#define HMC5843_CRA_RATE_10HZ 0x10 //default
#define HMC5843_CRA_RATE_20HZ 0x14
#define HMC5843_CRA_RATE_50HZ 0x18
// bit mask for mode
#define HMC5843_CRA_MODE_MASK 0x03
#define HMC5843_CRA_MODE_NORMAL 0x00 //default
#define HMC5843_CRA_MODE_POSBIAS 0x01
#define HMC5843_CRA_MODE_NEGBIAS 0x02
#define HMC5843_CRA_MODE_SELFTEST 0x03
// bit mask for gain
#define HMC5843_CRB_GAIN_MASK 0xE0
#define HMC5843_CRB_GAIN_07GA 0x00
#define HMC5843_CRB_GAIN_10GA 0x20 //default
#define HMC5843_CRB_GAIN_15GA 0x40
#define HMC5843_CRB_GAIN_20GA 0x60
#define HMC5843_CRB_GAIN_32GA 0x80
#define HMC5843_CRB_GAIN_38GA 0xA0
#define HMC5843_CRB_GAIN_45GA 0xC0
#define HMC5843_CRB_GAIN_65GA 0xE0
// bit mask for measurement mode
#define HMC5843_MODE_MASK 0xFF
#define HMC5843_MODE_CONTINUOUS 0x00
#define HMC5843_MODE_SINGLE 0x01 // default
#define HMC5843_MODE_IDLE 0x02
#define HMC5843_MODE_SLEEP 0x03
typedef struct
{
u8 A
;
u8 B
;
u8 C
;
} __attribute__
((packed
)) HMC5843_Identification_t
;
volatile HMC5843_Identification_t HMC5843_Identification
;
// ---------- call back handlers -----------------------------------------
// rx data handler for id info request
void HMC5843_UpdateIdentification
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of byte are matching
if(RxBufferSize
== sizeof(HMC5843_Identification
) )
{
memcpy((u8
*)&HMC5843_Identification
, pRxBuffer
, sizeof(HMC5843_Identification
));
}
}
// rx data handler for mag vector request
void HMC5843_UpdateMagVector
(u8
* pRxBuffer
, u8 RxBufferSize
)
{ // if number of byte are matching
if(RxBufferSize
== sizeof(MagVector
) )
{ // byte order from big to little endian
MagVector.
X = pRxBuffer
[0]<<8;
MagVector.
X+= pRxBuffer
[1];
MagVector.
Y = pRxBuffer
[2]<<8;
MagVector.
Y+= pRxBuffer
[3];
MagVector.
Z = pRxBuffer
[4]<<8;
MagVector.
Z+= pRxBuffer
[5];
}
// tbd. calculate heading from mag vector and attitude
CompassHeading
= 50; // tbd.
}
// ----------------------------------------------------------------------------------------
void HMC5843_GetIdentification
(void)
{
if(I2C_LockBuffer
(100))
{
u16 TxBytes
= 0;
HMC5843_Identification.
A = 0xFF;
HMC5843_Identification.
B = 0xFF;
HMC5843_Identification.
C = 0xFF;
I2C_Buffer
[TxBytes
++] = REGISTER_IDA
;
// initiate transmission
if(I2C_Transmission
(HMC5843_SLAVE_ADDRESS
, TxBytes
, &HMC5843_UpdateIdentification
, sizeof(HMC5843_Identification
)))
{
I2C_WaitForEndOfTransmission
(100);
}
}
}
// ----------------------------------------------------------------------------------------
u8 HMC5843_SetConfiguration
(u8 cra
, u8 crb
, u8 mode
)
{
u8 retval
= 0;
if(I2C_LockBuffer
(100))
{
u16 TxBytes
= 0;
I2C_Buffer
[TxBytes
++] = REGISTER_CRA
;
I2C_Buffer
[TxBytes
++] = cra
;
I2C_Buffer
[TxBytes
++] = crb
;
I2C_Buffer
[TxBytes
++] = mode
;
// initiate transmission
if(I2C_Transmission
(HMC5843_SLAVE_ADDRESS
, TxBytes
, NULL
, 0))
{
retval
= I2C_WaitForEndOfTransmission
(50);
}
}
return(retval
);
}
//----------------------------------------------------------------
void HMC5843_GetMagVector
(void)
{
if(I2C_LockBuffer
(100))
{
u16 TxBytes
= 0;
// set register pointer
I2C_Buffer
[TxBytes
++] = REGISTER_DATAX_MSB
;
// initiate transmission
I2C_Transmission
(HMC5843_SLAVE_ADDRESS
, TxBytes
, &HMC5843_UpdateMagVector
, sizeof(MagVector
));
}
}
void HMC5843_UpdateCompass
(void)
{
static u32 TimerCompassUpdate
= 0;
if( (I2C_State
== I2C_STATE_OFF
) || !HMC5843_Present
) return;
if(CheckDelay
(TimerCompassUpdate
))
{
// check for incomming compass calibration request
// update CalByte from spi input queue
/*fifo_get(&CompassCalcStateFiFo, (u8 *)&(MK3MAG_WriteCal.CalByte));
// send new calstate
if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
{
do the calibration here
}
else // request current heading */
{
HMC5843_GetMagVector
();
}
TimerCompassUpdate
= SetDelay
(20); // every 20 ms are 50 Hz
}
}
u8 HMC5843_SelfTest
(void)
{
u32
time;
s32 XMin
= 0, XMax
= 0, YMin
= 0, YMax
= 0, ZMin
= 0, ZMax
= 0;
s32 scale
, scale_min
, scale_max
;
u8 i
= 0;
// activate positive bias field of 0.55 gauss
HMC5843_SetConfiguration
(HMC5843_CRA_RATE_50HZ
|HMC5843_CRA_MODE_POSBIAS
, HMC5843_CRB_GAIN_10GA
, HMC5843_MODE_CONTINUOUS
);
// averaging
for(i
= 0; i
<20; i
++)
{
HMC5843_GetMagVector
();
time = SetDelay
(20);
while(!CheckDelay
(time));
XMax
+= MagVector.
X;
YMax
+= MagVector.
Y;
ZMax
+= MagVector.
Z;
}
// activate negative bias field of 0.55 gauss
HMC5843_SetConfiguration
(HMC5843_CRA_RATE_50HZ
|HMC5843_CRA_MODE_NEGBIAS
, HMC5843_CRB_GAIN_10GA
, HMC5843_MODE_CONTINUOUS
);
while(!CheckDelay
(time));
// averaging
for(i
= 0; i
< 20; i
++)
{
HMC5843_GetMagVector
();
time = SetDelay
(20);
while(!CheckDelay
(time));
XMin
+= MagVector.
X;
YMin
+= MagVector.
Y;
ZMin
+= MagVector.
Z;
}
// setup final configuration
HMC5843_SetConfiguration
(HMC5843_CRA_RATE_50HZ
|HMC5843_CRA_MODE_NORMAL
, HMC5843_CRB_GAIN_10GA
, HMC5843_MODE_CONTINUOUS
);
// prepare scale limits
scale
= 715; // 1300 counts/Gauss * 0.55 Gauss = 715 counts
scale_min
= (scale
* 90)/100;
scale_max
= (scale
* 110)/100;
// check scale for all axes
scale
= (XMax
- XMin
)/40;
if((scale
> scale_max
) && (scale
< scale_min
)) return 0;
scale
= (YMax
+ YMin
)/40;
if((scale
> scale_max
) && (scale
< scale_min
)) return 0;
scale
= (ZMax
+ ZMin
)/40;
if((scale
> scale_max
) && (scale
< scale_min
)) return 0;
return 1;
}
//----------------------------------------------------------------
u8 HMC5843_Init
(void)
{
u8 msg
[64];
u8 repeat
;
HMC5843_Present
= 0;
HMC5843_Identification.
A = 0xFF;
HMC5843_Identification.
B = 0xFF;
HMC5843_Identification.
C = 0xFF;
// polling of identification
repeat
= 0;
do
{
HMC5843_GetIdentification
();
if (HMC5843_Identification.
A != 0xFF) break; // break loop on success
UART1_PutString
(".");
repeat
++;
}while ((HMC5843_Identification.
A == 0xFF) && (repeat
< 12)); // 12*250ms=3s
// if we got it
if(HMC5843_Identification.
A != 0xFF)
{
sprintf(msg
, " HMC5843 ID %d/%d/%d", HMC5843_Identification.
A, HMC5843_Identification.
B, HMC5843_Identification.
C);
UART1_PutString
(msg
);
if ( (HMC5843_Identification.
A == HMC5843_IDA
)
&& (HMC5843_Identification.
B == HMC5843_IDB
)
&& (HMC5843_Identification.
C == HMC5843_IDC
))
{
//HMC5843_SetConfiguration(HMC5843_CRA_RATE_50HZ|HMC5843_CRA_MODE_NORMAL, HMC5843_CRB_GAIN_10GA, HMC5843_MODE_CONTINUOUS);
if(!HMC5843_SelfTest
())
{
UART1_PutString
("\n\r HMC5843 selftest failed!");
LED_RED_ON
;
}
else HMC5843_Present
= 1;
}
else
{
UART1_PutString
("\n\r HMC5843 not compatible!");
LED_RED_ON
;
}
}
return(HMC5843_Present
);
}