WebSVN - NaviCtrl - Diff - Rev 492 and 493


/*#######################################################################################*/
/* !!! 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 "91x_lib.h"
#include "compass.h"
#include "mk3mag.h"
#include "ncmag.h"
#include "spi_slave.h"
#include "mymath.h"
#include "uart1.h"
#include "fifo.h"
#include "led.h"
#include "main.h"
 
u8 CompassCalStateQueue[10];
fifo_t CompassCalcStateFiFo;
 
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
volatile s16vec_t AccVector;   // current acceleration vector of compass, not supported by any HW version
volatile s16vec_t MagVectorHorizontal; // vector componenents in horizontal projection
volatile s16 Compass_Heading;  // is written by mk3mag or ncmag implementation
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
 
 
s32 EarthMagneticField = 100;
s32 EarthMagneticFieldFiltered = 100;
s32 EarthMagneticInclination = 0;
s32 EarthMagneticInclinationFiltered = 0;
s32 EarthMagneticInclinationTheoretic = 0;
u8  ErrorDisturbedEarthMagnetField = 0;
s16 EarthMagneticStrengthTheoretic = 0; // in mT (50 in Germany - 22 in Brazil)
u32 CompassValueErrorCount = 0;
 
#define COMPASS_NONE    0
#define COMPASS_MK3MAG  1
#define COMPASS_NCMAG   2
u8 Compass_Device = COMPASS_NONE;
I2C_TypeDef* Compass_I2CPort = I2C1;
 
void Compass_Init(void)
{
        switch(Compass_Device)
        {
                case COMPASS_NONE:
                        UART1_PutString("\r\n Looking for compass");
                        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
                        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
                        break;
 
                case COMPASS_NCMAG:
                        if( NCMAG_Init() )      Compass_Device = COMPASS_NCMAG;
                        else                            Compass_Device = COMPASS_NONE;
                        break;
 
                case COMPASS_MK3MAG:
                        if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
                        else                            Compass_Device = COMPASS_NONE;
                        break;
 
                default:
                        // nothing to do
                        break;
 
        }
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
 
        if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags |= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;
 
        MagVector.X = 0;
        MagVector.Y = 0;
        MagVector.Z = 0;
        Compass_Heading = -1;
}
 
 
void Compass_Check(void)
{
        switch(Compass_Device)
        {
                case COMPASS_NCMAG:
                        NCMAG_CheckOrientation();
                        break;
 
                case COMPASS_MK3MAG:
                case COMPASS_NONE:
                default:
                        // nothing to do
                        break;
        }
}
 
void Compass_CalcHeading(void)
{
        if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) || Compass_CalState)
        {
                Compass_Heading = -1;
        }
        else // fc attitude is avialable and no compass calibration active
        {
                // calculate attitude correction
 
                s16 tmp;
                s32 sinnick, cosnick, sinroll, cosroll;
                tmp = FromFlightCtrl.AngleNick/10; // in deg
                sinnick = (s32)c_sin_8192(tmp);
                cosnick = (s32)c_cos_8192(tmp);
                tmp = FromFlightCtrl.AngleRoll/10; // in deg
                sinroll = (s32)c_sin_8192(tmp);
                cosroll = (s32)c_cos_8192(tmp);
 
                MagVectorHorizontal.X = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
                MagVectorHorizontal.Y = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
                // calculate heading
                tmp = (s16)(c_atan2_546(MagVectorHorizontal.Y, MagVectorHorizontal.X)/546L);
                if (tmp > 0) tmp = 360 - tmp;
                else tmp = -tmp;
                Compass_Heading = tmp;
        }
}
 
 
void Compass_Update(void)
{
static s16vec_t old;
static u32 check_value_counter = 0;
        // check for new cal state
        Compass_UpdateCalState();
        if(Compass_CalState) FC_is_Calibrated = 0;
        // initiate new compass communication
        switch(Compass_Device)
        {
                case COMPASS_MK3MAG:
                        MK3MAG_Update();
                        break;
                case COMPASS_NCMAG:
                        NCMAG_Update(0);
                        break;
                default:
                        break;
        }
 
        DebugOut.Analog[24] = MagVector.X;
        DebugOut.Analog[25] = MagVector.Y;
        DebugOut.Analog[26] = MagVector.Z;
 
    if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normaly changing
 
        if(check_value_counter == 2000) // 2 seconds no change of the compass values
        {
                UART1_PutString("\n\r Init Mag.-Sensor");
                Compass_Init();
        }
       
        if(check_value_counter > 5000) // 5 seconds no change of the compass values
        {
                Compass_Heading = -1; // -> probably a compass-fault
        }
        else check_value_counter++;
 
        old.X = MagVector.X;
        old.Y = MagVector.Y;
        old.Z = MagVector.Z;
}
 
// put cal state into fifo
void Compass_SetCalState(u8 CalState)
{
        fifo_put(&CompassCalcStateFiFo, CalState);
}
 
// get cal state from fifo
void Compass_UpdateCalState()
{
        fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState);
}
 
 

Rev 492	Rev 493
1	/#######################################################################################/	1	/#######################################################################################/
2	/* !!! THIS IS NOT FREE SOFTWARE !!! */	2	/* !!! THIS IS NOT FREE SOFTWARE !!! */
3	/#######################################################################################/	3	/#######################################################################################/
4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	4	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5	// + www.MikroKopter.com	5	// + www.MikroKopter.com
6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	6	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7	// + Software Nutzungsbedingungen (english version: see below)	7	// + Software Nutzungsbedingungen (english version: see below)
8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -	8	// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den	9	// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool	10	// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.	11	// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.	12	// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	13	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im	14	// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.	15	// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie	16	// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.	17	// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren	18	// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	19	// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren	20	// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand	21	// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
22	// + des Mitverschuldens offen.	22	// + des Mitverschuldens offen.
23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.	23	// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.	24	// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.	25	// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang	26	// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.	27	// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.	28	// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'	29	// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
30	// + 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.	30	// + 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.
31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	31	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
32	// + Software LICENSING TERMS	32	// + Software LICENSING TERMS
33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	33	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -	34	// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
35	// + 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	35	// + 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
36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.	36	// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
37	// + The Software may only be used with the Licensor's products.	37	// + The Software may only be used with the Licensor's products.
38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this	38	// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this	39	// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
40	// + agreement shall be the property of the Licensor.	40	// + agreement shall be the property of the Licensor.
41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other	41	// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
42	// + features that can be used to identify the program may not be altered or defaced by the customer.	42	// + features that can be used to identify the program may not be altered or defaced by the customer.
43	// + The customer shall be responsible for taking reasonable precautions	43	// + The customer shall be responsible for taking reasonable precautions
44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the	44	// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and	45	// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product	46	// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.	47	// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test	48	// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.	49	// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data	50	// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.	51	// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.	52	// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53	// + #### END OF LICENSING TERMS ####	53	// + #### END OF LICENSING TERMS ####
54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.	54	// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++	55	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56	#include "91x_lib.h"	56	#include "91x_lib.h"
57	#include "compass.h"	57	#include "compass.h"
58	#include "mk3mag.h"	58	#include "mk3mag.h"
59	#include "ncmag.h"	59	#include "ncmag.h"
60	#include "spi_slave.h"	60	#include "spi_slave.h"
61	#include "mymath.h"	61	#include "mymath.h"
62	#include "uart1.h"	62	#include "uart1.h"
63	#include "fifo.h"	63	#include "fifo.h"
64	#include "led.h"	64	#include "led.h"
65	#include "main.h"	65	#include "main.h"
66		66
67	u8 CompassCalStateQueue[10];	67	u8 CompassCalStateQueue[10];
68	fifo_t CompassCalcStateFiFo;	68	fifo_t CompassCalcStateFiFo;
69		69
70	volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation	70	volatile s16vec_t MagVector; // is written by mk3mag or ncmag implementation
71	volatile s16vec_t AccVector; // current acceleration vector of compass, not supported by any HW version	71	volatile s16vec_t AccVector; // current acceleration vector of compass, not supported by any HW version
-		72	volatile s16vec_t MagVectorHorizontal; // vector componenents in horizontal projection
72	volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation	73	volatile s16 Compass_Heading; // is written by mk3mag or ncmag implementation
73	volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation	74	volatile u8 Compass_CalState; // is written by mk3mag or ncmag implementation
74	s16 Hx = 0, Hy = 0;	-
-		75
75	s32 EarthMagneticField = 100;	76	s32 EarthMagneticField = 100;
76	s32 EarthMagneticFieldFiltered = 100;	77	s32 EarthMagneticFieldFiltered = 100;
77	s32 EarthMagneticInclination = 0;	78	s32 EarthMagneticInclination = 0;
78	s32 EarthMagneticInclinationFiltered = 0;	79	s32 EarthMagneticInclinationFiltered = 0;
79	s32 EarthMagneticInclinationTheoretic = 0;	80	s32 EarthMagneticInclinationTheoretic = 0;
80	u8 ErrorDisturbedEarthMagnetField = 0;	81	u8 ErrorDisturbedEarthMagnetField = 0;
81	s16 EarthMagneticStrengthTheoretic = 0; // in mT (50 in Germany - 22 in Brazil)	82	s16 EarthMagneticStrengthTheoretic = 0; // in mT (50 in Germany - 22 in Brazil)
82	u32 CompassValueErrorCount = 0;	83	u32 CompassValueErrorCount = 0;
83		84
84	#define COMPASS_NONE 0	85	#define COMPASS_NONE 0
85	#define COMPASS_MK3MAG 1	86	#define COMPASS_MK3MAG 1
86	#define COMPASS_NCMAG 2	87	#define COMPASS_NCMAG 2
87	u8 Compass_Device = COMPASS_NONE;	88	u8 Compass_Device = COMPASS_NONE;
88	I2C_TypeDef* Compass_I2CPort = I2C1;	89	I2C_TypeDef* Compass_I2CPort = I2C1;
89		90
90	void Compass_Init(void)	91	void Compass_Init(void)
91	{	92	{
92	switch(Compass_Device)	93	switch(Compass_Device)
93	{	94	{
94	case COMPASS_NONE:	95	case COMPASS_NONE:
95	UART1_PutString("\r\n Looking for compass");	96	UART1_PutString("\r\n Looking for compass");
96	if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;	97	if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
97	else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;	98	else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
98	break;	99	break;
99		100
100	case COMPASS_NCMAG:	101	case COMPASS_NCMAG:
101	if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;	102	if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
102	else Compass_Device = COMPASS_NONE;	103	else Compass_Device = COMPASS_NONE;
103	break;	104	break;
104		105
105	case COMPASS_MK3MAG:	106	case COMPASS_MK3MAG:
106	if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;	107	if( MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
107	else Compass_Device = COMPASS_NONE;	108	else Compass_Device = COMPASS_NONE;
108	break;	109	break;
109		110
110	default:	111	default:
111	// nothing to do	112	// nothing to do
112	break;	113	break;
113		114
114	}	115	}
115	fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);	116	fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
116		117
117	if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags \|= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;	118	if(Compass_Device == COMPASS_MK3MAG) UART_VersionInfo.Flags \|= NC_VERSION_FLAG_MK3MAG_PRESENT; else UART_VersionInfo.Flags &= ~NC_VERSION_FLAG_MK3MAG_PRESENT;
118		119
119	MagVector.X = 0;	120	MagVector.X = 0;
120	MagVector.Y = 0;	121	MagVector.Y = 0;
121	MagVector.Z = 0;	122	MagVector.Z = 0;
122	Compass_Heading = -1;	123	Compass_Heading = -1;
123	}	124	}
124		125
125		126
126	void Compass_Check(void)	127	void Compass_Check(void)
127	{	128	{
128	switch(Compass_Device)	129	switch(Compass_Device)
129	{	130	{
130	case COMPASS_NCMAG:	131	case COMPASS_NCMAG:
131	NCMAG_CheckOrientation();	132	NCMAG_CheckOrientation();
132	break;	133	break;
133		134
134	case COMPASS_MK3MAG:	135	case COMPASS_MK3MAG:
135	case COMPASS_NONE:	136	case COMPASS_NONE:
136	default:	137	default:
137	// nothing to do	138	// nothing to do
138	break;	139	break;
139	}	140	}
140	}	141	}
141		142
142	void Compass_CalcHeading(void)	143	void Compass_CalcHeading(void)
143	{	144	{
144	if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) \|\| Compass_CalState)	145	if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) \|\| Compass_CalState)
145	{	146	{
146	Compass_Heading = -1;	147	Compass_Heading = -1;
147	}	148	}
148	else // fc attitude is avialable and no compass calibration active	149	else // fc attitude is avialable and no compass calibration active
149	{	150	{
150	// calculate attitude correction	151	// calculate attitude correction
151	// a float implementation takes too long	-
152	s16 tmp;	152	s16 tmp;
153	s32 sinnick, cosnick, sinroll, cosroll;	153	s32 sinnick, cosnick, sinroll, cosroll;
154	tmp = FromFlightCtrl.AngleNick/10; // in deg	154	tmp = FromFlightCtrl.AngleNick/10; // in deg
155	sinnick = (s32)c_sin_8192(tmp);	155	sinnick = (s32)c_sin_8192(tmp);
156	cosnick = (s32)c_cos_8192(tmp);	156	cosnick = (s32)c_cos_8192(tmp);
157	tmp = FromFlightCtrl.AngleRoll/10; // in deg	157	tmp = FromFlightCtrl.AngleRoll/10; // in deg
158	sinroll = (s32)c_sin_8192(tmp);	158	sinroll = (s32)c_sin_8192(tmp);
159	cosroll = (s32)c_cos_8192(tmp);	159	cosroll = (s32)c_cos_8192(tmp);
160	// tbd. compensation signs and oriantation has to be fixed	-
161	Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);	160	MagVectorHorizontal.X = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
162	Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);	161	MagVectorHorizontal.Y = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
163	// calculate heading	162	// calculate heading
164	tmp = (s16)(c_atan2_546(Hy, Hx)/546L);	163	tmp = (s16)(c_atan2_546(MagVectorHorizontal.Y, MagVectorHorizontal.X)/546L);
165	if (tmp > 0) tmp = 360 - tmp;	164	if (tmp > 0) tmp = 360 - tmp;
166	else tmp = -tmp;	165	else tmp = -tmp;
167	Compass_Heading = tmp;	166	Compass_Heading = tmp;
168	}	167	}
169	}	168	}
170		169
171		170
172	void Compass_Update(void)	171	void Compass_Update(void)
173	{	172	{
174	static s16vec_t old;	173	static s16vec_t old;
175	static u32 check_value_counter = 0;	174	static u32 check_value_counter = 0;
176	// check for new cal state	175	// check for new cal state
177	Compass_UpdateCalState();	176	Compass_UpdateCalState();
178	if(Compass_CalState) FC_is_Calibrated = 0;	177	if(Compass_CalState) FC_is_Calibrated = 0;
179	// initiate new compass communication	178	// initiate new compass communication
180	switch(Compass_Device)	179	switch(Compass_Device)
181	{	180	{
182	case COMPASS_MK3MAG:	181	case COMPASS_MK3MAG:
183	MK3MAG_Update();	182	MK3MAG_Update();
184	break;	183	break;
185	case COMPASS_NCMAG:	184	case COMPASS_NCMAG:
186	NCMAG_Update(0);	185	NCMAG_Update(0);
187	break;	186	break;
188	default:	187	default:
189	break;	188	break;
190	}	189	}
191		190
192	DebugOut.Analog[24] = MagVector.X;	191	DebugOut.Analog[24] = MagVector.X;
193	DebugOut.Analog[25] = MagVector.Y;	192	DebugOut.Analog[25] = MagVector.Y;
194	DebugOut.Analog[26] = MagVector.Z;	193	DebugOut.Analog[26] = MagVector.Z;
195		194
196	if(!((old.X == MagVector.X) \|\| (old.Y == MagVector.Y) \|\| (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normaly changing	195	if(!((old.X == MagVector.X) \|\| (old.Y == MagVector.Y) \|\| (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normaly changing
197		196
198	if(check_value_counter == 2000) // 2 seconds no change of the compass values	197	if(check_value_counter == 2000) // 2 seconds no change of the compass values
199	{	198	{
200	UART1_PutString("\n\r Init Mag.-Sensor");	199	UART1_PutString("\n\r Init Mag.-Sensor");
201	Compass_Init();	200	Compass_Init();
202	}	201	}
203		202
204	if(check_value_counter > 5000) // 5 seconds no change of the compass values	203	if(check_value_counter > 5000) // 5 seconds no change of the compass values
205	{	204	{
206	Compass_Heading = -1; // -> probably a compass-fault	205	Compass_Heading = -1; // -> probably a compass-fault
207	}	206	}
208	else check_value_counter++;	207	else check_value_counter++;
209		208
210	old.X = MagVector.X;	209	old.X = MagVector.X;
211	old.Y = MagVector.Y;	210	old.Y = MagVector.Y;
212	old.Z = MagVector.Z;	211	old.Z = MagVector.Z;
213	}	212	}
214		213
215	// put cal state into fifo	214	// put cal state into fifo
216	void Compass_SetCalState(u8 CalState)	215	void Compass_SetCalState(u8 CalState)
217	{	216	{
218	fifo_put(&CompassCalcStateFiFo, CalState);	217	fifo_put(&CompassCalcStateFiFo, CalState);
219	}	218	}
220		219
221	// get cal state from fifo	220	// get cal state from fifo
222	void Compass_UpdateCalState()	221	void Compass_UpdateCalState()
223	{	222	{
224	fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState);	223	fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState);
225	}	224	}
226		225
227		226

Subversion Repositories NaviCtrl

(root)/tags/V2.06i/compass.c @ 910 - Rev 492 → 493