Subversion Repositories FlightCtrl

Rev

Rev 2093 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 2093 Rev 2174
1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 
// + www.MikroKopter.com
 2 
// + www.MikroKopter.com
3 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 3 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4 
// + Software Nutzungsbedingungen (english version: see below)
 4 
// + Software Nutzungsbedingungen (english version: see below)
5 
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
 5 
// + der Fa. HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland - nachfolgend Lizenzgeber genannt -
6 
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
 6 
// + Der Lizenzgeber räumt dem Kunden ein nicht-ausschließliches, zeitlich und räumlich* unbeschränktes Recht ein, die im den
7 
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
7 
// + Mikrocontroller verwendete Firmware für die Hardware Flight-Ctrl, Navi-Ctrl, BL-Ctrl, MK3Mag & PC-Programm MikroKopter-Tool
8 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
 8 
// + - nachfolgend Software genannt - nur für private Zwecke zu nutzen.
9 
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
 9 
// + Der Einsatz dieser Software ist nur auf oder mit Produkten des Lizenzgebers zulässig.
10 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 10 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
11 
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
 11 
// + Die vom Lizenzgeber gelieferte Software ist urheberrechtlich geschützt. Alle Rechte an der Software sowie an sonstigen im
12 
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
 12 
// + Rahmen der Vertragsanbahnung und Vertragsdurchführung überlassenen Unterlagen stehen im Verhältnis der Vertragspartner ausschließlich dem Lizenzgeber zu.
13 
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
 13 
// + Die in der Software enthaltenen Copyright-Vermerke, Markenzeichen, andere Rechtsvorbehalte, Seriennummern sowie
14 
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
 14 
// + sonstige der Programmidentifikation dienenden Merkmale dürfen vom Kunden nicht verändert oder unkenntlich gemacht werden.
15 
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
 15 
// + Der Kunde trifft angemessene Vorkehrungen für den sicheren Einsatz der Software. Er wird die Software gründlich auf deren
16 
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 16 
// + Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
17 
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
 17 
// + Die Haftung des Lizenzgebers wird - soweit gesetzlich zulässig - begrenzt in Höhe des typischen und vorhersehbaren
18 
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
18 
// + Schadens. Die gesetzliche Haftung bei Personenschäden und nach dem Produkthaftungsgesetz bleibt unberührt. Dem Lizenzgeber steht jedoch der Einwand
19 
// + des Mitverschuldens offen.
 19 
// + des Mitverschuldens offen.
20 
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
 20 
// + Der Kunde trifft angemessene Vorkehrungen für den Fall, dass die Software ganz oder teilweise nicht ordnungsgemäß arbeitet.
21 
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
 21 
// + Er wird die Software gründlich auf deren Verwendbarkeit zu dem von ihm beabsichtigten Zweck testen, bevor er diese operativ einsetzt.
22 
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
 22 
// + Der Kunde wird er seine Daten vor Einsatz der Software nach dem Stand der Technik sichern.
23 
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
 23 
// + Der Kunde ist darüber unterrichtet, dass der Lizenzgeber seine Daten im zur Vertragsdurchführung erforderlichen Umfang
24 
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
 24 
// + und auf Grundlage der Datenschutzvorschriften erhebt, speichert, verarbeitet und, sofern notwendig, an Dritte übermittelt.
25 
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
 25 
// + *) Die räumliche Nutzung bezieht sich nur auf den Einsatzort, nicht auf die Reichweite der programmierten Software.
26 
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
 26 
// + #### ENDE DER NUTZUNGSBEDINGUNGEN ####'
27 
// +  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.
 27 
// +  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.
28 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 28 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29 
// + Software LICENSING TERMS
 29 
// + Software LICENSING TERMS
30 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 30 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31 
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
 31 
// + of HiSystems GmbH, Flachsmeerstrasse 2, 26802 Moormerland, Germany - the Licensor -
32 
// + 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
32 
// + 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
33 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
 33 
