repair batter 100-90-100 bug

hal/hal_battery/hal_battery_NoPowerEnPin.c

@@ -25,89 +25,87 @@ int logbuftemp_len = 0;
 char print_log = 0;
 void cb_BLE_Client_ERR(void* handle)
 {
-	DEBUG_LOG("cb_BLE_Client_ERR:%d,%d\n", 1, 1);
-	print_log = 1;
+    DEBUG_LOG("cb_BLE_Client_ERR:%d,%d\n", 1, 1);
+    print_log = 1;
 }
 
 battercb_t* battercb = NULL;
 
 void printbatter_cb(battercb_t* c, battercb_t* C_flash)
 {
-	char bytes[256];
-	int len = 0;
-	len = sprintf(bytes, "%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d\r\n",
-		c->preBestResult_Voltage2power,
-		c->preBestResult_chargeV2P_f,
-		c->P_mAh,
-		c->kg,
-		c->P2,
-		c->P1,
-		c->Battery_capacity_mAh,
-		c->adc_tp4056_power,
-		c->init,
-		c->sta,
-		c->chargeV2P_f_init,
-		c->Voltage2power_init);
-	SEGGER_RTT_Write(0, bytes, len);
-
-	len = sprintf(bytes, "%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d\r\n",
-		C_flash->preBestResult_Voltage2power,
-		C_flash->preBestResult_chargeV2P_f,
-		C_flash->P_mAh,
-		C_flash->kg,
-		C_flash->P2,
-		C_flash->P1,
-		C_flash->Battery_capacity_mAh,
-		C_flash->adc_tp4056_power,
-		C_flash->init,
-		C_flash->sta,
-		C_flash->chargeV2P_f_init,
-		C_flash->Voltage2power_init);
-
-	SEGGER_RTT_Write(0, bytes, len);
-	DEBUG_LOG("\r\n");
+    char bytes[256];
+    int len = 0;
+    len = sprintf(bytes, "%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d\r\n",
+            c->preBestResult_Voltage2power,
+            c->preBestResult_chargeV2P_f,
+            c->P_mAh,
+            c->kg,
+            c->P2,
+            c->P1,
+            c->Battery_capacity_mAh,
+            c->adc_tp4056_power,
+            c->init,
+            c->sta,
+            c->chargeV2P_f_init,
+            c->Voltage2power_init);
+    SEGGER_RTT_Write(0, bytes, len);
+    len = sprintf(bytes, "%f,%f,%f,%f,%f,%f,%f,%f,%d,%d,%d,%d\r\n",
+            C_flash->preBestResult_Voltage2power,
+            C_flash->preBestResult_chargeV2P_f,
+            C_flash->P_mAh,
+            C_flash->kg,
+            C_flash->P2,
+            C_flash->P1,
+            C_flash->Battery_capacity_mAh,
+            C_flash->adc_tp4056_power,
+            C_flash->init,
+            C_flash->sta,
+            C_flash->chargeV2P_f_init,
+            C_flash->Voltage2power_init);
+    SEGGER_RTT_Write(0, bytes, len);
+    DEBUG_LOG("\r\n");
 }
 
 void cb_init(void)
 {
-	battercb = Except_Get_Battery_Record_Buff();
-	if (battercb->init != 3)
-	{
-		battercb->init = 3;
-		battercb->P1 = 100.0f;
-		battercb->adc_tp4056_power = 0;
-		battercb->Battery_capacity_mAh = 0;
-		battercb->kg = 1;
-		battercb->P2 = 0;
-		battercb->preBestResult_chargeV2P_f = 0;
-		battercb->preBestResult_Voltage2power = 0;
-		battercb->sta = 0;
-		battercb->P_mAh = 0;
-		battercb->Voltage2power_init = 1;
-		battercb->chargeV2P_f_init = 1;
-	}
+    battercb = Except_Get_Battery_Record_Buff();
+    if (battercb->init != 3)
+    {
+        battercb->init = 3;
+        battercb->P1 = 100.0f;
+        battercb->adc_tp4056_power = 0;
+        battercb->Battery_capacity_mAh = 0;
+        battercb->kg = 1;
+        battercb->P2 = 0;
+        battercb->preBestResult_chargeV2P_f = 0;
+        battercb->preBestResult_Voltage2power = 0;
+        battercb->sta = 0;
+        battercb->P_mAh = 0;
+        battercb->Voltage2power_init = 1;
+        battercb->chargeV2P_f_init = 1;
+    }
 }
 
 
 //返回5V信号，有5V的话返回1，没有的话返回0
 static char charge_in(void)
 {
-	if (nrf_gpio_pin_read(PIN_CHARGING))
-	{
-		return 1;
-	}
-	else
-	{
-		return 0;
-	}
+    if (nrf_gpio_pin_read(PIN_CHARGING))
+    {
+        return 1;
+    }
+    else
+    {
+        return 0;
+    }
 }
 
 static float filter(float value, float kg, float* preBestResult)
 {
-	float new_v = value;
-	new_v = *preBestResult * (1.0f - kg) + value * kg;
-	*preBestResult = new_v;
-	return new_v;
+    float new_v = value;
+    new_v = *preBestResult * (1.0f - kg) + value * kg;
+    *preBestResult = new_v;
+    return new_v;
 }
 
 #if IIDUAN == 0
