Subversion Repositories NaviCtrl

Rev

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

Rev Author Line No. Line
241 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
489 killagreg 10
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
360 holgerb 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.
241 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
489 killagreg 21
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
360 holgerb 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.
241 killagreg 31
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 32
// + Software LICENSING TERMS
241 killagreg 33
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
360 holgerb 34
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
489 killagreg 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
360 holgerb 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.
241 killagreg 55
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
292 killagreg 56
#include <stdio.h>
241 killagreg 57
#include <string.h>
58
#include "91x_lib.h"
59
#include "mk3mag.h"
489 killagreg 60
#include "i2c.h"
241 killagreg 61
#include "timer1.h"
242 killagreg 62
#include "led.h"
241 killagreg 63
#include "main.h"
64
#include "uart1.h"
242 killagreg 65
#include "compass.h"
454 holgerb 66
#include "spi_slave.h"
241 killagreg 67
 
68
#define MK3MAG_SLAVE_ADDRESS            0x50  // i2C slave address
69
 
242 killagreg 70
u8 MK3MAG_Present = 0;
241 killagreg 71
 
242 killagreg 72
typedef struct
73
{
74
  s16 Nick;
75
  s16 Roll;
76
} __attribute__((packed)) MK3MAG_WriteAttitude_t;
77
 
292 killagreg 78
typedef struct
79
{
80
  u8 Major;
81
  u8 Minor;
82
  u8 Patch;
83
  u8 Compatible;
84
} __attribute__((packed)) MK3MAG_Version_t;
85
 
86
typedef struct
87
{
88
  u8 CalByte;
89
  u8 Dummy1;
90
  u8 Dummy2;
91
} __attribute__((packed)) MK3MAG_Cal_t;
92
 
241 killagreg 93
// Transfer buffers
94
volatile MK3MAG_WriteAttitude_t         MK3MAG_WriteAttitude;
95
volatile MK3MAG_Version_t                       MK3MAG_Version;
96
volatile MK3MAG_Cal_t                           MK3MAG_WriteCal;
97
volatile MK3MAG_Cal_t                           MK3MAG_ReadCal;
98
 
99
// -------------------------------------------------------------------------------
100
// the calculation of the MK3MAG packet checksum
101
 
102
// calculate the crc of  bytecount bytes in buffer
103
u8 MK3MAG_CalcCRC(u8* pBuffer, u8 bytecount)
104
{
105
        u8 i, crc = 0;
106
        for(i=0; i < bytecount; i++)
107
        {
108
                crc += pBuffer[i];
109
        }
110
        crc = ~crc;
111
        return crc;
112
}
113
 
114
// assuming the last byte in buffer is the crc byte
115
u8 MK3MAG_CheckCRC(u8* pBuffer, u8 BuffLen)
116
{
117
        u8 crc = 0, retval = 0;
118
        if(BuffLen == 0) return(retval);
119
        crc = MK3MAG_CalcCRC(pBuffer, BuffLen-1);
120
        if(crc == pBuffer[BuffLen-1])
121
        {
122
                retval = 1;
123
                DebugOut.Analog[15]++; // count mk3mag ok
124
        }
125
        else
126
        {
127
                retval = 0;
128
                DebugOut.Analog[14]++; // count mk3mag crc error
129
        }
130
        return(retval);
131
}
132
 
133
// -------------------------------------------------------------------------------
134
// the I2C rx data handler functions
135
 
136
// rx data handler for version info request
137
void MK3MAG_UpdateVersion(u8* pRxBuffer, u8 RxBufferSize)
138
{       // if crc is ok and number of byte are matching
139
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_Version)+1)) )
140
        {
141
                memcpy((u8 *)&MK3MAG_Version, pRxBuffer, sizeof(MK3MAG_Version));
489 killagreg 142
        }
241 killagreg 143
}
144
 
145
// rx data handler for calibration request
146
void MK3MAG_UpdateCalibration(u8* pRxBuffer, u8 RxBufferSize)
147
{       // if crc is ok and number of byte are matching
148
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MK3MAG_ReadCal)+1)) )
149
        {
150
                memcpy((u8 *)&MK3MAG_ReadCal, pRxBuffer, sizeof(MK3MAG_ReadCal));
489 killagreg 151
        }