// + (the Software) exclusively for private purposes. The License is unrestricted with respect to time and territory*.
34 
// + The Software may only be used with the Licensor's products.
 34 
// + The Software may only be used with the Licensor's products.
35 
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
 35 
// + The Software provided by the Licensor is protected by copyright. With respect to the relationship between the parties to this
36 
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
 36 
// + agreement, all rights pertaining to the Software and other documents provided during the preparation and execution of this
37 
// + agreement shall be the property of the Licensor.
 37 
// + agreement shall be the property of the Licensor.
38 
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
 38 
// + The information contained in the Software copyright notices, trademarks, other legal reservations, serial numbers and other
39 
// + features that can be used to identify the program may not be altered or defaced by the customer.
 39 
// + features that can be used to identify the program may not be altered or defaced by the customer.
40 
// + The customer shall be responsible for taking reasonable precautions
 40 
// + The customer shall be responsible for taking reasonable precautions
41 
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
 41 
// + for the safe use of the Software. The customer shall test the Software thoroughly regarding its suitability for the
42 
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
 42 
// + intended purpose before implementing it for actual operation. The Licensor's liability shall be limited to the extent of typical and
43 
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
 43 
// + foreseeable damage to the extent permitted by law, notwithstanding statutory liability for bodily injury and product
44 
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
 44 
// + liability. However, the Licensor shall be entitled to the defense of contributory negligence.
45 
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
 45 
// + The customer will take adequate precautions in the case, that the software is not working properly. The customer will test
46 
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
 46 
// + the software for his purpose before any operational usage. The customer will backup his data before using the software.
47 
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
 47 
// + The customer understands that the Licensor collects, stores and processes, and, where required, forwards, customer data
48 
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
 48 
// + to third parties to the extent necessary for executing the agreement, subject to applicable data protection and privacy regulations.
49 
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
 49 
// + *) The territory aspect only refers to the place where the Software is used, not its programmed range.
50 
// + #### END OF LICENSING TERMS ####
 50 
// + #### END OF LICENSING TERMS ####
51 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
 51 
// + Note: For information on license extensions (e.g. commercial use), please contact us at info(@)hisystems.de.
52 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 52 
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
53 
 
 53 
 
54 
#include "capacity.h"
 54 
#include "capacity.h"
55 
#include "twimaster.h"
 55 
#include "twimaster.h"
56 
#include "main.h"
 56 
#include "main.h"
57 
#include "timer0.h"
 57 
#include "timer0.h"
58 
#include "analog.h"
 58 
#include "analog.h"
59 
 
 59 
 
60 
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
 60 
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
61 
#define FC_OFFSET_CURRENT 5  // calculate with a current of 0.5A
 61 
#define FC_OFFSET_CURRENT 5  // calculate with a current of 0.5A
62 
#define BL_OFFSET_CURRENT 2  // calculate with a current of 0.2A
 62 
#define BL_OFFSET_CURRENT 2  // calculate with a current of 0.2A
63 
 
 63 
 
64 
// global varialbles
 64 
// global varialbles
65 
unsigned short update_timer =  0;
 65 
unsigned short update_timer =  0;
66 
Capacity_t Capacity;
 66 
Capacity_t Capacity;
67 
 
 67 
 
68 
// initialize capacity calculation
 68 
// initialize capacity calculation
69 
void Capacity_Init(void)
 69 
void Capacity_Init(void)
70 
{
 70 
{
71 
        Capacity.ActualCurrent = 0;
 71 
        Capacity.ActualCurrent = 0;
72 
        Capacity.UsedCapacity = 0;
 72 
        Capacity.UsedCapacity = 0;
73 
        Capacity.ActualPower = 0;
 73 
        Capacity.ActualPower = 0;
74 
        Capacity.MinOfMaxPWM = 0;
 74 
        Capacity.MinOfMaxPWM = 0;
75 
        #ifdef WITH_REMAINCAPACITY      // only include functions if DEBUG is defined in main.h
 - 
 
76 
 
 - 
 
77 
        #warning : "### with REMAIN CAPACITY ###"
 - 
 
78 
        Capacity.RemainCapacity = (Parameter_UserParam5*100);
 - 
 
79 
        #endif
 - 
 
80 
        update_timer = SetDelay(CAPACITY_UPDATE_INTERVAL);
 75 
        update_timer = SetDelay(CAPACITY_UPDATE_INTERVAL);
81 
}
 76 
}
82 
 
 77 
 