@@ -116,11 +114,11 @@ static float filter(float value, float kg, float* preBestResult)
 static const float poo1o[] = { 0, 0, 0.0279893723606430, 0.174605323652602, 0.325796538285416, 0.495164949358988, 0.661918800578876, 0.829024800123971, 1.00225498989324, 1.17936073685608, 1.37258677752597, 1.56525700069634, 1.78680072433224, 2.00361106262195, 2.24466616203811, 2.46699160701705, 2.77834696254638, 3.12186809827754, 3.58442625993982, 4.15025435558636, 4.75855743544068, 5.51189718744822, 6.35834306864975, 7.38461196888009, 8.48997478633724, 9.43096936165977, 10.3817319764220, 11.4116388420216, 12.3939372566211, 13.5048186806524, 14.5904959858255, 15.5237940825920, 16.4790857938893, 17.8137595522187, 18.9982251103467, 20.3392608271850, 21.5817542329461, 22.7218253119165, 23.9444316340532, 25.2939077624602, 26.6264082603126, 27.6802415218000, 29.0022881606974, 30.1783424265851, 31.1179209268523, 32.2887764986448, 33.3732790985050, 34.2380544358441, 35.2041112278740, 36.0163848326001, 36.8624779801428, 37.6634899287154, 38.5186413495501, 39.4878256764553, 40.2471232681709, 41.2081417271725, 42.3322924899204, 43.7047997876243, 44.9058976548061, 46.5044971286874, 47.8927266715832, 49.8558978793141, 51.9022338412845, 54.2586141300707, 56.3903798469888, 58.7696803719223, 60.8764981712366, 62.2358527791606, 63.8383633243999, 65.5021323737117, 67.1556090613014, 69.0159229136298, 70.1420773342446, 71.2282683025524, 72.4548338447843, 73.6556507850819, 74.8128040906371, 75.8695501837768, 77.1323517287879, 78.6365237973046, 80.3752005495001, 82.8468947240450, 86.6163997907370, 91.2910588313494, 93.9702969410882, 95.4930183746766, 96.9114001488224, 97.8493292727541, 98.7169169431273, 99.3270162091455, 99.6869018017068, 99.9917942993789, 99.9689500363163, 99.97, 99.98, 99.98, 99.98, 99.98, 99.99, 99.99, 99.99 };
 static float interp1(float x)
 {
-	int absx = (int)x - 320;
-	float max = poo1o[absx + 1];
-	float min = poo1o[absx];
-	float temp = x - (float)absx - 320.0f;
-	return (max - min) * temp + min;
+    int absx = (int)x - 320;
+    float max = poo1o[absx + 1];
+    float min = poo1o[absx];
+    float temp = x - (float)absx - 320.0f;
+    return (max - min) * temp + min;
 }
 
 #else
@@ -137,20 +135,68 @@ static float interp1(float x)
 #define BAT_10_P 345.0f
 #define BAT_0_P  320.0f
 
