* estimate the hover thrust value according to the battery voltage

Author: nieyong

Control/C/Control.c

@@ -238,6 +238,32 @@ uint8_t isAltLimit=0;
 float altLand;
 //#define DEBUG_HOLD_REAL_ALT
 
+
+//函数名：estimateHoverThru()
+//输入：无
+//输出: 预估得到的悬停油门值
+//描述：预估悬停油门值，直接影响到该飞行器的z轴悬停
+//悬停油门值相关因素有：电池电压
+//Get a estimated value for hold throttle.It will have a direct affection on hover
+//Battery voltage
+float estimateHoverThru(void){
+	float hoverHru = 0.55f;
+	
+	//电池电压检测  
+	Battery.BatteryAD  = GetBatteryAD();
+	Battery.BatteryVal = Battery.Bat_K * (Battery.BatteryAD/4096.0) * Battery.ADRef;//实际电压 值计算
+	
+	if(Battery.BatteryVal > 3.9){
+		hoverHru = 0.45f;
+	}else if(Battery.BatteryVal > 3.8){
+		hoverHru = 0.52f;
+	}else{
+		hoverHru = 0.58f;
+	}
+	
+	return hoverHru;
+}
+
 //函数名：CtrlAlti()
 //输入：无
 //输出: 最终结果输出到全局变量thrustZSp
@@ -322,11 +348,12 @@ void CtrlAlti(void)
 		posZVelSp = LAND_SPEED;
 
 	//--------------pos z vel ctrl -----------//
-	if(zIntReset)		//tobe tested .  how to get hold throttle. give it a estimated value!!!!!!!!!!!
-	{
-		thrustZInt=HOVER_THRU; //-manThr;		//650/1000 = 0.65
-		zIntReset=0;
+	//get hold throttle. give it a estimated value
+	if(zIntReset){
+		thrustZInt = estimateHoverThru();
+		zIntReset = 0;
 	}
+	
 	velZ=nav.vz;	
 	velZErr = posZVelSp - velZ;
 	valZErrD = (spZMoveRate - velZ) * alt_PID.P - (velZ - velZPrev) / dt;	//spZMoveRate is from manual stick vel control

Control/H/control.h

@@ -18,10 +18,6 @@
 #define ALT_VEL_MAX 					4.0f
 #define THR_MIN								0.38f		//min thrust ，根据机重和最小降速而定，用于下降速度过大时，油门过小，导致失衡。再增加fuzzy control ，在油门小时用更大的姿态参数
 
-
-#define HOVER_THRU	         -0.55  //-0.5f  //悬停
-
-
 enum {CLIMB_RATE=0,MANUAL,LANDING};
 extern uint8_t altCtrlMode;
 extern float hoverThrust;
@@ -71,6 +67,7 @@ void CtrlMotor(void);
 void CtrlTest(void);
 void CtrlAttiRateNew(void);
 void CtrlAttiNew(void);
+float estimateHoverThru(void);
 
 void SetHeadFree(uint8_t on);
 

HardWareDriver/C/Tim.c

@@ -23,6 +23,7 @@ Tim.c file
 #include "tim.h"
 #include "config.h"
 #include "imu.h"
+#include "control.h"
 
 #define TASK_TICK_FREQ				1000			//Hz 主任务频率
 
@@ -74,6 +75,8 @@ extern u8 RX_ADDRESS[5];
 
 void TIM3_IRQHandler(void)		//打印中断服务程序
 {	
+		float hoverThru;
+	
     if( TIM_GetITStatus(TIM3 , TIM_IT_Update) != RESET ) 
     {     
 			Battery.BatteryAD  = GetBatteryAD();            //电池电压检测  
@@ -111,6 +114,9 @@ void TIM3_IRQHandler(void)		//打印中断服务程序
 						printf("====================================\r\n");
 						//根据采集到的AD值，计算实际电压。硬件上是对电池进行分压后给AD采集的，所以结果要乘以2
             printf(" Battery Voltage---> %3.2fv\r\n",Battery.BatteryVal);
+						
+						hoverThru = estimateHoverThru();
+						printf(" Hover Thru---> %3.2f\r\n",hoverThru);
 						printf(" RX Addr ---> 0x%x\r\n",RX_ADDRESS[4]);
             printf("====================================\r\n");
         }

User_Src/CommApp.c

@@ -66,7 +66,7 @@ void RCDataProcess(void)
 			altCtrlMode=MANUAL;		//上锁后加的处理
 			 zIntReset=1;		//
 			 thrustZSp=0;	
-			 thrustZInt=HOVER_THRU;
+			 thrustZInt=estimateHoverThru();
 			 offLandFlag=0;
 
 			armState=DISARMED;