Updated cell detection and avg of vols/amps

Changed the levels for cell-detection to avoid false detection of to
many cells. The wrong number of cells may be detected if the battery is
low on powerup. Won't change the cell count in flight if voltage drops.
Changed the averaging of volts/amps to report the average volts/amps
received through mavlink between each poll.
Added a delay to try to eliminate low battery-warnings on model powerup.

Author: chsw

MavLink_FrSkySPort/Average.ino

@@ -1,6 +1,8 @@
+
 uint16_t Volt_AverageBuffer[10]; 
 uint16_t Current_AverageBuffer[10]; 
 
+// Used to calculate an average vibration level using accelerometers
 #define accBufferSize 5
 int32_t accXBuffer[accBufferSize];
 int32_t accYBuffer[accBufferSize];
@@ -9,6 +11,103 @@ int nrSamplesX = 0;
 int nrSamplesY = 0;
 int nrSamplesZ = 0;
 
+// Used to calculate the average voltage/current between each frksy poll-request.
+// A bit overkill since we most of the time only receive one sample from mavlink between each poll.
+// voltageMinimum is used to report the lowest value received through mavlink between each poll frsky poll-request.
+uint32_t currentSum = 0;
+uint16_t currentCount = 0;
+uint32_t voltageSum = 0;
+uint16_t voltageCount = 0;
+uint16_t voltageMinimum = 0;
+
+// Don't respond to FAS/FLVSS request until it looks like the voltage received through mavlink as stabilized.
+// This is a try to eliminate most of the low voltage alarms recevied upon model power up.
+boolean voltageStabilized = false;
+uint16_t voltageLast = 0;
+
+// Store a voltage reading received through mavlink
+void storeVoltageReading(uint16_t value)
+{
+  // Try to determine if the voltage has stabilized
+  if(voltageStabilized == false)
+  {
+    // if we have a mavlink voltage, and its less then 0.5V higher then the last sample we got
+    if(value > 3000 && (value - voltageLast)<500)
+    {
+      // The voltage seems to have stabilized
+      voltageStabilized = true;
+    }
+    else
+    {
+      // Reported voltage seems to still increase. Save this sample
+      voltageLast = value; 
+    }
+    return;
+  }
+
+  // Store this reading so we can return the average if we get multiple readings between the polls
+  voltageSum += value;
+  voltageCount++;
+  // Update the minimu voltage if this is lover
+  if(voltageMinimum < 1 || value < voltageMinimum)
+    voltageMinimum = value;
+}
+
+// Store a current reading received through mavlink
+void storeCurrentReading(uint16_t value)
+{
+  // Only store if the voltage seems to have stabilized
+  if(!voltageStabilized)
+    return;
+  currentSum += value;
+  currentCount++;
+}
+
+// Calculates and returns the average voltage value received through mavlink since the last time this function was called.
+// After the function is called the average is cleared.
+// Return 0 if we have no voltage reading
+uint16_t readAndResetAverageVoltage()
+{
+  if(voltageCount < 1)
+    return 0;
+  debugSerial.print(millis());
+  debugSerial.print("\tNumber of samples for voltage average: ");
+  debugSerial.print(voltageCount);
+  debugSerial.println();      
+
+  uint16_t avg = voltageSum / voltageCount;
+
+  voltageSum = 0;
+  voltageCount = 0;
+
+  return avg;
+}
+
+// Return the lowest voltage reading received through mavlink since the last time function was called.
+// After the function is called the value is cleard.
+// Return 0 if we have no new reading
+uint16_t readAndResetMinimumVoltage()
+{
+  uint16_t tmp = voltageMinimum;
+  voltageMinimum = 0;
+  return tmp;  
+}
+
+// Calculates and returns the average current value received through mavlink since the last time this function was called.
+// After the function is called the average is cleared.
+// Return 0 if we have no voltage reading
+uint16_t readAndResetAverageCurrent()
+{
+  if(currentCount < 1)
+    return 0;
+  uint16_t avg = currentSum / currentCount;
+
+  currentSum = 0;
+  currentCount = 0;
+
+  return avg;
+}
+
 //returns the average of Voltage for the 10 last values  
 uint32_t Get_Volt_Average(uint16_t value)  {
   uint8_t i;
@@ -130,3 +229,6 @@ int32_t fetchAccZ()
 }
 
 
+
+
+

MavLink_FrSkySPort/FrSkySPort.ino

@@ -50,6 +50,9 @@ void FrSkySPort_Process(void) {
 }
 
 // ***********************************************************************