+#define CHA_100_P 415.0f //408
+#define CHA_90_P 412.0f //402
+#define CHA_80_P 408.0f //393
+#define CHA_70_P 403.0f //388
+#define CHA_60_P 397.0f //382
+#define CHA_50_P 387.0f //372
+#define CHA_40_P 380.0f //365
+#define CHA_30_P 377.0f //361
+#define CHA_20_P 369.0f //354
+#define CHA_10_P 360.0f //345
+#define CHA_0_P  335.0f
+
 static float interp1(float x)
 {
-	if (x > BAT_100_P)return 100.0f;
-	else if ((x <= BAT_100_P) && (x > BAT_90_P)) { return (x - BAT_90_P) / (BAT_100_P - BAT_90_P) * 10.0f + 90.0f; }
-	else if ((x <= BAT_90_P) && (x > BAT_80_P)) { return (x - BAT_80_P) / (BAT_90_P - BAT_80_P) * 10.0f + 80.0f; }
-	else if ((x <= BAT_80_P) && (x > BAT_70_P)) { return (x - BAT_70_P) / (BAT_80_P - BAT_70_P) * 10.0f + 70.0f; }
-	else if ((x <= BAT_70_P) && (x > BAT_60_P)) { return (x - BAT_60_P) / (BAT_70_P - BAT_60_P) * 10.0f + 60.0f; }
-	else if ((x <= BAT_60_P) && (x > BAT_50_P)) { return (x - BAT_50_P) / (BAT_60_P - BAT_50_P) * 10.0f + 50.0f; }
-	else if ((x <= BAT_50_P) && (x > BAT_40_P)) { return (x - BAT_40_P) / (BAT_50_P - BAT_40_P) * 10.0f + 40.0f; }
-	else if ((x <= BAT_40_P) && (x > BAT_30_P)) { return (x - BAT_30_P) / (BAT_40_P - BAT_30_P) * 10.0f + 30.0f; }
-	else if ((x <= BAT_30_P) && (x > BAT_20_P)) { return (x - BAT_20_P) / (BAT_30_P - BAT_20_P) * 10.0f + 20.0f; }
-	else if ((x <= BAT_20_P) && (x > BAT_10_P)) { return (x - BAT_10_P) / (BAT_20_P - BAT_10_P) * 10.0f + 10.0f; }
-	else if ((x <= BAT_10_P) && (x > BAT_0_P)) { return (x - BAT_0_P) / (BAT_10_P - BAT_0_P) * 10.0f + 00.0f; }
-	else return 0.0f;
+    if (x > BAT_100_P)
+    {
+        return 100.0f;
+    }
+    else if ((x <= BAT_100_P) && (x > BAT_90_P))
+    {
+        return (x - BAT_90_P) / (BAT_100_P - BAT_90_P) * 10.0f + 90.0f;
+    }
+    else if ((x <= BAT_90_P) && (x > BAT_80_P))
+    {
+        return (x - BAT_80_P) / (BAT_90_P - BAT_80_P) * 10.0f + 80.0f;
+    }
+    else if ((x <= BAT_80_P) && (x > BAT_70_P))
+    {
+        return (x - BAT_70_P) / (BAT_80_P - BAT_70_P) * 10.0f + 70.0f;
+    }
+    else if ((x <= BAT_70_P) && (x > BAT_60_P))
+    {
+        return (x - BAT_60_P) / (BAT_70_P - BAT_60_P) * 10.0f + 60.0f;
+    }
+    else if ((x <= BAT_60_P) && (x > BAT_50_P))
+    {
+        return (x - BAT_50_P) / (BAT_60_P - BAT_50_P) * 10.0f + 50.0f;
+    }
+    else if ((x <= BAT_50_P) && (x > BAT_40_P))
+    {
+        return (x - BAT_40_P) / (BAT_50_P - BAT_40_P) * 10.0f + 40.0f;
+    }
+    else if ((x <= BAT_40_P) && (x > BAT_30_P))
+    {
+        return (x - BAT_30_P) / (BAT_40_P - BAT_30_P) * 10.0f + 30.0f;
+    }
+    else if ((x <= BAT_30_P) && (x > BAT_20_P))
+    {
+        return (x - BAT_20_P) / (BAT_30_P - BAT_20_P) * 10.0f + 20.0f;
+    }
+    else if ((x <= BAT_20_P) && (x > BAT_10_P))
+    {
+        return (x - BAT_10_P) / (BAT_20_P - BAT_10_P) * 10.0f + 10.0f;
+    }
+    else if ((x <= BAT_10_P) && (x > BAT_0_P))
+    {
+        return (x - BAT_0_P) / (BAT_10_P - BAT_0_P) * 10.0f + 00.0f;
+    }
+    else
+    {
+        return 0.0f;
+    }
 }
 
 #endif
@@ -158,66 +204,92 @@ static float interp1(float x)
 //返回电压百分比
 static float Voltage2power(float mV)
 {
-	float rev = 0;
-	float k = 0;
-	if (battercb->Voltage2power_init)
-	{
-		battercb->preBestResult_Voltage2power = mV / 10;
-		battercb->Voltage2power_init = 0;
-	}
-	k = filter(mV / 10, 0.01, &battercb->preBestResult_Voltage2power);
-	if (k < 320.0f)
-	{
-		rev = 0;
-	}
-	else if (k > 420.0f)
-	{
-		rev = 100;
-	}
-	else
-	{
-		rev = interp1(k);
-	}
-	return rev;
+    float rev = 0;
+    float k = 0;
+    if (battercb->Voltage2power_init)
+    {
+        battercb->preBestResult_Voltage2power = mV / 10;
+        battercb->Voltage2power_init = 0;
+    }
+    k = filter(mV / 10, 0.01, &battercb->preBestResult_Voltage2power);
+    if (k < 320.0f)
+    {
+        rev = 0;
+    }
+    else if (k > 420.0f)
+    {
+        rev = 100;
+    }
+    else
+    {
+        rev = interp1(k);
+    }
+    return rev;
 }
 #if IIDUAN == 0
 static const float chargeV2P[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.606184684199367, 0.938307744774148, 1.30769659743727, 1.87378869014097, 2.39210582883903, 2.92471759425620, 3.79782257541779, 4.54241638183623, 5.09886597521223, 5.98930945903392, 6.49613916536905, 7.57967916311061, 8.42757721701290, 9.26589943580526, 10.3408809248851, 11.4278317273525, 12.3098089504989, 13.8095299712012, 15.4373756551577, 16.8805115746597, 18.4871688551768, 20.0578462608782, 21.6336381358003, 22.9916066660508, 24.4652374859292, 25.6565902258290, 26.8453581267414, 27.9070922567514, 28.9713295466563, 30.0209078713242, 30.9380166385130, 32.0419258329631, 33.1827748787620, 33.8691297160915, 34.8949274023278, 35.8840553861847, 36.9638277084030, 37.8687458690322, 38.9939274175310, 40.4336758333268, 41.1114865403869, 42.6160733592240, 44.2493873543177, 45.6026105469954, 47.4664557187522, 49.0734043728910, 51.2998758679562, 52.8880367841713, 54.7587352342972, 57.0178625862682, 58.8608281071146, 60.3657347075748, 62.4281274954232, 64.3086188700345, 65.8044424890286, 68.6475622104224, 72.7352289117192, 76.9893033499930, 84.0930819869950, 92.2923770276700, 96.3286261252036, 99.7542287364423 };
 static float interp1_chargeV2P(float x)
 {
-	int absx = (int)x - 350;
-	float max = chargeV2P[absx + 1];
-	float min = chargeV2P[absx];
-	float temp = x - (float)absx - 350.0f;
-	return (max - min) * temp + min;
+    int absx = (int)x - 350;
+    float max = chargeV2P[absx + 1];
+    float min = chargeV2P[absx];
+    float temp = x - (float)absx - 350.0f;
+    return (max - min) * temp + min;
 }
 #else
 
