解锁控制逻辑解析-CSDN博客

void unlock(u8 dT_ms)//解锁的函数
{
	
	if( flag.power_state <=2 && para_sta.save_trig == 0)//只有电池电压非最低并且没有操作flash时，才允许进行解锁
	{
		if(sens_hd_check.acc_ok && sens_hd_check.gyro_ok)
		{
			if(sens_hd_check.baro_ok)
			{
				if(flag.sensor_imu_ok  )//imu传感器正常时，才允许解锁
				{
					flag.unlock_err = 0;	//允许解锁标志位

				}
				else
				{
					flag.unlock_err = 1;//imu异常，不允许解锁

				}
			}
			else
			{
				LED_STA.errBaro = 1;
				flag.unlock_err = 2;//气压计异常，不允许解锁。
			}
		}
		else
		{
			LED_STA.errMpu = 1;
			flag.unlock_err = 3;//惯性传感器异常，不允许解锁。
		}
	}
	else
	{
		flag.unlock_err = 4;//电池电压异常，不允许解锁
	}

这个写不允许解锁的程序很简单我在上面都注释了。

	if(flag.unlock_sta == 0)
	{
		if(flag.unlock_cmd != 0)
		{		
			if(flag.unlock_err == 0)
			{
				//
				flag.unlock_sta = flag.unlock_cmd;
				//
				AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"Unlock OK!");
				
			}
			else 
			{
				//reset
				flag.unlock_cmd = 0;
				//
				if(flag.unlock_err == 1)
				{
					AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"Unlock Fail!");
				}
				else if(flag.unlock_err == 2)
				{
					AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"Unlock Fail!");
				}
				else if(flag.unlock_err == 3)
				{
					AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"Unlock Fail!");
				}
				else if(flag.unlock_err == 4)
				{
					AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"Power Low,Unlock Fail!");
				}
				else
				{
				
				}
			}
		}
		else
		{
		
		}
	}
	else
	{
		if(flag.unlock_cmd == 0)
		{
			AnoDTSendStr(USE_HID|USE_U2,SWJ_ADDR,LOG_COLOR_GREEN,"FC Output Locked! ");
		}		
		flag.unlock_sta = flag.unlock_cmd;
	}

好多层逻辑，但也不复杂：
如果unlock_sta等于0，也就是没有解锁，那么如果unlock_cmd=1，也就是解锁命令有效，而且unlock_err=0，解锁没有错误，那么将解锁置一。
否则如果前面条件满足，但解锁有错误，就将unlock_cmd置0，清楚解锁命令，并给出错误的原因。

if(CH_N[CH_THR] < -UN_THR_VALUE  )
	{
		//判断用户是否想要上锁、解锁
		if(ABS(CH_N[CH_YAW])>0.1f*UN_YAW_VALUE && CH_N[CH_PIT]< -0.1f*UN_PIT_VALUE)
		{
			if(flag.locking == 0)
			{
				flag.locking = 1;  还不知道locking什么意思
			}
		}
		else
		{
			flag.locking = 0;
		}

		//飞控上锁、解锁检测   外八或者内八都会使flag.locking = 2 
		if(CH_N[CH_PIT]<-UN_PIT_VALUE && CH_N[CH_ROL]>UN_ROL_VALUE && CH_N[CH_YAW]<-UN_YAW_VALUE)
		{
			stick_fun_0 = 1; //进行时间积分 判断是否要上锁
			flag.locking = 2;
		}
		else if(CH_N[CH_PIT]<-UN_PIT_VALUE && CH_N[CH_ROL]<-UN_ROL_VALUE && CH_N[CH_YAW]>UN_YAW_VALUE)
		{
			stick_fun_0 = 1;
			flag.locking = 2;
		}
		else
		{
			stick_fun_0 = 0;
		}

void fail_safe(void) //失控保护应用程序被注释掉了相当于没有用到。

stick_function_check_longpress时间积分函数大概是检测上锁标志位然后过段时间上锁。

void stick_function(u8 dT_ms)
//状态监测
//未解锁才允许检测摇杆功能

void RC_duty_task(u8 dT_ms) //建议2ms调用一次
{
	if(flag.start_ok)	
	{
		/////////////获得通道数据////////////////////////
		for(u8 i=0;i<CH_NUM;i++)
		这个很重要 但是我还不得其解，只大概知道CH_N[]是遥控器通道，
/*enum
{
 CH_ROL = 0,
 CH_PIT ,
 CH_THR ,
 CH_YAW ,
 AUX1 ,
 AUX2 ,
 AUX3 ,
 AUX4 ,
 CH_NUM,//8
};*/这个枚举就代表了对应通道及数字对应
		{
			CH_N[i] = rc_in.rc_ch.st_data.ch_[i]-1500;
			CH_N[i] = LIMIT(CH_N[i],-500,500);//限制到+—500
		}					
		///////////////////////////////////////////////
		//解锁监测	
		unlock(dT_ms);
		//摇杆触发功能监测
		stick_function(dT_ms);	

		//失控保护检查
		if(rc_in.fail_safe!=0)
		{
			fail_safe();
		}
		else
		{
			if(LED_STA.noRc == 1)
			{
				LED_STA.noRc = 0;
			}
		}

	}
}