241 killagreg 152
}
153
 
154
// rx data handler for magnetic vector request
155
void MK3MAG_UpdateMagVector(u8* pRxBuffer, u8 RxBufferSize)
156
{       // if crc is ok and number of byte are matching
242 killagreg 157
        if(MK3MAG_CheckCRC(pRxBuffer, RxBufferSize) && (RxBufferSize == (sizeof(MagVector)+1)) )
241 killagreg 158
        {
292 killagreg 159
                s16vec_t *pMagVector = (s16vec_t*)pRxBuffer;
160
                MagVector.X = pMagVector->Y;
161
                MagVector.Y = pMagVector->X;
489 killagreg 162
                MagVector.Z = -pMagVector->Z;
292 killagreg 163
                Compass_CalcHeading();
489 killagreg 164
        }
241 killagreg 165
}
166
 
292 killagreg 167
//----------------------------------------------------------------
168
#define MK3MAG_CMD_VERSION              0x01
169
#define MK3MAG_CMD_READ_MAGVECT         0x02
170
#define MK3MAG_CMD_WRITE_CAL            0x04
241 killagreg 171
 
464 ingob 172
// use I2C1 for communication
465 ingob 173
void MK3MAG_SendCommand(u8 command)
241 killagreg 174
{
288 killagreg 175
        // try to catch the I2C buffer
489 killagreg 176
        if(I2CBus_LockBuffer(I2C1, 0))
241 killagreg 177
        {
489 killagreg 178
                u8 TxData[100];
248 killagreg 179
                u16 TxBytes = 0;
180
                u16 RxBytes = 0;
241 killagreg 181
                I2C_pRxHandler_t pRxHandlerFunc = NULL;
489 killagreg 182
 
241 killagreg 183
                // update current command id
489 killagreg 184
                TxData[TxBytes++] = command;
241 killagreg 185
 
186
                // set pointers to data area with respect to the command id
187
                switch (command)
188
                {
189
                        case MK3MAG_CMD_VERSION:
190
                                RxBytes = sizeof(MK3MAG_Version)+1;
191
                                pRxHandlerFunc = &MK3MAG_UpdateVersion;
192
                                break;
193
                        case MK3MAG_CMD_WRITE_CAL:
194
                                RxBytes = sizeof(MK3MAG_ReadCal)+1;
195
                                pRxHandlerFunc = &MK3MAG_UpdateCalibration;
489 killagreg 196
                                memcpy(TxData+TxBytes, (u8*)&MK3MAG_WriteCal, sizeof(MK3MAG_WriteCal));
248 killagreg 197
                                TxBytes += sizeof(MK3MAG_WriteCal);
241 killagreg 198
                                break;
199
                        case MK3MAG_CMD_READ_MAGVECT:
242 killagreg 200
                                RxBytes = sizeof(MagVector)+1;
241 killagreg 201
                                pRxHandlerFunc = &MK3MAG_UpdateMagVector;
202
                                break;
203
                        default: // unknown command id
204
                                RxBytes = 0;
205
                                pRxHandlerFunc = NULL;
206
                                break;
207
                }
208
                // update packet checksum
489 killagreg 209
                TxData[TxBytes] = MK3MAG_CalcCRC(TxData, TxBytes);
248 killagreg 210
                TxBytes++;
241 killagreg 211
                // initiate I2C transmission
489 killagreg 212
                I2CBus_Transmission(I2C1, MK3MAG_SLAVE_ADDRESS, TxData, TxBytes, pRxHandlerFunc, RxBytes);
213
        } // EOF I2C_State == I2C_IDLE
241 killagreg 214
}
215
 
216
 
