Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1486 killagreg 1
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
 // + Copyright (c) Holger Buss, Ingo Busker
3
 // + Thanks to Marcel Haller (Lion) for the nice idea and first implementation
4
 // + Nur für den privaten Gebrauch
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
7
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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.
10
 // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
11
 // + bzgl. der Nutzungsbedingungen aufzunehmen.
12
 // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
13
 // + Verkauf von Luftbildaufnahmen, usw.
14
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
17
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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"
20
 // + eindeutig als Ursprung verlinkt werden
21
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
22
 // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
23
 // + Benutzung auf eigene Gefahr
24
 // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
25
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
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
28
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
29
 // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
30
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
31
 // + Redistributions of source code (with or without modifications) must retain the above copyright notice,
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
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
36
 // +     for non-commercial use (directly or indirectly)
37
 // +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
38
 // +     with our written permission
39
 // +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
40
 // +     clearly linked as origin
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"
43
 // +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
44
 // +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
45
 // +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
46
 // +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
47
 // +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
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)
50
 // +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
51
 // +  POSSIBILITY OF SUCH DAMAGE.
52
 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
53
#include "capacity.h"
54
#include "twimaster.h"
55
#include "main.h"
56
#include "timer0.h"
1526 killagreg 57
#include "analog.h"
1486 killagreg 58
 
59
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
1523 holgerb 60
#define FC_OFFSET_CURRENT 5  // calculate with a current of 0.5A
1510 killagreg 61
#define BL_OFFSET_CURRENT 2  // calculate with a current of 0.2A
1486 killagreg 62
 
63
// global varialbles
64
unsigned short update_timer =  0;
65
Capacity_t Capacity;
66
 
67
// initialize capacity calculation
68
void Capacity_Init(void)
69
{
70
        Capacity.ActualCurrent = 0;
71
        Capacity.UsedCapacity = 0;
1529 killagreg 72
        Capacity.ActualPower = 0;
1672 killagreg 73
        Capacity.MinOfMaxPWM = 0;
1829 - 74
        Capacity.RemainCapacity = (EE_Parameter.UserParam1*100);
1486 killagreg 75
        update_timer = SetDelay(CAPACITY_UPDATE_INTERVAL);
76
}
77
 
78
 
79
// called in main loop at a regular interval
80
void Capacity_Update(void)
81
{
1510 killagreg 82
        unsigned short Current, SetSum; // max value will be 255 * 12 = 3060
1486 killagreg 83
        static unsigned short SubCounter = 0;
1488 killagreg 84
        static unsigned short CurrentOffset = 0;
85
        static unsigned long SumCurrentOffset = 0;
1672 killagreg 86
        unsigned char i, NumOfMotors, MinOfMaxPWM;
1486 killagreg 87
 
88
        if(CheckDelay(update_timer))
89
        {
90
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
1488 killagreg 91
                // determine sum of all present BL currents and setpoints
92
                Current = 0;
1510 killagreg 93
                SetSum = 0;
94
                NumOfMotors = 0;
1672 killagreg 95
                MinOfMaxPWM = 255;
1486 killagreg 96
                for(i = 0; i < MAX_MOTORS; i++)
97
                {
98
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
99
                        {
1510 killagreg 100
                                NumOfMotors++;
101
                                Current += (unsigned int)(Motor[i].Current);
102
                                SetSum +=  (unsigned int)(Motor[i].SetPoint);
1672 killagreg 103
                                if(Motor[i].MaxPWM < MinOfMaxPWM) MinOfMaxPWM = Motor[i].MaxPWM;
1486 killagreg 104
                        }
105
                }
1672 killagreg 106
                Capacity.MinOfMaxPWM = MinOfMaxPWM;
107
 
1510 killagreg 108
                if(SetSum == 0) // if all setpoints are 0
1486 killagreg 109
                { // determine offsets of motor currents
1510 killagreg 110
                        #define CURRENT_AVERAGE 8  // 8bit = 256 * 10 ms = 2.56s average time
111
                        CurrentOffset = (unsigned int)(SumCurrentOffset>>CURRENT_AVERAGE);
1488 killagreg 112
                        SumCurrentOffset -= CurrentOffset;
113
                        SumCurrentOffset += Current;
1510 killagreg 114
                        // after averaging set current to static offset
115
                        Current = FC_OFFSET_CURRENT;
1486 killagreg 116
                }
1510 killagreg 117
                else // some motors are running, includes also motor test condition, where "MotorRunning" is false
118
                {   // subtract offset
119
                        if(Current > CurrentOffset) Current -= CurrentOffset;
120
                        else Current = 0;
121
                        // add the FC and BL Offsets
122
                        Current += FC_OFFSET_CURRENT + NumOfMotors * BL_OFFSET_CURRENT;
123
                }
124
 
125
                // update actual Current
126
                Capacity.ActualCurrent = Current;
1526 killagreg 127
                // update actual Power
1529 killagreg 128
                if(Current < 255)       Capacity.ActualPower = (UBat * Current) / 100; // in W higher resolution
129
                else                            Capacity.ActualPower = (UBat * (Current/4)) / 25; // in W
1510 killagreg 130
 
131
                // update used capacity
132
                SubCounter += Current;
133
 
1501 holgerb 134
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
135
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
136
                // = 1mAh / (36000 / CAPACITY_UPDATE_INTERVAL)
137
                #define SUB_COUNTER_LIMIT (36000 / CAPACITY_UPDATE_INTERVAL)
138
                if(SubCounter > SUB_COUNTER_LIMIT)
139
                {
140
                        Capacity.UsedCapacity++;                        // we have one mAh more
141
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
1833 - 142
 
143
                        //Added by metro
144
                        Capacity.RemainCapacity=(EE_Parameter.UserParam1*100)-Capacity.UsedCapacity;
145
                        if((Capacity.RemainCapacity<=0)&&(Capacity.RemainCapacity%100==0)&&(EE_Parameter.UserParam1!=0))
146
                                {
147
                                        FC_StatusFlags |= FC_STATUS_LOWBAT;
148
                                        if(BeepMuster == 0xffff)
149
                                        {
150
                                                beeptime = (abs(Capacity.RemainCapacity/100)+1)*2000;
151
                                                BeepMuster = 0x0400;
152
                                        }
153
                                }
154
                                else if(!beeptime) FC_StatusFlags &= ~FC_STATUS_LOWBAT;
155
                        //End Added by metro
1501 holgerb 156
                }
1829 - 157
 
158
 
1486 killagreg 159
        } // EOF check delay update timer
160
}