| 0,0 → 1,124 |
|---|
| #include <inttypes.h> |
| #include "CapCalc.h" |
| #include "twimaster.h" // for Motor |
| #include "uart0.h" // for DebugOut |
| #include "fc.h" // for FCParam |
| #include "timer0.h" // for BeepTime |
| |
| uint32_t ulUsedCap = 0;// [0.1A * 10ms] |
| uint32_t ulAvailCap = 0;// [0.1A * 10ms] |
| uint8_t uchIx; |
| uint16_t usActCurrent;// [0,1A] |
| uint16_t usTimer; |
| uint8_t uchBeepRythm; |
| |
| uint16_t usCurrentOffset; // |
| uint8_t uchCurrentOffsCalcIx; |
| |
| // initializes the Capacity Calculation |
| void CapCalc_Init(void) |
| { |
| ulUsedCap = 0; |
| uchCurrentOffsCalcIx = 0; |
| usCurrentOffset = 0; |
| uchBeepRythm = 3; |
| } |
| |
| |
| // called in main loop every 2ms |
| void CapCalc_Update(void) |
| { |
| static int8_t delay = 0; |
| |
| if(!delay--) // 10 ms intervall |
| { |
| delay = 4; |
| |
| // calculate actual Current of the Motors |
| usActCurrent = 0; |
| for (uchIx = 0; uchIx < 4; uchIx++) |
| { |
| usActCurrent += Motor[uchIx].Current; |
| } |
| |
| // find out the Curret Offset of the BL-Ctrl (mean of 50 Measurements) |
| // the BL_Ctrl's measure a current of about 1.2A while Motors are off |
| if ( (uchCurrentOffsCalcIx <= 100) // Currentoffset of Motors not jet found |
| && (Motor[0].Current && Motor[1].Current && Motor[2].Current && Motor[3].Current) // all Motors have updated Current (!= 0) |
| &&!(MKFlags & MKFLAG_MOTOR_RUN)) // Motor Off |
| { |
| uchCurrentOffsCalcIx++; |
| // ignore fist 50 times (currents are not Stable) |
| if ((uchCurrentOffsCalcIx >= 49)&&(uchCurrentOffsCalcIx < 100)) |
| { |
| usCurrentOffset += usActCurrent; |
| } |
| else if (uchCurrentOffsCalcIx == 100) |
| { |
| usCurrentOffset = usCurrentOffset/50; |
| } |
| } |
| else // got currentOffse And/Or Motor Run |
| { |
| if (usActCurrent >= usCurrentOffset) // don't get negative Values in a unsigned datatype |
| { |
| usActCurrent -= usCurrentOffset; |
| } |
| else |
| { |
| usActCurrent = 0; |
| } |
| } |
| |
| // add 10% to the actual Current (measurement Error of the BL-Ctrls) |
| usActCurrent += usActCurrent/10; |
| |
| // Add Current for FC, LED, ... |
| usActCurrent += FCParam.UserParam5; |
| |
| // Add Current for J16 |
| if (PORTC & (1<<PORTC2)) |
| { |
| usActCurrent += FCParam.UserParam6; |
| } |
| |
| // add actual Current to used Capacity |
| ulUsedCap += usActCurrent; |
| |
| // assign Debug Values |
| DebugOut.Analog[22] = usActCurrent; |
| DebugOut.Analog[23] = ulUsedCap / 3600;// used Capacity [mAh] |
| DebugOut.Analog[24] = usCurrentOffset; |
| |
| // calculate LimitValue |
| //[0.1A * 10ms] = Capacity P4[0.1Ah] *3600[s/h] *100[10ms/s] = [A ms] |
| ulAvailCap = (uint32_t)FCParam.UserParam4 * 3600 * 100; |
| |
| // check Limits and do Beep |
| if ((FCParam.UserParam4 >0)&&(ulUsedCap*10 > ulAvailCap*8 )) //Avaliable Capacity configured AND 80% reached |
| { |
| if (BeepModulation == 0xFFFF)// no other Beep |
| { |
| if(CheckDelay(usTimer)) |
| { |
| BeepTime = 500; //50ms |
| usTimer = SetDelay(100); // every 100 ms |
| if (!uchBeepRythm--) |
| { |
| uchBeepRythm=3; |
| if (ulUsedCap*10 > ulAvailCap*9) // 90% reached (beep fast) |
| { |
| usTimer = SetDelay(2000); // 2s |
| } |
| else // 80% reached (beep slow) |
| { |
| usTimer = SetDelay(10000); // 10s |
| } |
| } // if beepRathm |
| } // if CheckDelay |
| } // if no other Beep |
| } // if time to beep |
| } // if 10ms Interval |
| |
| } // CapCalc_Update |
| |