Subversion Repositories FlightCtrl

#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