Permalink
Browse files

* estimate the hover thrust value according to the battery voltage

  • Loading branch information...
nieyong committed Aug 14, 2015
1 parent 420d33c commit 9bf1e83f78c5f238e65faac1f0ac2cba1527cc29
Showing with 39 additions and 9 deletions.
  1. +31 −4 Control/C/Control.c
  2. +1 −4 Control/H/control.h
  3. +6 −0 HardWareDriver/C/Tim.c
  4. +1 −1 User_Src/CommApp.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
@@ -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);
@@ -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");
}
@@ -66,7 +66,7 @@ void RCDataProcess(void)
altCtrlMode=MANUAL; //上锁后加的处理
zIntReset=1; //
thrustZSp=0;
thrustZInt=HOVER_THRU;
thrustZInt=estimateHoverThru();
offLandFlag=0;
armState=DISARMED;

0 comments on commit 9bf1e83

Please sign in to comment.