Permalink
Browse files

* estimate the hover thrust value according to the battery voltage

1 parent 420d33c commit 9bf1e83f78c5f238e65faac1f0ac2cba1527cc29 @nieyong nieyong committed Aug 14, 2015
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
View
@@ -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
View
@@ -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");
}
View
@@ -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.