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 |