Subversion Repositories FlightCtrl

Rev

Rev 1486 | Rev 1501 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1486 Rev 1488
Line 72... Line 72...
72 
 
 72 
 
73 
 
 73 
 
74 
// called in main loop at a regular interval
 74 
// called in main loop at a regular interval
- 
 
 75 
void Capacity_Update(void)
75 
void Capacity_Update(void)
 76 
{
76 
{
 77 
        unsigned short Current, SetSum; // max value will be 255 * 12 = 3060
77 
        static unsigned short SubCounter = 0;
 78 
        static unsigned short SubCounter = 0;
78 
        static unsigned short SumCurrentOffset = 0;
 79 
        static unsigned short CurrentOffset = 0;
Line 79... Line 80...
79 
        unsigned short SumCurrent; // max value will be 255 * 12 = 3060
 80 
        static unsigned long SumCurrentOffset = 0;
80 
        unsigned char i;
 81 
        unsigned char i;
81 
 
 82 
 
Line 82... Line 83...
82 
        if(CheckDelay(update_timer))
 83 
        if(CheckDelay(update_timer))
83 
        {
 84 
        {
- 
 
 85 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
84 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
 86 
 
85 
 
 87 
                // determine sum of all present BL currents and setpoints
86 
                // determine sum of all present BL currents
 88 
                Current = 0;
87 
                SumCurrent = 0;
 89 
                SetSum = 0;
88 
                for(i = 0; i < MAX_MOTORS; i++)
 90 
                for(i = 0; i < MAX_MOTORS; i++)
- 
 
 91 
                {
89 
                {
 92 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
90 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
 93 
                        {
Line 91... Line 94...
91 
                        {
 94 
                                Current += (int16_t)(Motor[i].Current);
92 
                                SumCurrent += (unsigned short)(Motor[i].Current);
 95 
                                SetSum += (int16_t)(Motor[i].SetPoint);
93 
                        }
 96 
                        }
94 
                }
 97 
                }
95 
 
 98 
 
96 
                if(FCFlags & FCFLAG_MOTOR_RUN)
 99 
                if(SetSum != 0) // if one of the present motors has a setpoint > 0
- 
 
 100 
                {
97 
                {
 101 
                        if(Current > CurrentOffset) Capacity.ActualCurrent = Current - CurrentOffset;
98 
                        if(SumCurrent > SumCurrentOffset) Capacity.ActualCurrent -= SumCurrentOffset;
 102 
                        else Capacity.ActualCurrent = 0;
99 
                        else Capacity.ActualCurrent = 0;
 103 
                        // update used capacity
100 
                        // update used capacity
 104 
                        SubCounter += Capacity.ActualCurrent;
101 
                        SubCounter += Capacity.ActualCurrent;
 105 
 
Line 109... Line 113...
109 
                                SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
 113 
                                SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
110 
                        }
 114 
                        }
111 
                }
 115 
                }
112 
                else // motors are stopped
 116 
                else // motors are stopped
113 
                { // determine offsets of motor currents
 117 
                { // determine offsets of motor currents
- 
 
 118 
                        Capacity.ActualCurrent = 0;
- 
 
 119 
                        #define CURRENT_AVERAGE 256L  // 256 * 10 ms = 2.56s average time
114 
                        SumCurrentOffset = (15 * SumCurrentOffset + SumCurrent)/16; // 65535 / (12*255) = 21 for max averaging
 120 
                        CurrentOffset = (unsigned short)(SumCurrentOffset/CURRENT_AVERAGE);
- 
 
 121 
                        SumCurrentOffset -= CurrentOffset;
- 
 
 122 
                        SumCurrentOffset += Current;
115 
                }
 123 
                }
116 
 
 - 
 
117 
        } // EOF check delay update timer
 124 
        } // EOF check delay update timer
118 
}
 125 
}
- 
 
 126