Permalink
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Browse files
* estimate the hover thrust value according to the battery voltage
- Loading branch information
Showing
with
39 additions
and
9 deletions.
-
+31
−4
Control/C/Control.c
-
+1
−4
Control/H/control.h
-
+6
−0
HardWareDriver/C/Tim.c
-
+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; |
|
|