| 0,0 → 1,361 |
|---|
| /*#######################################################################################*/ |
| /* !!! 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 "i2c1.h" |
| #include "i2c0.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(I2C1_LockBuffer(0)) |
| { |
| u16 TxBytes = 0; |
| u16 RxBytes = 0; |
| I2C_pRxHandler_t pRxHandlerFunc = NULL; |
| |
| // update current command id |
| I2C1_Buffer[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((u8*)I2C1_Buffer+1, (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 |
| I2C1_Buffer[TxBytes] = MK3MAG_CalcCRC((u8*)I2C1_Buffer, TxBytes); |
| TxBytes++; |
| // initiate I2C transmission |
| I2C1_Transmission(MK3MAG_SLAVE_ADDRESS, TxBytes, pRxHandlerFunc, RxBytes); |
| } // EOF I2C_State == I2C_IDLE |
| } |
| |
| |
| //---------------------------------------------------------------- |
| u8 MK3MAG_Init(void) |
| { |
| u8 msg[64]; |
| u8 repeat; |
| u32 timeout; |
| |
| MK3MAG_Present = 0; |
| |
| MK3MAG_Version.Major = 0xFF; |
| MK3MAG_Version.Minor = 0xFF; |
| MK3MAG_Version.Patch = 0xFF; |
| MK3MAG_Version.Compatible = 0xFF; |
| |
| Compass_Heading = -1; |
| |
| // polling of version info |
| repeat = 0; |
| do |
| { |
| MK3MAG_SendCommand(MK3MAG_CMD_VERSION); |
| if(Version_HW > 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("\n\r 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( (I2C1_State == I2C_STATE_OFF) || !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\n\r"); |
| 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\n\r"); |
| } |
| 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 <-\n\r"); |
| 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 |
| } |
| } |