-#define CHA_100_P BAT_100_P+0.5f
-#define CHA_90_P BAT_90_P+0.5f
-#define CHA_80_P BAT_80_P+0.2f
-#define CHA_70_P BAT_70_P+0.2f
-#define CHA_60_P BAT_60_P+0.2f
-#define CHA_50_P BAT_50_P+0.2f
-#define CHA_40_P BAT_40_P+0.2f
-#define CHA_30_P BAT_30_P+0.2f
-#define CHA_20_P BAT_20_P+0.2f
-#define CHA_10_P BAT_10_P+0.2f
-#define CHA_0_P  BAT_0_P+0.2f
+
 
 static float interp1_chargeV2P(float x)
 {
-	if (x > CHA_100_P)return 100.0f;
-	else if ((x <= CHA_100_P) && (x > CHA_90_P)) { return (x - CHA_90_P) / (CHA_100_P - CHA_90_P) + 90.0f; }
-	else if ((x <= CHA_90_P) && (x > CHA_80_P)) { return (x - CHA_80_P) / (CHA_90_P - CHA_80_P) + 80.0f; }
-	else if ((x <= CHA_80_P) && (x > CHA_70_P)) { return (x - CHA_70_P) / (CHA_80_P - CHA_70_P) + 70.0f; }
-	else if ((x <= CHA_70_P) && (x > CHA_60_P)) { return (x - CHA_60_P) / (CHA_70_P - CHA_60_P) + 60.0f; }
-	else if ((x <= CHA_60_P) && (x > CHA_50_P)) { return (x - CHA_50_P) / (CHA_60_P - CHA_50_P) + 50.0f; }
-	else if ((x <= CHA_50_P) && (x > CHA_40_P)) { return (x - CHA_40_P) / (CHA_50_P - CHA_40_P) + 40.0f; }
-	else if ((x <= CHA_40_P) && (x > CHA_30_P)) { return (x - CHA_30_P) / (CHA_40_P - CHA_30_P) + 30.0f; }
-	else if ((x <= CHA_30_P) && (x > CHA_20_P)) { return (x - CHA_20_P) / (CHA_30_P - CHA_20_P) + 20.0f; }
-	else if ((x <= CHA_20_P) && (x > CHA_10_P)) { return (x - CHA_10_P) / (CHA_20_P - CHA_10_P) + 10.0f; }
-	else if ((x <= CHA_10_P) && (x > CHA_0_P)) { return (x - CHA_0_P) / (BAT_10_P - CHA_0_P) + 0.0f; }
-	else return 0.0f;
+    if (x > CHA_100_P)
+    {
+        return 100.0f;
+    }
+    else if ((x <= CHA_100_P) && (x > CHA_90_P))
+    {
+        return (x - CHA_90_P) / (CHA_100_P - CHA_90_P) * 10.0f + 90.0f;
+    }
+    else if ((x <= CHA_90_P) && (x > CHA_80_P))
+    {
+        return (x - CHA_80_P) / (CHA_90_P - CHA_80_P) * 10.0f + 80.0f;
+    }
+    else if ((x <= CHA_80_P) && (x > CHA_70_P))
+    {
+        return (x - CHA_70_P) / (CHA_80_P - CHA_70_P) * 10.0f + 70.0f;
+    }
+    else if ((x <= CHA_70_P) && (x > CHA_60_P))
+    {
+        return (x - CHA_60_P) / (CHA_70_P - CHA_60_P) * 10.0f + 60.0f;
+    }
+    else if ((x <= CHA_60_P) && (x > CHA_50_P))
+    {
+        return (x - CHA_50_P) / (CHA_60_P - CHA_50_P) * 10.0f + 50.0f;
+    }
+    else if ((x <= CHA_50_P) && (x > CHA_40_P))
+    {
+        return (x - CHA_40_P) / (CHA_50_P - CHA_40_P) * 10.0f + 40.0f;
+    }
+    else if ((x <= CHA_40_P) && (x > CHA_30_P))
+    {
+        return (x - CHA_30_P) / (CHA_40_P - CHA_30_P) * 10.0f + 30.0f;
+    }
+    else if ((x <= CHA_30_P) && (x > CHA_20_P))
+    {
+        return (x - CHA_20_P) / (CHA_30_P - CHA_20_P) * 10.0f + 20.0f;
+    }
+    else if ((x <= CHA_20_P) && (x > CHA_10_P))
+    {
+        return (x - CHA_10_P) / (CHA_20_P - CHA_10_P) * 10.0f + 10.0f;
+    }
+    else if ((x <= CHA_10_P) && (x > CHA_0_P))
+    {
+        return (x - CHA_0_P) / (BAT_10_P - CHA_0_P) * 10.0f + 0.0f;
+    }
+    else
+    {
+        return 0.0f;
+    }
 }
 
 #endif
