Rev 1342 | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 1342 | Lion | 1 | #include <inttypes.h> |
| 2 | #include "CapCalc.h" |
||
| 3 | #include "twimaster.h" // for Motor |
||
| 4 | #include "uart0.h" // for DebugOut |
||
| 5 | #include "fc.h" // for FCParam |
||
| 6 | #include "timer0.h" // for BeepTime |
||
| 7 | |||
| 8 | uint32_t ulUsedCap = 0;// [0.1A * 10ms] |
||
| 9 | uint32_t ulAvailCap = 0;// [0.1A * 10ms] |
||
| 10 | uint8_t uchIx; |
||
| 11 | uint16_t usActCurrent;// [0,1A] |
||
| 12 | uint16_t usTimer; |
||
| 13 | uint8_t uchBeepRythm; |
||
| 14 | |||
| 15 | uint16_t usCurrentOffset; // |
||
| 16 | uint8_t uchCurrentOffsCalcIx; |
||
| 17 | |||
| 18 | // initializes the Capacity Calculation |
||
| 19 | void CapCalc_Init(void) |
||
| 20 | { |
||
| 21 | ulUsedCap = 0; |
||
| 22 | uchCurrentOffsCalcIx = 0; |
||
| 23 | usCurrentOffset = 0; |
||
| 24 | uchBeepRythm = 3; |
||
| 25 | } |
||
| 26 | |||
| 27 | |||
| 28 | // called in main loop every 2ms |
||
| 29 | void CapCalc_Update(void) |
||
| 30 | { |
||
| 31 | static int8_t delay = 0; |
||
| 32 | |||
| 33 | if(!delay--) // 10 ms intervall |
||
| 34 | { |
||
| 35 | delay = 4; |
||
| 36 | |||
| 37 | // calculate actual Current of the Motors |
||
| 38 | usActCurrent = 0; |
||
| 39 | for (uchIx = 0; uchIx < 4; uchIx++) |
||
| 40 | { |
||
| 41 | usActCurrent += Motor[uchIx].Current; |
||
| 42 | } |
||
| 43 | |||
| 44 | // find out the Curret Offset of the BL-Ctrl (mean of 50 Measurements) |
||
| 45 | // the BL_Ctrl's measure a current of about 1.2A while Motors are off |
||
| 46 | if ( (uchCurrentOffsCalcIx <= 100) // Currentoffset of Motors not jet found |
||
| 47 | && (Motor[0].Current && Motor[1].Current && Motor[2].Current && Motor[3].Current) // all Motors have updated Current (!= 0) |
||
| 48 | &&!(MKFlags & MKFLAG_MOTOR_RUN)) // Motor Off |
||
| 49 | { |
||
| 50 | uchCurrentOffsCalcIx++; |
||
| 51 | // ignore fist 50 times (currents are not Stable) |
||
| 52 | if ((uchCurrentOffsCalcIx >= 49)&&(uchCurrentOffsCalcIx < 100)) |
||
| 53 | { |
||
| 54 | usCurrentOffset += usActCurrent; |
||
| 55 | } |
||
| 56 | else if (uchCurrentOffsCalcIx == 100) |
||
| 57 | { |
||
| 58 | usCurrentOffset = usCurrentOffset/50; |
||
| 59 | } |
||
| 60 | } |
||
| 61 | else // got currentOffse And/Or Motor Run |
||
| 62 | { |
||
| 63 | if (usActCurrent >= usCurrentOffset) // don't get negative Values in a unsigned datatype |
||
| 64 | { |
||
| 65 | usActCurrent -= usCurrentOffset; |
||
| 66 | } |
||
| 67 | else |
||
| 68 | { |
||
| 69 | usActCurrent = 0; |
||
| 70 | } |
||
| 71 | } |
||
| 72 | |||
| 73 | // add 10% to the actual Current (measurement Error of the BL-Ctrls) |
||
| 74 | usActCurrent += usActCurrent/10; |
||
| 75 | |||
| 76 | // Add Current for FC, LED, ... |
||
| 77 | usActCurrent += FCParam.UserParam5; |
||
| 78 | |||
| 79 | // Add Current for J16 |
||
| 80 | if (PORTC & (1<<PORTC2)) |
||
| 81 | { |
||
| 82 | usActCurrent += FCParam.UserParam6; |
||
| 83 | } |
||
| 84 | |||
| 85 | // add actual Current to used Capacity |
||
| 86 | ulUsedCap += usActCurrent; |
||
| 87 | |||
| 88 | // assign Debug Values |
||
| 89 | DebugOut.Analog[22] = usActCurrent; |
||
| 90 | DebugOut.Analog[23] = ulUsedCap / 3600;// used Capacity [mAh] |
||
| 91 | DebugOut.Analog[24] = usCurrentOffset; |
||
| 92 | |||
| 93 | // calculate LimitValue |
||
| 94 | //[0.1A * 10ms] = Capacity P4[0.1Ah] *3600[s/h] *100[10ms/s] = [A ms] |
||
| 95 | ulAvailCap = (uint32_t)FCParam.UserParam4 * 3600 * 100; |
||
| 96 | |||
| 97 | // check Limits and do Beep |
||
| 98 | if ((FCParam.UserParam4 >0)&&(ulUsedCap*10 > ulAvailCap*8 )) //Avaliable Capacity configured AND 80% reached |
||
| 99 | { |
||
| 100 | if (BeepModulation == 0xFFFF)// no other Beep |
||
| 101 | { |
||
| 102 | if(CheckDelay(usTimer)) |
||
| 103 | { |
||
| 104 | BeepTime = 500; //50ms |
||
| 105 | usTimer = SetDelay(100); // every 100 ms |
||
| 106 | if (!uchBeepRythm--) |
||
| 107 | { |
||
| 108 | uchBeepRythm=3; |
||
| 109 | if (ulUsedCap*10 > ulAvailCap*9) // 90% reached (beep fast) |
||
| 110 | { |
||
| 111 | usTimer = SetDelay(2000); // 2s |
||
| 112 | } |
||
| 113 | else // 80% reached (beep slow) |
||
| 114 | { |
||
| 115 | usTimer = SetDelay(10000); // 10s |
||
| 116 | } |
||
| 117 | } // if beepRathm |
||
| 118 | } // if CheckDelay |
||
| 119 | } // if no other Beep |
||
| 120 | } // if time to beep |
||
| 121 | } // if 10ms Interval |
||
| 122 | |||
| 123 | } // CapCalc_Update |
||
| 124 |