83 
 
 78 
 
84 
// called in main loop at a regular interval
 79 
// called in main loop at a regular interval
85 
void Capacity_Update(void)
 80 
void Capacity_Update(void)
86 
{
 81 
{
87 
        unsigned short Current, SetSum; // max value will be 255 * 12 = 3060
 82 
        unsigned short Current, SetSum; // max value will be 255 * 12 = 3060
88 
        static unsigned short SubCounter = 0;
 83 
        static unsigned short SubCounter = 0;
89 
        static unsigned short CurrentOffset = 0;
 84 
        static unsigned short CurrentOffset = 0;
90 
        static unsigned long SumCurrentOffset = 0;
 85 
        static unsigned long SumCurrentOffset = 0;
91 
        unsigned char i, NumOfMotors, MinOfMaxPWM;
 86 
        unsigned char i, NumOfMotors, MinOfMaxPWM;
92 
 
 87 
 
93 
        if(CheckDelay(update_timer))
 88 
        if(CheckDelay(update_timer))
94 
        {
 89 
        {
95 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
 90 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
96 
                // determine sum of all present BL currents and setpoints
 91 
                // determine sum of all present BL currents and setpoints
97 
                Current = 0;
 92 
                Current = 0;
98 
                SetSum = 0;
 93 
                SetSum = 0;
99 
                NumOfMotors = 0;
 94 
                NumOfMotors = 0;
100 
                MinOfMaxPWM = 255;
 95 
                MinOfMaxPWM = 255;
101 
                for(i = 0; i < MAX_MOTORS; i++)
 96 
                for(i = 0; i < MAX_MOTORS; i++)
102 
                {
 97 
                {
103 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
 98 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
104 
                        {
 99 
                        {
105 
                                NumOfMotors++;
 100 
                                NumOfMotors++;
106 
                                Current += (unsigned int)(Motor[i].Current);
 101 
                                Current += (unsigned int)(Motor[i].Current);
107 
                                SetSum +=  (unsigned int)(Motor[i].SetPoint);
 102 
                                SetSum +=  (unsigned int)(Motor[i].SetPoint);
108 
                                if(Motor[i].MaxPWM < MinOfMaxPWM) MinOfMaxPWM = Motor[i].MaxPWM;
 103 
                                if(Motor[i].MaxPWM < MinOfMaxPWM) MinOfMaxPWM = Motor[i].MaxPWM;
109 
                        }
 104 
                        }
110 
                }
 105 
                }
111 
                Capacity.MinOfMaxPWM = MinOfMaxPWM;
 106 
                Capacity.MinOfMaxPWM = MinOfMaxPWM;
112 
 
 107 
 
113 
                if(SetSum == 0) // if all setpoints are 0
 108 
                if(SetSum == 0) // if all setpoints are 0
114 
                { // determine offsets of motor currents
 109 
                { // determine offsets of motor currents
115 
                        #define CURRENT_AVERAGE 8  // 8bit = 256 * 10 ms = 2.56s average time
 110 
                        #define CURRENT_AVERAGE 8  // 8bit = 256 * 10 ms = 2.56s average time
116 
                        CurrentOffset = (unsigned int)(SumCurrentOffset>>CURRENT_AVERAGE);
 111 
                        CurrentOffset = (unsigned int)(SumCurrentOffset>>CURRENT_AVERAGE);
117 
                        SumCurrentOffset -= CurrentOffset;
 112 
                        SumCurrentOffset -= CurrentOffset;
118 
                        SumCurrentOffset += Current;
 113 
                        SumCurrentOffset += Current;
119 
                        // after averaging set current to static offset
 114 
                        // after averaging set current to static offset
120 
                        Current = FC_OFFSET_CURRENT;
 115 
                        Current = FC_OFFSET_CURRENT;
121 
                }
 116 
                }
122 
                else // some motors are running, includes also motor test condition, where "MotorRunning" is false
 117 
                else // some motors are running, includes also motor test condition, where "MotorRunning" is false
123 
                {   // subtract offset
 118 
                {   // subtract offset
124 
                        if(Current > CurrentOffset) Current -= CurrentOffset;
 119 
                        if(Current > CurrentOffset) Current -= CurrentOffset;
125 
                        else Current = 0;
 120 
                        else Current = 0;
126 
                        // add the FC and BL Offsets
 121 
                        // add the FC and BL Offsets
127 
                        Current += FC_OFFSET_CURRENT + NumOfMotors * BL_OFFSET_CURRENT;
 122 
                        Current += FC_OFFSET_CURRENT + NumOfMotors * BL_OFFSET_CURRENT;
128 
                }
 123 
                }
129 
 
 124 
 
130 
                // update actual Current
 125 
                // update actual Current
131 
                Capacity.ActualCurrent = Current;
 126 
                Capacity.ActualCurrent = Current;
132 
                // update actual Power
 127 
                // update actual Power
133 
                if(Current < 255)       Capacity.ActualPower = (UBat * Current) / 100; // in W higher resolution
 128 
                if(Current < 255)       Capacity.ActualPower = (UBat * Current) / 100; // in W higher resolution
134 
                else                            Capacity.ActualPower = (UBat * (Current/4)) / 25; // in W
 129 
                else                            Capacity.ActualPower = (UBat * (Current/4)) / 25; // in W
135 
 
 130 
 
136 
                // update used capacity
 131 
                // update used capacity
137 
                SubCounter += Current;
 132 
                SubCounter += Current;
138 
 
 133 
 
139 
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
 134 
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
140 
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
 135 
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
141 
                // = 1mAh / (36000 / CAPACITY_UPDATE_INTERVAL)
 136 
                // = 1mAh / (36000 / CAPACITY_UPDATE_INTERVAL)
142 
                #define SUB_COUNTER_LIMIT (36000 / CAPACITY_UPDATE_INTERVAL)
 137 
                #define SUB_COUNTER_LIMIT (36000 / CAPACITY_UPDATE_INTERVAL)
143 
                if(SubCounter > SUB_COUNTER_LIMIT)
 138 
                if(SubCounter > SUB_COUNTER_LIMIT)
144 
                {
 139 
                {
145 
                        Capacity.UsedCapacity++;                        // we have one mAh more
 140 
                        Capacity.UsedCapacity++;                        // we have one mAh more
146 
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
 141 
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
147 
                #ifdef WITH_REMAINCAPACITY      // only include functions if DEBUG is defined in main.h
 - 
 
148 
 
 - 
 
149 
                        #warning : "### with REMAIN CAPACITY ###"
 - 
 
150 
                        Capacity.RemainCapacity=(Parameter_UserParam5*100)-Capacity.UsedCapacity; //Added by metro
 - 
 
151 
                        if((Capacity.RemainCapacity<=0)&&(Capacity.RemainCapacity%10==0)&&(Parameter_UserParam5!=0)) beeptime = 1000;
 - 
 
152 
                        if((Capacity.RemainCapacity<=500)&&(Capacity.RemainCapacity%100==0)&&(Parameter_UserParam5!=0)) beeptime = 10000;
 - 
 
153 
                #endif
 - 
 
154 
                       
 - 
 
155 
                }
 142 
                }
156 
        } // EOF check delay update timer
 143 
        } // EOF check delay update timer
157 
}
 144 
}
158 
 
 145