217
//----------------------------------------------------------------
242 killagreg 218
u8 MK3MAG_Init(void)
241 killagreg 219
{
489 killagreg 220
        if(MK3MAG_Present) // do only short init ! , full init was called before
221
        {
222
                // try reconnect by reseting the I2C bus
223
                I2CBus_Deinit(I2C1);
224
                I2CBus_Init(I2C1);
225
        }
226
        else // full init
227
        {
228
                u8 msg[64];
229
                u8 repeat;
230
                u32 timeout;
483 holgerb 231
 
489 killagreg 232
                Compass_I2CPort = I2C1;
233
                MK3MAG_Present = 0;
483 holgerb 234
 
489 killagreg 235
                MK3MAG_Version.Major = 0xFF;
236
                MK3MAG_Version.Minor = 0xFF;
237
                MK3MAG_Version.Patch = 0xFF;
238
                MK3MAG_Version.Compatible = 0xFF;
483 holgerb 239
 
489 killagreg 240
                // polling of version info
241
                repeat = 0;
242
                do
243
                {
244
                        MK3MAG_SendCommand(MK3MAG_CMD_VERSION);
530 holgerb 245
                        if(UART_VersionInfo.HWMajor > 11) timeout = SetDelay(100);
489 killagreg 246
                        else timeout = SetDelay(250);
483 holgerb 247
 
489 killagreg 248
                        do
249
                        {
250
                                if (MK3MAG_Version.Major != 0xFF) break; // break loop on success
251
                        }while (!CheckDelay(timeout));
252
                        UART1_PutString(".");
253
                        repeat++;
254
                }while ((MK3MAG_Version.Major == 0xFF) && (repeat < 12)); // 12*250ms=3s
255
                // if we got it
256
                if (MK3MAG_Version.Major != 0xFF)
483 holgerb 257
                {
489 killagreg 258
                        sprintf(msg, " MK3MAG V%d.%d%c", MK3MAG_Version.Major, MK3MAG_Version.Minor, 'a' + MK3MAG_Version.Patch);
259
                        UART1_PutString(msg);
260
                        if(MK3MAG_Version.Compatible != MK3MAG_I2C_COMPATIBLE)
261
                        {
262
                                UART1_PutString("\n\r MK3MAG not compatible!");
263
                                UART_VersionInfo.HardwareError[0] |= NC_ERROR0_COMPASS_INCOMPATIBLE;
264
                                LED_RED_ON;
265
                        }
266
                        else
267
                        {       // version ok
268
                                MK3MAG_Present = 1;
269
                        }
242 killagreg 270
                }
241 killagreg 271
        }
242 killagreg 272
        return(MK3MAG_Present);
241 killagreg 273
}
274
 
