Subversion Repositories FlightCtrl

Rev

Rev 1501 | Rev 1510 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1501 Rev 1506
1 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2 
 // + Copyright (c) Holger Buss, Ingo Busker
 2 
 // + Copyright (c) Holger Buss, Ingo Busker
3 
 // + Thanks to Marcel Haller (Lion) for the nice idea and first implementation
 3 
 // + Thanks to Marcel Haller (Lion) for the nice idea and first implementation
4 
 // + Nur für den privaten Gebrauch
 4 
 // + Nur für den privaten Gebrauch
5 
 // + www.MikroKopter.com
 5 
 // + www.MikroKopter.com
6 
 // + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
 6 
 // + porting the sources to other systems or using the software on other systems (except hardware from www.mikrokopter.de) is not allowed
7 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 7 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
8 
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
 8 
 // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
9 
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
 9 
 // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
10 
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
 10 
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
11 
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
 11 
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
12 
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
 12 
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
13 
 // + Verkauf von Luftbildaufnahmen, usw.
 13 
 // + Verkauf von Luftbildaufnahmen, usw.
14 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 14 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
15 
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
 15 
 // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
16 
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
 16 
 // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
17 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 17 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
18 
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
 18 
 // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
19 
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
 19 
 // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
20 
 // + eindeutig als Ursprung verlinkt werden
 20 
 // + eindeutig als Ursprung verlinkt werden
21 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 21 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22 
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
 22 
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
23 
 // + Benutzung auf eigene Gefahr
 23 
 // + Benutzung auf eigene Gefahr
24 
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
 24 
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
25 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 25 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
26 
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
 26 
 // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
27 
 // + mit unserer Zustimmung zulässig
 27 
 // + mit unserer Zustimmung zulässig
28 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 28 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29 
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
 29 
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
30 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 30 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31 
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
 31 
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
32 
 // + this list of conditions and the following disclaimer.
 32 
 // + this list of conditions and the following disclaimer.
33 
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
 33 
 // +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
34 
 // +     from this software without specific prior written permission.
 34 
 // +     from this software without specific prior written permission.
35 
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
 35 
 // +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
36 
 // +     for non-commercial use (directly or indirectly)
 36 
 // +     for non-commercial use (directly or indirectly)
37 
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
 37 
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
38 
 // +     with our written permission
 38 
 // +     with our written permission
39 
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
 39 
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
40 
 // +     clearly linked as origin
 40 
 // +     clearly linked as origin
41 
 // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
 41 
 // +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
42 
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 42 
 // +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
43 
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 43 
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
44 
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 44 
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
45 
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 45 
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
46 
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 46 
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
47 
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 47 
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
48 
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 48 
 // +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
49 
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 49 
 // +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
50 
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 50 
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
51 
 // +  POSSIBILITY OF SUCH DAMAGE.
 51 
 // +  POSSIBILITY OF SUCH DAMAGE.
52 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 52 
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
53 
#include "capacity.h"
 53 
#include "capacity.h"
54 
#include "twimaster.h"
 54 
#include "twimaster.h"
55 
#include "main.h"
 55 
#include "main.h"
56 
#include "timer0.h"
 56 
#include "timer0.h"
57 
 
 57 
 
58 
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
 58 
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
59 
#define OFFSET_CURRENT 4  // always calculate with a Current of 0,4A
 59 
unsigned char OwnConsumptionCurrent;  // always calculate with a Current of 0,xA
- 
 
 60 
 
60 
 
 61 
 
61 
// global varialbles
 62 
// global varialbles
62 
unsigned short update_timer =  0;
 63 
unsigned short update_timer =  0;
63 
Capacity_t Capacity;
 64 
Capacity_t Capacity;
64 
 
 65 
 
65 
// initialize capacity calculation
 66 
// initialize capacity calculation
66 
void Capacity_Init(void)
 67 
void Capacity_Init(void)
67 
{
 68 
{
68 
        Capacity.ActualCurrent = 0;
 69 
        Capacity.ActualCurrent = 0;
69 
        Capacity.UsedCapacity = 0;
 70 
        Capacity.UsedCapacity = 0;
70 
 
 71 
 
71 
        update_timer = SetDelay(CAPACITY_UPDATE_INTERVAL);
 72 
        update_timer = SetDelay(CAPACITY_UPDATE_INTERVAL);
72 
}
 73 
}
73 
 
 74 
 
74 
 
 75 
 
75 
// called in main loop at a regular interval
 76 
// called in main loop at a regular interval
76 
void Capacity_Update(void)
 77 
