// input: rcData , raw data from remote control source
// output: RC_DATA, desired thro, pitch, roll, yaw
void RCDataProcess(void)
{
CONSTRAIN(rcData[THROTTLE],1000,2000);
CONSTRAIN(rcData[YAW],1000,2000);
CONSTRAIN(rcData[PITCH],1000,2000);
CONSTRAIN(rcData[ROLL],1000,2000);
// CUT_DB(rcData[YAW],1500,APP_YAW_DB);
// CUT_DB(rcData[PITCH],1500,APP_PR_DB);
// CUT_DB(rcData[ROLL],1500,APP_PR_DB);
RC_DATA.THROTTLE=rcData[THROTTLE]-1000;
// RC_DATA.YAW=(rcData[YAW]-1500)/500.0 *Angle_Max;
// RC_DATA.PITCH=(rcData[PITCH]-1500)/500.0 *Angle_Max; // + Pitch_error_init;
// RC_DATA.ROOL=(rcData[ROLL]-1500)/500.0 *Angle_Max; // + Rool_error_init;
RC_DATA.YAW= YAW_RATE_MAX * dbScaleLinear((rcData[YAW] - 1500),500,APP_YAW_DB);
RC_DATA.PITCH= Angle_Max * dbScaleLinear((rcData[PITCH] - 1500),500,APP_PR_DB);
RC_DATA.ROOL= Angle_Max * dbScaleLinear((rcData[ROLL] - 1500),500,APP_PR_DB);
switch(armState)
{
case REQ_ARM:
if(IMUCheck() && !Battery.alarm)
{
armState=ARMED;
FLY_ENABLE=0xA5;
}
else
{
FLY_ENABLE=0;
armState=DISARMED;
}
break;
case REQ_DISARM:
FLY_ENABLE=0;
altCtrlMode=MANUAL; //上锁后加的处理
zIntReset=1; //
thrustZSp=0;
thrustZInt=HOVER_THRU;
offLandFlag=0;
armState=DISARMED;
break;
default:
break;
}
}
//函数名：CtrlAlti()
//输入：无
//输出: 最终结果输出到全局变量thrustZSp
//描述：控制高度，也就是高度悬停控制函数
//only in climb rate mode and landind mode. now we don't work on manual mode
void CtrlAlti(void)
{
float manThr=0,alt=0,velZ=0;
float altSp=0;
float posZVelSp=0;
float altSpOffset,altSpOffsetMax=0;
float dt=0,t=0;
static float tPrev=0,velZPrev=0;
float posZErr=0,velZErr=0,valZErrD=0;
float thrustXYSpLen=0,thrustSpLen=0;
float thrustXYMax=0;
//get dt
//保证dt运算不能被打断，保持更新，否则dt过大，积分爆满。
if(tPrev==0){
tPrev=micros();
return;
}else{
t=micros();
dt=(t-tPrev) /1000000.0f;
tPrev=t;
}
//only in climb rate mode and landind mode. now we don't work on manual mode
//手动模式不使用该高度控制算法
if(MANUAL == altCtrlMode || !FLY_ENABLE){
return;
}
//--------------pos z ctrol---------------//
//get current alt
alt=-nav.z;
//get desired move rate from stick
manThr=RC_DATA.THROTTLE / 1000.0f;
spZMoveRate= -dbScaleLinear(manThr-0.5f,0.5f,ALT_CTRL_Z_DB); // scale to -1~1 . NED frame
spZMoveRate = spZMoveRate * ALT_VEL_MAX; // scale to vel min max
//get alt setpoint in CLIMB rate mode
altSp =-nav.z; //only alt is not in ned frame.
altSp -= spZMoveRate * dt;
//limit alt setpoint
altSpOffsetMax=ALT_VEL_MAX / alt_PID.P * 2.0f;
altSpOffset = altSp-alt;
if( altSpOffset > altSpOffsetMax){
altSp=alt + altSpOffsetMax;
}else if( altSpOffset < -altSpOffsetMax){
altSp=alt - altSpOffsetMax;
}
//限高
if(isAltLimit)
{
if(altSp - altLand > ALT_LIMIT)
{
altSp=altLand+ALT_LIMIT;
spZMoveRate=0;
}
}
// pid and feedforward control . in ned frame
posZErr= -(altSp - alt);
posZVelSp = posZErr * alt_PID.P + spZMoveRate * ALT_FEED_FORWARD;
//consider landing mode
if(altCtrlMode==LANDING)
posZVelSp = LAND_SPEED;
//获取一个预估的Z轴悬停基准值，相关因素有电池电压
//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
velZPrev=velZ;
thrustZSp= velZErr * alt_vel_PID.P + valZErrD * alt_vel_PID.D + thrustZInt; //in ned frame. thrustZInt contains hover thrust
//limit thrust min !!
if(altCtrlMode!=LANDING){
if (-thrustZSp < THR_MIN){
thrustZSp = -THR_MIN;
}
}
//与动力分配相关 testing
satXY=0;
satZ=0;
thrustXYSp[0]= sinf(RC_DATA.ROOL * M_PI_F /180.0f) ;//目标角度转加速度
thrustXYSp[1]= sinf(RC_DATA.PITCH * M_PI_F /180.0f) ; //归一化
thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);
//limit tilt max
if(thrustXYSpLen >0.01f )
{
thrustXYMax=-thrustZSp * tanf(TILT_MAX);
if(thrustXYSpLen > thrustXYMax)
{
float k=thrustXYMax / thrustXYSpLen;
thrustXYSp[1] *= k;
thrustXYSp[0] *= k;
satXY=1;
thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);
}
}
//limit max thrust!!
thrustSpLen=sqrtf(thrustXYSpLen * thrustXYSpLen + thrustZSp * thrustZSp);
if(thrustSpLen > THR_MAX)
{
if(thrustZSp < 0.0f) //going up
{
if (-thrustZSp > THR_MAX)
{
/* thrust Z component is too large, limit it */
thrustXYSp[0] = 0.0f;
thrustXYSp[1] = 0.0f;
thrustZSp = -THR_MAX;
satXY = 1;
satZ = 1;
}
else {
float k = 0;
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
thrustXYMax = sqrtf(THR_MAX * THR_MAX- thrustZSp * thrustZSp);
k=thrustXYMax / thrustXYSpLen;
thrustXYSp[1] *=k;
thrustXYSp[0] *= k;
satXY=1;
}
}
else { //going down
/* Z component is negative, going down, simply limit thrust vector */
float k = THR_MAX / thrustSpLen;
thrustZSp *= k;
thrustXYSp[1] *=k;
thrustXYSp[0] *= k;
satXY = 1;
satZ = 1;
}
}
rollSp= asinf(thrustXYSp[0]) * 180.0f /M_PI_F;
pitchSp = asinf(thrustXYSp[1]) * 180.0f /M_PI_F;
// if saturation ,don't integral
if(!satZ )//&& fabs(thrustZSp)<THR_MAX
{
thrustZInt += velZErr * alt_vel_PID.I * dt;
if (thrustZInt > 0.0f)
thrustZInt = 0.0f;
}
}
//函数名：CtrlAlti()
//输入：无
//输出: 最终结果输出到全局变量thrustZSp //The end result is output to the global variable thrustZSp
//描述：控制高度，也就是高度悬停控制函数 //ontrol the height , that is the height of hovering control function
//only in climb rate mode and landind mode. now we don't work on manual mode
void CtrlAlti(void)
{
float manThr=0,alt=0,velZ=0;
float altSp=0;
float posZVelSp=0;
float altSpOffset,altSpOffsetMax=0;
float dt=0,t=0;
static float tPrev=0,velZPrev=0;
float posZErr=0,velZErr=0,valZErrD=0;
float thrustXYSpLen=0,thrustSpLen=0;
float thrustXYMax=0;
//get dt //保证dt运算不能被打断，保持更新，否则dt过大，积分爆满。//Ensure dt operation can not be interrupted , kept up to date , otherwise dt too large, full integration
if(tPrev==0){
tPrev=micros();
return;
}else{
t=micros();
dt=(t-tPrev) /1000000.0f;
tPrev=t;
}
if(altCtrlMode==MANUAL || !FLY_ENABLE)
return;
//--------------pos z ctrol---------------//
//get current alt
alt=-nav.z;
//get desired move rate from stick
manThr=RC_DATA.THROTTLE / 1000.0f;
spZMoveRate= -dbScaleLinear(manThr-0.5f,0.5f,ALT_CTRL_Z_DB); // scale to -1~1 . NED frame
spZMoveRate = spZMoveRate * ALT_VEL_MAX; // scale to vel min max
#ifdef DEBUG_HOLD_REAL_ALT
if(spZMoveRate==0)
{
if(!recAltFlag)
{
holdAlt=alt;
recAltFlag=1;
}
altSp=holdAlt;
}
else
{
recAltFlag=0;
#endif
//get alt setpoint in CLIMB rate mode
altSp =-nav.z; //only alt is not in ned frame.
altSp -= spZMoveRate * dt;
//limit alt setpoint
altSpOffsetMax=ALT_VEL_MAX / alt_PID.P * 2.0f;
altSpOffset = altSp-alt;
if( altSpOffset > altSpOffsetMax) //or alt - alt > altSpOffsetMax
altSp=alt + altSpOffsetMax;
else if( altSpOffset < -altSpOffsetMax)
altSp=alt - altSpOffsetMax;
#ifdef DEBUG_HOLD_REAL_ALT
}
#endif
//限高
if(isAltLimit)
{
if(altSp - altLand > ALT_LIMIT)
{
altSp=altLand+ALT_LIMIT;
spZMoveRate=0;
}
}
// pid and feedforward control . in ned frame
posZErr= -(altSp - alt);
posZVelSp = posZErr * alt_PID.P + spZMoveRate * ALT_FEED_FORWARD;
//consider landing mode
if(altCtrlMode==LANDING)
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;
}
velZ=nav.vz;
velZErr = posZVelSp - velZ;
valZErrD = (spZMoveRate - velZ) * alt_PID.P - (velZ - velZPrev) / dt; //spZMoveRate is from manual stick vel control
velZPrev=velZ;
thrustZSp= velZErr * alt_vel_PID.P + valZErrD * alt_vel_PID.D + thrustZInt; //in ned frame. thrustZInt contains hover thrust
//limit thrust min !!
if(altCtrlMode!=LANDING)
{
if (-thrustZSp < THR_MIN) {
thrustZSp = -THR_MIN;
}
}
//与动力分配相关 testing //Associated with power distribution testing
satXY=0;
satZ=0;
thrustXYSp[0]= sinf(RC_DATA.ROOL * M_PI_F /180.0f) ;//目标角度转加速度 //Target angle turn acceleration
thrustXYSp[1]= sinf(RC_DATA.PITCH * M_PI_F /180.0f) ; //归一化 //Normalization
thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);
//limit tilt max
if(thrustXYSpLen >0.01f )
{
thrustXYMax=-thrustZSp * tanf(TILT_MAX);
if(thrustXYSpLen > thrustXYMax)
{
float k=thrustXYMax / thrustXYSpLen;
thrustXYSp[1] *= k;
thrustXYSp[0] *= k;
satXY=1;
thrustXYSpLen= sqrtf(thrustXYSp[0] * thrustXYSp[0] + thrustXYSp[1] * thrustXYSp[1]);
}
}
//limit max thrust!!
thrustSpLen=sqrtf(thrustXYSpLen * thrustXYSpLen + thrustZSp * thrustZSp);
if(thrustSpLen > THR_MAX)
{
if(thrustZSp < 0.0f) //going up
{
if (-thrustZSp > THR_MAX)
{
/* thrust Z component is too large, limit it */
thrustXYSp[0] = 0.0f;
thrustXYSp[1] = 0.0f;
thrustZSp = -THR_MAX;
satXY = 1;
satZ = 1;
}
else {
float k = 0;
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
thrustXYMax = sqrtf(THR_MAX * THR_MAX- thrustZSp * thrustZSp);
k=thrustXYMax / thrustXYSpLen;
thrustXYSp[1] *=k;
thrustXYSp[0] *= k;
satXY=1;
}
}
else { //going down
/* Z component is negative, going down, simply limit thrust vector */
float k = THR_MAX / thrustSpLen;
thrustZSp *= k;
thrustXYSp[1] *=k;
thrustXYSp[0] *= k;
satXY = 1;
satZ = 1;
}
}
rollSp= asinf(thrustXYSp[0]) * 180.0f /M_PI_F;
pitchSp = asinf(thrustXYSp[1]) * 180.0f /M_PI_F;
// if saturation ,don't integral
if(!satZ )//&& fabs(thrustZSp)<THR_MAX
{
thrustZInt += velZErr * alt_vel_PID.I * dt; //
if (thrustZInt > 0.0f)
thrustZInt = 0.0f;
}
}
