// 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; } }