* estimate the hover thrust value according to the battery voltage

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;