//函数名：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 > 4.05){
hoverHru = -0.35f;
}else if(Battery.BatteryVal > 3.90){
hoverHru = -0.40f;
}else if(Battery.BatteryVal > 3.80){
hoverHru = -0.45f;
}else if(Battery.BatteryVal > 3.70){
hoverHru = -0.50f;
}else{
hoverHru = -0.55f;
}
// if(Battery.BatteryVal > 4.05){
// hoverHru = -0.05f;
// }else if(Battery.BatteryVal > 3.90){
// hoverHru = -0.10f;
// }else if(Battery.BatteryVal > 3.80){
// hoverHru = -0.15f;
// }else if(Battery.BatteryVal > 3.70){
// hoverHru = -0.20f;
// }else{
// hoverHru = -0.25f;
// }
return hoverHru;
}
//检测电池电压
void BatteryCheck(void)
{
Battery.BatteryAD = GetBatteryAD(); //电池电压检测
Battery.BatteryVal = Battery.Bat_K * (Battery.BatteryAD/4096.0) * Battery.ADRef;//实际电压 值计算
if(FLY_ENABLE)
{
if(Battery.BatteryAD <= Battery.BatteryADmin)
{
Battery.alarm=1;
}
else
Battery.alarm=0;
}
else
{
if((Battery.BatteryVal < BAT_ALARM_VAL)&&(Battery.BatteryVal > BAT_CHG_VAL)) //低于3.7v 且大于充电检测电压 BAT_CHG_VAL
Battery.alarm=1;
else
Battery.alarm=0;
}
if(Battery.BatteryVal < BAT_CHG_VAL) //on charge
{
Battery.chargeSta = 1;
BT_off();
}
else
Battery.chargeSta = 0;
}
void TIM3_IRQHandler(void) //打印中断服务程序
{
if( TIM_GetITStatus(TIM3 , TIM_IT_Update) != RESET )
{
Battery.BatteryAD = GetBatteryAD(); //电池电压检测
Battery.BatteryVal = Battery.Bat_K * (Battery.BatteryAD/4096.0) * Battery.ADRef;//实际电压 值计算
DebugCounter++;
if( DebugCounter==500)
{
DebugCounter=0;
printf(" ******************************************************************\r\n");
printf(" * ____ _____ +---+ *\r\n");
printf(" * / ___\\ / __ \\ | R | *\r\n");
printf(" * / / / /_/ / +---+ *\r\n");
printf(" * / / ________ ____ ___ / ____/___ ____ __ __ *\r\n");
printf(" * / / / ___/ __ `/_ / / _ \\/ / / __ \\/ _ \\/ / / / *\r\n");
printf(" * / /__/ / / /_/ / / /_/ __/ / / /_/ / / / / /__/ / *\r\n");
printf(" * \\___/_/ \\__,_/ /___/\\___/_/ \\___ /_/ /_/____ / *\r\n");
printf(" * / / *\r\n");
printf(" * ____/ / *\r\n");
printf(" * /_____/ *\r\n");
printf(" ******************************************************************\r\n");
printf("\r\n");
printf("\r\n");
printf(" Yaw ---> %5.2f degree\r\n",(float)imu.yaw);
printf(" Pitch---> %5.2f degree\r\n",(float)imu.pitch);
printf(" Roll ---> %5.2f degree\r\n",(float)imu.roll);
printf("====================================\r\n");
printf(" Motor M1 PWM---> %d\r\n",TIM2->CCR1);
printf(" Motor M2 PWM---> %d\r\n",TIM2->CCR2);
printf(" Motor M3 PWM---> %d\r\n",TIM2->CCR3);
printf(" Motor M4 PWM---> %d\r\n",TIM2->CCR4);
printf("====================================\r\n");
printf(" Pressure ---> %5.2f Pa\r\n",(float)MS5611_Pressure);
printf(" Altitude ---> %5.2f M\r\n",(float)MS5611_Altitude);
printf(" Temperature---> %5.2f C\r\n",(float)MS5611_Temperature);
printf("====================================\r\n");
//根据采集到的AD值，计算实际电压。硬件上是对电池进行分压后给AD采集的，所以结果要乘以2
printf(" Battery Voltage---> %3.2fv\r\n",Battery.BatteryVal);
printf(" RX Addr ---> 0x%x\r\n",RX_ADDRESS[4]);
printf("====================================\r\n");
}
TIM_ClearITPendingBit(TIM3 , TIM_FLAG_Update); //清除中断标志
}
}