Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
* estimate the hover thrust value according to the battery voltage
  • Loading branch information
nieyong committed Aug 14, 2015
1 parent 420d33c commit 9bf1e83
Show file tree
Hide file tree
Showing 4 changed files with 39 additions and 9 deletions.
35 changes: 31 additions & 4 deletions Control/C/Control.c
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
5 changes: 1 addition & 4 deletions Control/H/control.h
Expand Up @@ -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;
Expand Down Expand Up @@ -71,6 +67,7 @@ void CtrlMotor(void);
void CtrlTest(void);
void CtrlAttiRateNew(void);
void CtrlAttiNew(void);
float estimateHoverThru(void);

void SetHeadFree(uint8_t on);

Expand Down
6 changes: 6 additions & 0 deletions HardWareDriver/C/Tim.c
Expand Up @@ -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 主任务频率

Expand Down Expand Up @@ -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(); //电池电压检测
Expand Down Expand Up @@ -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");
}
Expand Down
2 changes: 1 addition & 1 deletion User_Src/CommApp.c
Expand Up @@ -66,7 +66,7 @@ void RCDataProcess(void)
altCtrlMode=MANUAL; //上锁后加的处理
zIntReset=1; //
thrustZSp=0;
thrustZInt=HOVER_THRU;
thrustZInt=estimateHoverThru();
offLandFlag=0;

armState=DISARMED;
Expand Down

0 comments on commit 9bf1e83

Please sign in to comment.