@@ -225,268 +297,248 @@ static float interp1_chargeV2P(float x)
 
 static float chargeV2P_f(float mV)
 {
-	float rev = 0;
-	float k = 0;
-	if (battercb->chargeV2P_f_init)
-	{
-		battercb->preBestResult_chargeV2P_f = mV / 10;
-		battercb->chargeV2P_f_init = 0;
-	}
-	k = filter(mV / 10, 0.05, &battercb->preBestResult_chargeV2P_f);
-	if (k < 350.0f)
-	{
-		rev = 0;
-	}
-	else if (k > 415.0f)
-	{
-		rev = 100;
-	}
-	else
-	{
-		rev = interp1_chargeV2P(k);
-	}
-	return rev;
+    float rev = 0;
+    float k = 0;
+    if (battercb->chargeV2P_f_init)
+    {
+        battercb->preBestResult_chargeV2P_f = mV / 10;
+        battercb->chargeV2P_f_init = 0;
+    }
+    k = filter(mV / 10, 0.05, &battercb->preBestResult_chargeV2P_f);
+    if (k < 350.0f)
+    {
+        rev = 0;
+    }
+    else if (k > 415.0f)
+    {
+        rev = 100;
+    }
+    else
+    {
+        rev = interp1_chargeV2P(k);
+    }
+    return rev;
 }
 
 //返回电量百分比
 static float Voltage2mah(float mah, float storage_capacity)
 {
-	return mah / storage_capacity * 100.0f;
+    return mah / storage_capacity * 100.0f;
 }
 //返回电压剩余绝对容量
 static float mah2Voltage(float P, float storage_capacity)
 {
-	return storage_capacity * P / 100.0f;
+    return storage_capacity * P / 100.0f;
 }
 
 static void Charge(float mV, float* mAh, float interval_s)
 {
-	float A = mV / 3000.0f * 1.1f;
-	float dmAh = A * 1000.0f * interval_s / 3600.0f;
-	*mAh = *mAh + dmAh;
+    float A = mV / 3000.0f * 1.1f;
+    float dmAh = A * 1000.0f * interval_s / 3600.0f;
+    *mAh = *mAh + dmAh;
 }
 
 static float Power_management(float mV_Battery, float mV_Charge)
 {
-	float storage_capacity = 350;
-
-	switch (battercb->sta)
-	{
-	case 0:
-		if (mV_Charge > 20)
-		{
-			battercb->sta = 2; //充电过程
-			battercb->P1 = chargeV2P_f(mV_Battery);
-			battercb->Battery_capacity_mAh = mah2Voltage(battercb->P1, storage_capacity);
-		}
-		else
-		{
-			battercb->P1 = Voltage2power(mV_Battery);
-			battercb->sta = 1; //放电过程
-			
-			if ( ( interp1(mV_Battery/10.0f) - battercb->P1 > 15.0f ) || ( interp1(mV_Battery/10.0f) - battercb->P1 < -15.0f ) )
-			{
-					battercb->P1 = interp1(mV_Battery/10.0f);
-			}
-			
-		}
-		break;
-	case 1://放电
-		if (mV_Charge > 20)
-		{
-			battercb->sta = 2; //充电过程
-			battercb->kg = 0;
-			battercb->Battery_capacity_mAh = mah2Voltage(battercb->P1, storage_capacity);
-		}
-		else
-		{
-			battercb->P2 = Voltage2power(mV_Battery);
-			if ( ( battercb->P2 - battercb->P1 > 15.0f ) || ( battercb->P2 - battercb->P1 < -15.0f ) )
-			{
-					battercb->P1 = battercb->P2;
-			}
-			else
-			{
-					if ( battercb->P1 > battercb->P2 )
-					{
-							battercb->P1 = battercb->P2;
-					}
-			}
-			
-			//解决充满电后充电器不拔出来的情况显示不到100%的情况
-			if (battercb->P1 > 95.0f)
-			{
-				if (charge_in())
-				{
-					battercb->P1 = 100.0f;
-					DEBUG_LOG("charge_in()\r\n");
-				}
-				else
-				{
-					DEBUG_LOG("charge_in()==%d\r\n", charge_in());
-				}
-			}
-		}
-		break;
-	case 2://充电
-		if (mV_Charge < 20)
-		{
-			battercb->sta = 1; //放电过程
-			//初始化滤波器波器
-			battercb->preBestResult_Voltage2power = mV_Battery / 10;
-#ifdef PIN_BATFULL
-			if ((battercb->P1 > 99.1f) && (charge_in()) && (nrf_gpio_pin_read(PIN_BATFULL) == 0))
-#else 
-			if ((battercb->P1 > 99.1f) && (charge_in()))
-#endif
-			{
-				battercb->P1 = 100.0f;
-			}
-		}
-		else
-		{
-			if ((battercb->P1 == 100.0f)&&( mV_Charge < 200.0f))
-			{
-				DEBUG_LOG("((battercb->P1 == 100.0f)&&( mV_Charge < 200.0f))()\r\n");
-				break;
-			}
-
-			//---------------------------------------------------
-			Charge(mV_Charge, &battercb->Battery_capacity_mAh, 1);
-			battercb->P_mAh = Voltage2mah(battercb->Battery_capacity_mAh, storage_capacity);
-			//---------------------------------------------------
-			battercb->P2 = chargeV2P_f(mV_Battery);
-
-
-			battercb->kg = mV_Charge / 1000.0f;
-			if (battercb->kg > 1.0f)
-			{
-				battercb->kg = 1.0f;
-			}
-			battercb->P2 = (1.0f - battercb->kg) * battercb->P2 + battercb->kg * battercb->P_mAh;
-
-			if (battercb->P1 < battercb->P2) //过滤刚插上充电线时候的虚低
-			{
-				battercb->P1 = battercb->P2;
-			}
-			if (battercb->P1 >= 100.0f)
-			{
-				battercb->P1 = 99.9;
-			}
-		}
-		break;
-	}
-	return battercb->P1;
+    float storage_capacity = 350;
+    switch (battercb->sta)
+    {
+        case 0:
+            if (mV_Charge > 20)
+            {
+                battercb->sta = 2; //充电过程
+                battercb->P1 = chargeV2P_f(mV_Battery);
+                battercb->Battery_capacity_mAh = mah2Voltage(battercb->P1, storage_capacity);
+            }
+            else
+            {
+                battercb->P1 = Voltage2power(mV_Battery);
+                battercb->sta = 1; //放电过程
+                if ((interp1(mV_Battery / 10.0f) - battercb->P1 > 15.0f) || (interp1(mV_Battery / 10.0f) - battercb->P1 < -15.0f))
+                {
+                    battercb->P1 = interp1(mV_Battery / 10.0f);
+                }
+            }
+            break;
+        case 1://放电
+            if (mV_Charge > 20)
+            {
+                battercb->sta = 2; //充电过程
+                battercb->kg = 0;
+                battercb->Battery_capacity_mAh = mah2Voltage(battercb->P1, storage_capacity);
+            }
+            else
+            {
+                battercb->P2 = Voltage2power(mV_Battery);
+                if ((battercb->P2 - battercb->P1 > 15.0f) || (battercb->P2 - battercb->P1 < -15.0f))
+                {
+                    battercb->P1 = battercb->P2;
+                }
+                else
+                {
+                    if (battercb->P1 > battercb->P2)
+                    {
+                        battercb->P1 = battercb->P2;
+                    }
+                }
+            }
+            break;
+        case 2://充电
+            if (mV_Charge < 20)
+            {
+                battercb->sta = 1; //放电过程
+                //初始化滤波器波器
+                battercb->preBestResult_Voltage2power = mV_Battery / 10;
+                #ifdef PIN_BATFULL
+                if ((battercb->P1 > 99.1f) && (charge_in()) && (nrf_gpio_pin_read(PIN_BATFULL) == 0))
+                #else
+                if ((battercb->P1 > 99.1f) && (charge_in()))
+                #endif
+                {
+                    battercb->P1 = 100.0f;
+                }
+            }
+            else
+            {
+                if ((battercb->P1 > 99.9f))
+                {
+                    DEBUG_LOG("((battercb->P1 == 100.0f)&&( mV_Charge < 200.0f))()\r\n");
+                    break;
+                }
+                //---------------------------------------------------
+                Charge(mV_Charge, &battercb->Battery_capacity_mAh, 1);
+                battercb->P_mAh = Voltage2mah(battercb->Battery_capacity_mAh, storage_capacity);
+                //---------------------------------------------------
+                battercb->P2 = chargeV2P_f(mV_Battery);
+                battercb->kg = mV_Charge / 1000.0f;
+                if (battercb->kg > 1.0f)
+                {
+                    battercb->kg = 1.0f;
+                }
+                battercb->P2 = (1.0f - battercb->kg) * battercb->P2 + battercb->kg * battercb->P_mAh;
+                if (battercb->P1 < battercb->P2) //过滤刚插上充电线时候的虚低
+                {
+                    battercb->P1 = battercb->P2;
+                }
+                if (battercb->P1 >= 100.0f)
+                {
+                    battercb->P1 = 99.9;
+                }
+            }
+            break;
+    }
+    return battercb->P1;
 }
 
 static int16_t BatadcVal = 0;
 