275
//----------------------------------------------------------------
292 killagreg 276
void MK3MAG_Update(void)
241 killagreg 277
{
292 killagreg 278
        static u32 TimerUpdate = 0;
279
        static s16 x_max,y_max,z_max,x_min,y_min,z_min;
454 holgerb 280
        static u8 last_state,speak = 0;
339 holgerb 281
        u8 msg[64];
282
        u16 MinCaclibration = 500;
241 killagreg 283
 
489 killagreg 284
        if( (I2CBus(I2C1)->State == I2C_STATE_UNDEF) || !MK3MAG_Present ) return;
285
 
292 killagreg 286
        if(CheckDelay(TimerUpdate))
241 killagreg 287
        {
288
                // check for incomming compass calibration request
254 killagreg 289
                Compass_UpdateCalState();
290
                MK3MAG_WriteCal.CalByte = Compass_CalState;
489 killagreg 291
 
241 killagreg 292
                if(MK3MAG_ReadCal.CalByte != MK3MAG_WriteCal.CalByte)
292 killagreg 293
                {       // send new calibration state
241 killagreg 294
                        MK3MAG_SendCommand(MK3MAG_CMD_WRITE_CAL);
295
                }
292 killagreg 296
                else
297
                {       // calibration state matches
298
                        MK3MAG_SendCommand(MK3MAG_CMD_READ_MAGVECT); // initiate magvector transfer
489 killagreg 299
 
292 killagreg 300
                        switch(Compass_CalState)
301
                        {
302
                                case 1:
493 killagreg 303
                                        if(last_state != Compass_CalState)
304
                                        {
305
                                                UART1_PutString("\r\nMK3Mag calibration\n\r");
306
                                                if(EarthMagneticStrengthTheoretic)
342 holgerb 307
                                                {
493 killagreg 308
                                                        MinCaclibration = (MinCaclibration * EarthMagneticStrengthTheoretic) / 50;
309
                                                        sprintf(msg, "Earth field on your location should be: %iuT\r\n",EarthMagneticStrengthTheoretic);
310
                                                        UART1_PutString(msg);
311
                                                }
312
                                                else UART1_PutString("without GPS\n\r");
313
                                        }
292 killagreg 314
                                        x_max = -30000; y_max = -30000; z_max = -30000;
315
                                        x_min = 30000; y_min = 30000; z_min = 30000;
454 holgerb 316
                                        speak = 1;
292 killagreg 317
                                        break;
318
                                case 2:
454 holgerb 319
                                        if(speak) SpeakHoTT = SPEAK_CALIBRATE; speak = 0;
320
 
292 killagreg 321
                                                 if(MagVector.X > x_max) { x_max = MagVector.X; BeepTime = 60; }
322
                                        else if(MagVector.X < x_min) { x_min = MagVector.X; BeepTime = 20; }
323
                                                 if(MagVector.Y > y_max) { y_max = MagVector.Y; BeepTime = 60; }
324
                                        else if(MagVector.Y < y_min) { y_min = MagVector.Y; BeepTime = 20; }
454 holgerb 325
 
292 killagreg 326
                                        break;
489 killagreg 327
                                case 3:
454 holgerb 328
                                        speak = 1;
329
                                        break;
292 killagreg 330
                                case 4:
454 holgerb 331
                                        if(speak) SpeakHoTT = SPEAK_CALIBRATE;  speak = 0;
292 killagreg 332
                                                 if(MagVector.Z > z_max) { z_max = MagVector.Z; BeepTime = 80; }
333
                                        else if(MagVector.Z < z_min) { z_min = MagVector.Z; BeepTime = 80; }
334
                                        break;
339 holgerb 335
                                case 5:
336
                                    if(last_state == Compass_CalState) break;
337
                                        if(((x_max - x_min) > MinCaclibration) && ((y_max - y_min) > MinCaclibration) && ((z_max - z_min) > MinCaclibration))
338
                                        {
493 killagreg 339
                                                BeepTime = 2500;
340
                                                UART1_PutString("\r\n-> Calibration okay <-\n\r");
341
                                                SpeakHoTT = SPEAK_MIKROKOPTER;
339 holgerb 342
                                        }
343
                                        else
344
                                        {
493 killagreg 345
                                                UART1_PutString("\r\nCalibration FAILED - Values too low: ");
346
                                            if((x_max - x_min) < MinCaclibration) UART1_PutString("X! ");
347
                                            if((y_max - y_min) < MinCaclibration) UART1_PutString("Y! ");
348
                                            if((z_max - z_min) < MinCaclibration) UART1_PutString("Z! ");
349
                                                UART1_PutString("\r\n");
350
                                                SpeakHoTT = SPEAK_ERR_CALIBARTION;
339 holgerb 351
                                        }
352
                                        UART1_PutString(msg);
353
                                        sprintf(msg, "\r\nX: (%i - %i = %i)\r\n",x_max,x_min,x_max - x_min);
354
                                        UART1_PutString(msg);
355
                                        sprintf(msg, "Y: (%i - %i = %i)\r\n",y_max,y_min,y_max - y_min);
356
                                        UART1_PutString(msg);
357
                                        sprintf(msg, "Z: (%i - %i = %i)\r\n",z_max,z_min,z_max - z_min);
358
                                        UART1_PutString(msg);
342 holgerb 359
                                        sprintf(msg, "(Minimum ampilitude is: %i)\r\n",MinCaclibration);
360
                                        UART1_PutString(msg);
339 holgerb 361
                                        break;
292 killagreg 362
                                default:
363
                                        break;
339 holgerb 364
                        }
365
                        last_state = Compass_CalState;
287 holgerb 366
                }
292 killagreg 367
                TimerUpdate = SetDelay(20);    // every 20 ms are 50 Hz
241 killagreg 368
        }
369
}