+uint16_t sendValueFlvssVoltage = 0;
+uint16_t sendValueFASCurrent = 0;
+uint16_t sendValueFASVoltage = 0;
 void FrSkySPort_ProcessSensorRequest(uint8_t sensorId) 
 {
   uint32_t temp=0;
@@ -59,29 +62,41 @@ void FrSkySPort_ProcessSensorRequest(uint8_t sensorId)
   case SENSOR_ID_FLVSS:
     {
       printDebugPackageSend("FLVSS", nextFLVSS+1, 3);
+      // We need cells to continue
+      if(ap_cell_count < 1)
+        break;
+      // Make sure all the cells gets updated from the same voltage average
+      if(nextFLVSS == 0)
+      {
+        sendValueFlvssVoltage = readAndResetMinimumVoltage();  
+      }
+      // Only respond to request if we have a value
+      if(sendValueFlvssVoltage < 1)
+        break; 
+
       switch(nextFLVSS)
       {
       case 0:
         if(ap_cell_count > 0) 
         {
           // First 2 cells
           offset = 0x00 | ((ap_cell_count & 0xF)<<4);
-          temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
+          temp=((sendValueFlvssVoltage/(ap_cell_count * 2)) & 0xFFF);
           FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset);  // Battery cell 0,1
         }
         break;
       case 1:    
         // Optional 3 and 4 Cells
         if(ap_cell_count > 2) {
           offset = 0x02 | ((ap_cell_count & 0xF)<<4);
-          temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
+          temp=((sendValueFlvssVoltage/(ap_cell_count * 2)) & 0xFFF);
           FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset);  // Battery cell 2,3
         }
         break;
       case 2:    // Optional 5 and 6 Cells
         if(ap_cell_count > 4) {
           offset = 0x04 | ((ap_cell_count & 0xF)<<4);
-          temp=((ap_voltage_battery/(ap_cell_count * 2)) & 0xFFF);
+          temp=((sendValueFlvssVoltage/(ap_cell_count * 2)) & 0xFFF);
           FrSkySPort_SendPackage(FR_ID_CELLS,(temp << 20) | (temp << 8) | offset);  // Battery cell 2,3
         }
         break;     
@@ -110,13 +125,20 @@ void FrSkySPort_ProcessSensorRequest(uint8_t sensorId)
   case SENSOR_ID_FAS:
     {
       printDebugPackageSend("FAS", nextFAS+1, 2);
+      if(nextFAS == 0)
+      {
+        sendValueFASVoltage = readAndResetAverageVoltage();
+        sendValueFASCurrent = readAndResetAverageCurrent();  
+      }
+      if(sendValueFASVoltage < 1)
+        break;
       switch(nextFAS)
       {
       case 0:
-        FrSkySPort_SendPackage(FR_ID_CURRENT,ap_current_battery / 10);
+        FrSkySPort_SendPackage(FR_ID_VFAS,sendValueFASVoltage/10); // Sends voltage as a VFAS value
         break;
       case 1:
-        FrSkySPort_SendPackage(FR_ID_VFAS,ap_voltage_battery/10); // Sends voltage as a VFAS value
+        FrSkySPort_SendPackage(FR_ID_CURRENT, sendValueFASCurrent / 10);
         break;
       }
       if(++nextFAS > 1)
@@ -203,6 +225,7 @@ void FrSkySPort_ProcessSensorRequest(uint8_t sensorId)
     debugSerial.print(sensorId, HEX);
     debugSerial.println();      
 #endif
+    ;
   }
 }
 
@@ -266,4 +289,3 @@ void FrSkySPort_SendPackage(uint16_t id, uint32_t value) {
 
   digitalWrite(led,LOW);
 }
-

MavLink_FrSkySPort/MavLink_FrSkySPort.ino

@@ -52,11 +52,11 @@ APM2.5 Mavlink to FrSky X8R SPort interface using Teensy 3.1  http://www.pjrc.co
 #define START                   1
 #define MSG_RATE            10              // Hertz
 
-#define DEBUG_VFR_HUD
-#define DEBUG_GPS_RAW
-#define DEBUG_ACC
-#define DEBUG_BAT
-#define DEBUG_FRSKY_SENSOR_REQUEST
+//#define DEBUG_VFR_HUD
+//#define DEBUG_GPS_RAW
+//#define DEBUG_ACC
+//#define DEBUG_BAT
+//#define DEBUG_FRSKY_SENSOR_REQUEST
 
 
 // ******************************************
@@ -197,6 +197,8 @@ void _MavLink_receive() {
         ap_voltage_battery = Get_Volt_Average(mavlink_msg_sys_status_get_voltage_battery(&msg));  // 1 = 1mV
         ap_current_battery = Get_Current_Average(mavlink_msg_sys_status_get_current_battery(&msg));     // 1=10mA
 
+        storeVoltageReading(ap_voltage_battery);
+        storeCurrentReading(ap_current_battery);
 #ifdef DEBUG_BAT
         debugSerial.print(millis());
         debugSerial.print("\tMAVLINK_MSG_ID_SYS_STATUS: voltage_battery: ");
@@ -205,13 +207,15 @@ void _MavLink_receive() {
         debugSerial.print(mavlink_msg_sys_status_get_current_battery(&msg));
         debugSerial.println();
 #endif
-
-        if(ap_voltage_battery > 21000) ap_cell_count = 6;
-        else if (ap_voltage_battery > 16800 && ap_cell_count != 6) ap_cell_count = 5;
-        else if(ap_voltage_battery > 12600 && ap_cell_count != 5) ap_cell_count = 4;
-        else if(ap_voltage_battery > 8400 && ap_cell_count != 4) ap_cell_count = 3;
-        else if(ap_voltage_battery > 4200 && ap_cell_count != 3) ap_cell_count = 2;
-        else ap_cell_count = 0;
+        uint8_t temp_cell_count;
+        if(ap_voltage_battery > 21000) temp_cell_count = 6;
+        else if (ap_voltage_battery > 17500) temp_cell_count = 5;
+        else if(ap_voltage_battery > 12750) temp_cell_count = 4;
+        else if(ap_voltage_battery > 8500) temp_cell_count = 3;
+        else if(ap_voltage_battery > 4250) temp_cell_count = 2;
+        else temp_cell_count = 0;
+        if(temp_cell_count > ap_cell_count)
+          ap_cell_count = temp_cell_count;
         break;
       case MAVLINK_MSG_ID_GPS_RAW_INT:   // 24
         ap_fixtype = mavlink_msg_gps_raw_int_get_fix_type(&msg);                               // 0 = No GPS, 1 =No Fix, 2 = 2D Fix, 3 = 3D Fix