void Capacity_Update(void)
77 
{
 78 
{
78 
        unsigned short Current, SetSum; // max value will be 255 * 12 = 3060
 79 
        unsigned short Current; // max value will be 255 * 12 = 3060
79 
        static unsigned short SubCounter = 0;
 80 
        static unsigned short SubCounter = 0;
80 
        static unsigned short CurrentOffset = 0;
 81 
        static unsigned short CurrentOffset = 0;
81 
        static unsigned long SumCurrentOffset = 0;
 82 
        static unsigned long SumCurrentOffset = 0;
82 
        unsigned char i;
 83 
        unsigned char i;
83 
 
 84 
 
84 
        if(CheckDelay(update_timer))
 85 
        if(CheckDelay(update_timer))
85 
        {
 86 
        {
86 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
 87 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
87 
 
 - 
 
88 
                // determine sum of all present BL currents and setpoints
 88 
                // determine sum of all present BL currents and setpoints
89 
                Current = 0;
 89 
                Current = 0;
90 
                SetSum = 0;
 - 
 
91 
                for(i = 0; i < MAX_MOTORS; i++)
 90 
                for(i = 0; i < MAX_MOTORS; i++)
92 
                {
 91 
                {
93 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
 92 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
94 
                        {
 93 
                        {
95 
                                Current += (int16_t)(Motor[i].Current);
 94 
                                Current += (int16_t)(Motor[i].Current);
96 
                                SetSum += (int16_t)(Motor[i].SetPoint);
 - 
 
97 
                        }
 95 
                        }
98 
                }
 96 
                }
99 
 
 - 
 
100 
                if(SetSum != 0) // if one of the present motors has a setpoint > 0
 97 
                if(MotorenEin) // if one of the present motors has a setpoint > 0
101 
                {
 98 
                {
102 
                        if(Current > CurrentOffset) Capacity.ActualCurrent = Current - CurrentOffset;
 99 
                        if(Current > CurrentOffset) Capacity.ActualCurrent = Current - CurrentOffset;
103 
                        else Capacity.ActualCurrent = 0;
 100 
                        else Capacity.ActualCurrent = 0;
104 
                        // update used capacity
 101 
                        Capacity.ActualCurrent += OwnConsumptionCurrent;
105 
                        SubCounter += Capacity.ActualCurrent;
 102 
//                      SubCounter += Capacity.ActualCurrent;
106 
 
 - 
 
107 
/*     
 - 
 
108 
                        // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
 - 
 
109 
                        // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
 - 
 
110 
                        // = 1mAh / (36000 / CAPACITY_UPDATE_INTERVAL)
 - 
 
111 
                        #define SUB_COUNTER_LIMIT (36000 / CAPACITY_UPDATE_INTERVAL)
 - 
 
112 
                        if(SubCounter > SUB_COUNTER_LIMIT)
 - 
 
113 
                        {
 - 
 
114 
                                Capacity.UsedCapacity++;                        // we have one mAh more
 - 
 
115 
                                SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
 - 
 
116 
                        }
 - 
 
117 
*/
 - 
 
118 
                }
 103 
                }
119 
                else // motors are stopped
 104 
                else // motors are stopped
120 
                { // determine offsets of motor currents
 105 
                { // determine offsets of motor currents
121 
                        Capacity.ActualCurrent = 0;
 106 
                        Capacity.ActualCurrent = STATIC_CURRENT;
122 
                        #define CURRENT_AVERAGE 256L  // 256 * 10 ms = 2.56s average time
 107 
                        #define CURRENT_AVERAGE 256L  // 256 * 10 ms = 2.56s average time
123 
                        CurrentOffset = (unsigned short)(SumCurrentOffset/CURRENT_AVERAGE);
 108 
                        CurrentOffset = (unsigned short)(SumCurrentOffset/CURRENT_AVERAGE);
124 
                        SumCurrentOffset -= CurrentOffset;
 109 
                        SumCurrentOffset -= CurrentOffset;
125 
                        SumCurrentOffset += Current;
 110 
                        SumCurrentOffset += Current;
126 
                }
 111 
                }
- 
 
 112 
                        // update used capacity
127 
                SubCounter += OFFSET_CURRENT;
 113 
                SubCounter += Capacity.ActualCurrent;
128 
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
 114 
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
129 
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
 115 
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
130 
                // = 1mAh / (36000 / CAPACITY_UPDATE_INTERVAL)
 116 
                // = 1mAh / (36000 / CAPACITY_UPDATE_INTERVAL)
131 
                #define SUB_COUNTER_LIMIT (36000 / CAPACITY_UPDATE_INTERVAL)
 117 
                #define SUB_COUNTER_LIMIT (36000 / CAPACITY_UPDATE_INTERVAL)
132 
                if(SubCounter > SUB_COUNTER_LIMIT)
 118 
                if(SubCounter > SUB_COUNTER_LIMIT)
133 
                {
 119 
                {
134 
                        Capacity.UsedCapacity++;                        // we have one mAh more
 120 
                        Capacity.UsedCapacity++;                        // we have one mAh more
135 
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
 121 
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
136 
                }
 122 
                }
137 
        } // EOF check delay update timer
 123 
        } // EOF check delay update timer
138 
}
 124 
}
139 
 
 125 
 
140 
 
 126