//for copter launch and flight mode switch. it's raw and simple for climb rate mode now
//TOBE IMPROVED
void FlightModeFSMSimple(void)
{
if(FLY_ENABLE)
{
if(RC_DATA.THROTTLE>=600 )
{
if(altCtrlMode!=CLIMB_RATE)
{
// hoverThrust=-0.65f;
zIntReset=1;
thrustZSp=0;
altCtrlMode=CLIMB_RATE;
offLandFlag=1;
altLand=-nav.z; //记录起飞时的高度
#ifdef AUTO_MW
SetHeadFree(1);
#endif
}
}
else
{
if(altCtrlMode==MANUAL)
{
RC_DATA.THROTTLE=SLOW_THRO; //手动模式待机转200
}
}
}
}
void CommAppCmdProcess(void)
{
//process
switch(UdataBuf[4])//MSP_SET_4CON
{
case MSP_SET_4CON: //UdataBuf
rcData[THROTTLE]=UdataBuf[6]<<8 | UdataBuf[5];
rcData[YAW]=UdataBuf[8]<<8 | UdataBuf[7];
rcData[PITCH]=UdataBuf[10]<<8 | UdataBuf[9];
rcData[ROLL]=UdataBuf[12]<<8 | UdataBuf[11];
// RCDataProcess();
break;
case MSP_ARM_IT://MSP_ARM_IT
// FLY_ENABLE=0xA5;
armState=REQ_ARM;
break;
case MSP_DISARM_IT:
armState=REQ_DISARM;
break;
case MSP_FLY_STATE:
flyLogApp=1;
break;
case MSP_ACC_CALI:
imuCaliFlag=1;
break;
case MSP_HEAD_FREE:
SetHeadFree(1);
break;
case MSP_STOP_HEAD_FREE:
SetHeadFree(0);
break;
case MSP_LAND_DOWN: //自动降落
altCtrlMode=LANDING;
break;
}
}