Subversion Repositories FlightCtrl

Rev

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

Rev 1506 Rev 1510
Line 54... Line 54...
54 
#include "twimaster.h"
 54 
#include "twimaster.h"
55 
#include "main.h"
 55 
#include "main.h"
56 
#include "timer0.h"
 56 
#include "timer0.h"
Line 57... Line 57...
57 
 
 57 
 
- 
 
 58 
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
58 
#define CAPACITY_UPDATE_INTERVAL 10 // 10 ms
 59 
#define FC_OFFSET_CURRENT 4  // calculate with a current of 0.4A
Line 59... Line 60...
59 
unsigned char OwnConsumptionCurrent;  // always calculate with a Current of 0,xA
 60 
#define BL_OFFSET_CURRENT 2  // calculate with a current of 0.2A
60 
 
 61 
 
61 
 
 62 
 
Line 74... Line 75...
74 
 
 75 
 
75 
 
 76 
 
76 
// called in main loop at a regular interval
 77 
// called in main loop at a regular interval
77 
void Capacity_Update(void)
 78 
void Capacity_Update(void)
78 
{
 79 
{
79 
        unsigned short Current; // max value will be 255 * 12 = 3060
 80 
        unsigned short Current, SetSum; // max value will be 255 * 12 = 3060
80 
        static unsigned short SubCounter = 0;
 81 
        static unsigned short SubCounter = 0;
81 
        static unsigned short CurrentOffset = 0;
 82 
        static unsigned short CurrentOffset = 0;
Line 82... Line 83...
82 
        static unsigned long SumCurrentOffset = 0;
 83 
        static unsigned long SumCurrentOffset = 0;
83 
        unsigned char i;
 84 
        unsigned char i, NumOfMotors;
84 
 
 85 
 
85 
        if(CheckDelay(update_timer))
 86 
        if(CheckDelay(update_timer))
86 
        {
 87 
        {
- 
 
 88 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
- 
 
 89 
                // determine sum of all present BL currents and setpoints
87 
                update_timer += CAPACITY_UPDATE_INTERVAL; // do not use SetDelay to avoid timing leaks
 90 
                Current = 0;
88 
                // determine sum of all present BL currents and setpoints
 91 
                SetSum = 0;
89 
                Current = 0;
 92 
                NumOfMotors = 0;
90 
                for(i = 0; i < MAX_MOTORS; i++)
 93 
                for(i = 0; i < MAX_MOTORS; i++)
- 
 
 94 
                {
91 
                {
 95 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
- 
 
 96 
                        {
92 
                        if(Motor[i].State & MOTOR_STATE_PRESENT_MASK)
 97 
                                NumOfMotors++;
93 
                        {
 98 
                                Current += (unsigned int)(Motor[i].Current);
94 
                                Current += (int16_t)(Motor[i].Current);
 99 
                                SetSum +=  (unsigned int)(Motor[i].SetPoint);
95 
                        }
 - 
 
96 
                }
 - 
 
97 
                if(MotorenEin) // if one of the present motors has a setpoint > 0
 - 
 
98 
                {
 - 
 
99 
                        if(Current > CurrentOffset) Capacity.ActualCurrent = Current - CurrentOffset;
 - 
 
100 
                        else Capacity.ActualCurrent = 0;
 - 
 
101 
                        Capacity.ActualCurrent += OwnConsumptionCurrent;
 - 
 
102 
//                      SubCounter += Capacity.ActualCurrent;
 100 
                        }
103 
                }
 - 
 
104 
                else // motors are stopped
 101 
                }
105 
                { // determine offsets of motor currents
 102 
                if(SetSum == 0) // if all setpoints are 0
106 
                        Capacity.ActualCurrent = STATIC_CURRENT;
 103 
                { // determine offsets of motor currents
107 
                        #define CURRENT_AVERAGE 256L  // 256 * 10 ms = 2.56s average time
 104 
                        #define CURRENT_AVERAGE 8  // 8bit = 256 * 10 ms = 2.56s average time
- 
 
 105 
                        CurrentOffset = (unsigned int)(SumCurrentOffset>>CURRENT_AVERAGE);
- 
 
 106 
                        SumCurrentOffset -= CurrentOffset;
108 
                        CurrentOffset = (unsigned short)(SumCurrentOffset/CURRENT_AVERAGE);
 107 
                        SumCurrentOffset += Current;
- 
 
 108 
                        // after averaging set current to static offset
- 
 
 109 
                        Current = FC_OFFSET_CURRENT;
- 
 
 110 
                }
- 
 
 111 
                else // some motors are running, includes also motor test condition, where "MotorRunning" is false
- 
 
 112 
                {   // subtract offset
- 
 
 113 
                        if(Current > CurrentOffset) Current -= CurrentOffset;
- 
 
 114 
                        else Current = 0;
- 
 
 115 
                        // add the FC and BL Offsets
- 
 
 116 
                        Current += FC_OFFSET_CURRENT + NumOfMotors * BL_OFFSET_CURRENT;
- 
 
 117 
                }
- 
 
 118 
 
109 
                        SumCurrentOffset -= CurrentOffset;
 119 
                // update actual Current
110 
                        SumCurrentOffset += Current;
 120 
                Capacity.ActualCurrent = Current;
- 
 
 121 
 
111 
                }
 122 
                // update used capacity
112 
                        // update used capacity
 123 
                SubCounter += Current;
113 
                SubCounter += Capacity.ActualCurrent;
 124 
 
114 
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
 125 
                // 100mA * 1ms * CAPACITY_UPDATE_INTERVAL = 1 mA * 100 ms * CAPACITY_UPDATE_INTERVAL
115 
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
 126 
                // = 1mA * 0.1s * CAPACITY_UPDATE_INTERVAL = 1mA * 1min / (600 / CAPACITY_UPDATE_INTERVAL)
Line 120... Line 131...
120 
                        Capacity.UsedCapacity++;                        // we have one mAh more
 131 
                        Capacity.UsedCapacity++;                        // we have one mAh more
121 
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
 132 
                        SubCounter -= SUB_COUNTER_LIMIT;        // keep the remaining sub part
122 
                }
 133 
                }
123 
        } // EOF check delay update timer
 134 
        } // EOF check delay update timer
124 
}
 135 
}
125 
 
 -