WebSVN - NaviCtrl - Diff - Rev 351 and 360

 /*#######################################################################################*/
 /* !!! 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
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+// + Software Nutzungsbedingungen (english version: see below)
-// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
+// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
-// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
+// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
-// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
+// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
-// + bzgl. der Nutzungsbedingungen aufzunehmen.
+// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
-// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
+// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
-// + Verkauf von Luftbildaufnahmen, usw.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
+// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
-// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
+// + 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
-// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
+// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
-// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
+// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
-// + eindeutig als Ursprung verlinkt werden
+// + 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
-// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
+// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
-// + Benutzung auf eigene Gefahr
+// + 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.
-// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
+// + 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 Portierung oder Nutzung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
+// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
-// + mit unserer Zustimmung zulässig
+// + #### 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.
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
+// + Software LICENSING TERMS
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
+// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
-// + this list of conditions and the following disclaimer.
+// + 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
-// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
+// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
-// +     from this software without specific prior written permission.
+// + The Software may only be used with the Licensor's products.
-// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permitted
+// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
-// +     for non-commercial use (directly or indirectly)
-// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
+// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
-// +     with our written permission
+// + agreement shall be the property of the Licensor.
-// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
+// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
-// +     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
+// + features that can be used to identify the program may not be altered or defaced by the customer.
-//
-// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// + The customer shall be responsible for taking reasonable precautions
-// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
-// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
-// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
-// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
-// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
-// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
-// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
-// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
-// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
-// +  POSSIBILITY OF SUCH DAMAGE.
+// + #### 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 s16 Compass_Heading;   // is written by mk3mag or ncmag implementation
 volatile u8  Compass_CalState; // is written by mk3mag or ncmag implementation
 s16 Hx = 0, Hy = 0;
 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)
 #define COMPASS_NONE    0
 #define COMPASS_MK3MAG  1
 #define COMPASS_NCMAG   2
 u8 Compass_Device = COMPASS_NONE;
 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;
 /*                      if(Version_HW > 11)
+                        {
                                 if(      NCMAG_Init() ) Compass_Device = COMPASS_NCMAG;
                                 else if( MK3MAG_Init()) Compass_Device = COMPASS_MK3MAG;
+                        }
                         else
+                        {
                                 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:
                 default:
                         // nothing to do
                         break;
+        }
         fifo_init(&CompassCalcStateFiFo, CompassCalStateQueue, sizeof(CompassCalStateQueue), NO_ITLine, NO_ITLine);
+}
 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
                 // a float implementation takes too long
                 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);
                 // tbd. compensation signs and oriantation has to be fixed
                 Hx = (s16)((MagVector.Y * cosnick + MagVector.Z * sinnick)/8192L);
                 Hy = (s16)((MagVector.X * cosroll - MagVector.Z * sinroll)/8192L);
                 // calculate heading
                 tmp = (s16)(c_atan2_546(Hy, Hx)/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();
                         DebugOut.Analog[24] = MagVector.X;
                         DebugOut.Analog[25] = MagVector.Y;
                         DebugOut.Analog[26] = MagVector.Z;
                         break;
                 case COMPASS_NCMAG:
                         NCMAG_Update();
                         DebugOut.Analog[24] = MagRawVector.X;
                         DebugOut.Analog[25] = MagRawVector.Y;
                         DebugOut.Analog[26] = MagRawVector.Z;
                 default:
                         break;
+        }
     if(!((old.X == MagVector.X) || (old.Y == MagVector.Y) || (old.Z == MagVector.Z))) check_value_counter = 0; // Values are normally changing
         if(check_value_counter > 5000)
+         {
           Compass_Heading = -1; // values didn't change for 5 seconds -> 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);
+}

Subversion Repositories NaviCtrl

(root)/tags/V0.28h/compass.c @ 814 - Rev 351 → 360