Subversion Repositories NaviCtrl

Rev

Rev 394 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
242 killagreg 1
/*#######################################################################################*/
2
/* !!! THIS IS NOT FREE SOFTWARE !!!                                                     */
3
/*#######################################################################################*/
4
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
5
// + www.MikroKopter.com
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 7
// + Software Nutzungsbedingungen (english version: see below)
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
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.
12
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
242 killagreg 13
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 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.
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.
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.
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 
22
// + des Mitverschuldens offen.
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.
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
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.
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.
242 killagreg 31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 32
// + Software LICENSING TERMS
242 killagreg 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 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 
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.
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
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
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
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
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.
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.
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.
52
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
53
// + #### END OF LICENSING TERMS ####
54
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
242 killagreg 55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
56
#include "91x_lib.h"
57
#include "compass.h"
58
#include "mk3mag.h"
253 killagreg 59
#include "ncmag.h"
292 killagreg 60
#include "spi_slave.h"
61
#include "mymath.h"
242 killagreg 62
#include "uart1.h"
254 killagreg 63
#include "fifo.h"
264 killagreg 64
#include "led.h"
292 killagreg 65
#include "main.h"
242 killagreg 66
 
254 killagreg 67
u8 CompassCalStateQueue[10];
68
fifo_t CompassCalcStateFiFo;
69
 
70
volatile s16vec_t MagVector;   // is written by mk3mag or ncmag implementation
394 killagreg 71
volatile s16 Compass_Heading;  // is written by mk3mag or ncmag implementation
254 killagreg 72
volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
330 holgerb 73
s16 Hx = 0, Hy = 0;
74
s32 EarthMagneticField = 100;
75
s32 EarthMagneticFieldFiltered = 100;
76
s32 EarthMagneticInclination = 0;
340 holgerb 77
s32 EarthMagneticInclinationFiltered = 0;
338 holgerb 78
s32 EarthMagneticInclinationTheoretic = 0;
330 holgerb 79
u8  ErrorDisturbedEarthMagnetField = 0;
342 holgerb 80
s16 EarthMagneticStrengthTheoretic = 0; // in mT (50 in Germany - 22 in Brazil)
254 killagreg 81
 
253 killagreg 82
#define COMPASS_NONE    0
83
#define COMPASS_MK3MAG  1
84
#define COMPASS_NCMAG   2
254 killagreg 85
u8 Compass_Device = COMPASS_NONE;
242 killagreg 86
 
87
void Compass_Init(void)
88
{
268 killagreg 89
        switch(Compass_Device)
264 killagreg 90
        {
268 killagreg 91
                case COMPASS_NONE:
92
                        UART1_PutString("\r\n Looking for compass");
342 holgerb 93
                        if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
94
                        else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
95
 
96
/*                      if(Version_HW > 11)
268 killagreg 97
                        {
98
                                if(      NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
99
                                else if( MK3MAG_Init()) Compass_Device = COMPASS_MK3MAG;
100
                        }
101
                        else
102
                        {
103
                                if(     MK3MAG_Init() ) Compass_Device = COMPASS_MK3MAG;
104
                                else if( NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
105
                        }
342 holgerb 106
*/
268 killagreg 107
                        break;
311 killagreg 108
 
268 killagreg 109
                case COMPASS_NCMAG:
110
                        if( NCMAG_Init() )      Compass_Device = COMPASS_NCMAG;
311 killagreg 111
                        else                            Compass_Device = COMPASS_NONE;
268 killagreg 112
                        break;
113
 
114
                case COMPASS_MK3MAG:
115
                default:
116
                        // nothing to do
117
                        break;
118
 
264 killagreg 119
        }
254 killagreg 120
        fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
242 killagreg 121
}
122
 
123
 
292 killagreg 124
void Compass_CalcHeading(void)
242 killagreg 125
{
292 killagreg 126
        if((UART_VersionInfo.HardwareError[0] & NC_ERROR0_SPI_RX) || Compass_CalState)
242 killagreg 127
        {
292 killagreg 128
                Compass_Heading = -1;
242 killagreg 129
        }
292 killagreg 130
        else // fc attitude is avialable and no compass calibration active
131
        {
132
                // calculate attitude correction
133
                // a float implementation takes too long
330 holgerb 134
                s16 tmp;
292 killagreg 135
                s32 sinnick, cosnick, sinroll, cosroll;
311 killagreg 136
                tmp = FromFlightCtrl.AngleNick/10; // in deg
292 killagreg 137
                sinnick = (s32)c_sin_8192(tmp);
138
                cosnick = (s32)c_cos_8192(tmp);
311 killagreg 139
                tmp = FromFlightCtrl.AngleRoll/10; // in deg
292 killagreg 140
                sinroll = (s32)c_sin_8192(tmp);
141
                cosroll = (s32)c_cos_8192(tmp);
311 killagreg 142
                // tbd. compensation signs and oriantation has to be fixed
292 killagreg 143
                Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
311 killagreg 144
                Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
292 killagreg 145
                // calculate heading
293 killagreg 146
                tmp = (s16)(c_atan2_546(Hy, Hx)/546L);
292 killagreg 147
                if (tmp > 0) tmp = 360 - tmp;
148
                else tmp = -tmp;
149
                Compass_Heading = tmp;
150
        }
242 killagreg 151
}
152
 
330 holgerb 153
 
292 killagreg 154
void Compass_Update(void)
242 killagreg 155
{
321 holgerb 156
static s16vec_t old;  
157
static u32 check_value_counter = 0;
292 killagreg 158
        // check for new cal state
159
        Compass_UpdateCalState();
351 holgerb 160
        if(Compass_CalState) FC_is_Calibrated = 0;
292 killagreg 161
        // initiate new compass communication
254 killagreg 162
        switch(Compass_Device)
242 killagreg 163
        {
164
                case COMPASS_MK3MAG:
292 killagreg 165
                        MK3MAG_Update();
339 holgerb 166
                        DebugOut.Analog[24] = MagVector.X;
167
                        DebugOut.Analog[25] = MagVector.Y;
168
                        DebugOut.Analog[26] = MagVector.Z;
242 killagreg 169
                        break;
253 killagreg 170
                case COMPASS_NCMAG:
311 killagreg 171
                        NCMAG_Update();
339 holgerb 172
                        DebugOut.Analog[24] = MagRawVector.X;
173
                        DebugOut.Analog[25] = MagRawVector.Y;
174
                        DebugOut.Analog[26] = MagRawVector.Z;
242 killagreg 175
                default:
311 killagreg 176
                        break;
242 killagreg 177
        }
321 holgerb 178
    if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normally changing
179
 
180
        if(check_value_counter > 5000)
181
         {
182
          Compass_Heading = -1; // values didn't change for 5 seconds -> probably a compass-fault
183
         }
184
         else check_value_counter++;
185
 
186
        old.X = MagVector.X;
187
        old.Y = MagVector.Y;
188
        old.Z = MagVector.Z;
242 killagreg 189
}
254 killagreg 190
 
191
// put cal state into fifo
192
void Compass_SetCalState(u8 CalState)
193
{
311 killagreg 194
        fifo_put(&CompassCalcStateFiFo, CalState);
254 killagreg 195
}
196
 
197
// get cal state from fifo
198
void Compass_UpdateCalState()
199
{
311 killagreg 200
        fifo_get(&CompassCalcStateFiFo, (u8*)&Compass_CalState);
254 killagreg 201
}
202