-int16_t hal_GetBatttery_Adc(void) 
+int16_t hal_GetBatttery_Adc(void)
 {
-	return ADC_RESULT_IN_MILLI_VOLTS(BatadcVal) * 5 / 3;
+    return ADC_RESULT_IN_MILLI_VOLTS(BatadcVal) * 5 / 3;
 }
 
 static void hal_battery_Process(void)
 {
-	static int count = 0;
-	static int adc_midal = 0;
-	static int cprign = 0;
-#if PRINTBLE
-	char buff[256];
-	unsigned char len = 0;
-#endif
-	int16_t CHARGMEASURE;
-	int16_t volTemp;
-	int16_t volTemp_CHARGMEASURE;
-	if (-1 == fml_adc_get_value(PIN_ADC_BAT_CHANNEL, &BatadcVal))return;
-	else if (0 == BatadcVal)return;
-	DEBUG_LOG("hal_GetBatttery_Adc:%d\n", BatadcVal);
-	volTemp = ADC_RESULT_IN_MILLI_VOLTS(BatadcVal) * 5 / 3; // 电池电压转换计算
-	if (-1 == fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL, &CHARGMEASURE))return;
-	volTemp_CHARGMEASURE = ADC_RESULT_IN_MILLI_VOLTS(CHARGMEASURE);// 电池电压转换计算
-
-	if (count < 10)
-	{
-		adc_midal += volTemp;
-		DEBUG_LOG(0, "N %d  %5d battery %d mV\r\n", count, adc_midal, volTemp);
-		count++;
-		return;
-	}
-	else if (count == 10)
-	{
-		Process_UpdatePeroid(hal_battery_Process, 1000);
-		volTemp = adc_midal / count;
-		DEBUG_LOG(0, "average battery %d mV  %d\r\n", volTemp, count);
-		count++;
-	}
-
-	battercb->adc_tp4056_power = Power_management((float)volTemp, (float)volTemp_CHARGMEASURE);
-#if PRINTBLE
-	len = sprintf(buff, "%4d ,%4d ,%4d,%f\r\n", TIME_GetTicks(), volTemp, volTemp_CHARGMEASURE, battercb->adc_tp4056_power);
-	send_bytes_client((unsigned char*)buff, len);
-#endif
-	if (cprign % 600 == 0)
-	{
-		log("%4d,%4d,%4d,%2.1f,%d,%2.1f,%2.1f,%2.1f\n", TIME_GetTicks(), volTemp, volTemp_CHARGMEASURE, battercb->adc_tp4056_power, battercb->sta, battercb->P_mAh, battercb->P2, battercb->kg);
-		
-		logbuftemp[logbuftemp_len] = 0;
-		Except_TxError(EXCEPT_DATA_BATTERY, logbuftemp);
-	}
-	cprign++;
-
-	if (print_log == 1)
-	{
-		log("%4d,%4d,%4d,%2.1f,%d,%2.1f,%2.1f,%2.1f\n", TIME_GetTicks(), volTemp, volTemp_CHARGMEASURE, battercb->adc_tp4056_power, battercb->sta, battercb->P_mAh, battercb->P2, battercb->kg);
-
-		logbuftemp[logbuftemp_len] = 0;
-		Except_TxError(EXCEPT_DATA_BATTERY, logbuftemp);
-		print_log = 2;
-	}
-	if (print_log == 2)
-	{
-		unsigned char length = 0;
-		while (ringframe_peek(&battlog, logbuftemp, &length) == 0)
-		{
-			if (send_bytes_client((unsigned char*)logbuftemp, length) != 0)
-			{
-				return;
-			}
-			ringframe_throw(&battlog);
-		}
-		print_log = 0;
-	}
+    static int count = 0;
+    static int adc_midal = 0;
+    static int cprign = 0;
+    #if PRINTBLE
+    char buff[256];
+    unsigned char len = 0;
+    #endif
+    int16_t CHARGMEASURE;
+    int16_t volTemp;
+    int16_t volTemp_CHARGMEASURE;
+    if (-1 == fml_adc_get_value(PIN_ADC_BAT_CHANNEL, &BatadcVal))
+    {
+        return;
+    }
+    else if (0 == BatadcVal)
+    {
+        return;
+    }
+    DEBUG_LOG("hal_GetBatttery_Adc:%d\n", BatadcVal);
+    volTemp = ADC_RESULT_IN_MILLI_VOLTS(BatadcVal) * 5 / 3; // 电池电压转换计算
+    if (-1 == fml_adc_get_value(PIN_ADC_CHARGMEASURE_CHANNEL, &CHARGMEASURE))
+    {
+        return;
+    }
+    volTemp_CHARGMEASURE = ADC_RESULT_IN_MILLI_VOLTS(CHARGMEASURE);// 电池电压转换计算
+    if (count < 10)
+    {
+        adc_midal += volTemp;
+        DEBUG_LOG(0, "N %d  %5d battery %d mV\r\n", count, adc_midal, volTemp);
+        count++;
+        return;
+    }
+    else if (count == 10)
+    {
+        Process_UpdatePeroid(hal_battery_Process, 1000);
+        volTemp = adc_midal / count;
+        DEBUG_LOG(0, "average battery %d mV  %d\r\n", volTemp, count);
+        count++;
+    }
+    battercb->adc_tp4056_power = Power_management((float)volTemp, (float)volTemp_CHARGMEASURE);
+    #if PRINTBLE
+    len = sprintf(buff, "%4d ,%4d ,%4d,%f\r\n", TIME_GetTicks(), volTemp, volTemp_CHARGMEASURE, battercb->adc_tp4056_power);
+    send_bytes_client((unsigned char*)buff, len);
+    #endif
+    if (cprign % 600 == 0)
+    {
+        log("%4d,%4d,%4d,%2.1f,%d,%2.1f,%2.1f,%2.1f\n", TIME_GetTicks(), volTemp, volTemp_CHARGMEASURE, battercb->adc_tp4056_power, battercb->sta, battercb->P_mAh, battercb->P2, battercb->kg);
+        logbuftemp[logbuftemp_len] = 0;
+        Except_TxError(EXCEPT_DATA_BATTERY, logbuftemp);
+    }
+    cprign++;
+    if (print_log == 1)
+    {
+        log("%4d,%4d,%4d,%2.1f,%d,%2.1f,%2.1f,%2.1f\n", TIME_GetTicks(), volTemp, volTemp_CHARGMEASURE, battercb->adc_tp4056_power, battercb->sta, battercb->P_mAh, battercb->P2, battercb->kg);
+        logbuftemp[logbuftemp_len] = 0;
+        Except_TxError(EXCEPT_DATA_BATTERY, logbuftemp);
+        print_log = 2;
+    }
+    if (print_log == 2)
+    {
+        unsigned char length = 0;
+        while (ringframe_peek(&battlog, logbuftemp, &length) == 0)
+        {
+            if (send_bytes_client((unsigned char*)logbuftemp, length) != 0)
+            {
+                return;
+            }
+            ringframe_throw(&battlog);
+        }
+        print_log = 0;
+    }
 }
 
 //返回的电量范围： 0~100 表示电量百分比
 uint8_t GetBatteryPersent(void)
 {
-	uint8_t persent = 0;
-
-	persent = (uint8_t)(battercb->adc_tp4056_power + 0.5f);
-
-	if (persent > 10)
-	{
-		persent = persent / 10;
-		persent = persent * 10;
-	}
-
-	if (persent > 100)
-	{
-		persent = 100;
-	}
-	else if (persent <= 0)
-	{
-		persent = 0;
-	}
-	return persent;
+    uint8_t persent = 0;
+    persent = (uint8_t)(battercb->adc_tp4056_power);
+    if (persent > 10)
+    {
+        persent = persent / 10;
+        persent = persent * 10;
+    }
+    if (persent > 100)
+    {
+        persent = 100;
+    }
+    else if (persent <= 0)
+    {
+        persent = 0;
+    }
+    return persent;
 }
 void hal_battery_init(void)
 {
-	cb_init();
-#ifdef PIN_BATFULL
-	nrf_gpio_cfg_input(PIN_BATFULL, NRF_GPIO_PIN_PULLUP);
-#endif
-	BLE_Client_Rx_Regist(BLE_ERR, cb_BLE_Client_ERR);
-	Process_Start(10, "hal_battery", hal_battery_Process);
+    cb_init();
+    #ifdef PIN_BATFULL
+    nrf_gpio_cfg_input(PIN_BATFULL, NRF_GPIO_PIN_PULLUP);
+    #endif
+    BLE_Client_Rx_Regist(BLE_ERR, cb_BLE_Client_ERR);
+    Process_Start(10, "hal_battery", hal_battery_Process);
 }