ATK-MiniFly 最核心的飞控代码

于 2024-05-10 11:31:25 发布
阅读量203 收藏 2
点赞数 5
文章标签： 单片机 无人机 飞控
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/katerdaisy/article/details/138656562
版权

6.3 姿态解算和 PID 算法流程图

MiniFly 四轴的姿态解算和 PID 算法流程图如图 6.3.1 所示：
在这里插入图片描述

//姿态稳定任务，以每秒500次的速度采集传感器姿态信息，然后通过滤波和PID算法得到电机控制量。

void stabilizerTask(void* param)
{
	u32 tick = 0;
	u32 lastWakeTime = getSysTickCnt();
	
	ledseqRun(SYS_LED, seq_alive);
	
	while(!sensorsAreCalibrated())
	{
		vTaskDelayUntil(&lastWakeTime, MAIN_LOOP_DT);
	}
	
	while(1) 
	{
		vTaskDelayUntil(&lastWakeTime, MAIN_LOOP_DT);		/*1ms周期延时*/

		//获取6轴和气压数据（500Hz）
		if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
		{
			sensorsAcquire(&sensorData, tick);				/*获取6轴和气压数据*/
		}

		//四元数和欧拉角计算（250Hz）
		if (RATE_DO_EXECUTE(ATTITUDE_ESTIMAT_RATE, tick))
		{
			imuUpdate(sensorData.acc, sensorData.gyro, &state, ATTITUDE_ESTIMAT_DT);
		}

		//位置预估计算（250Hz）
		if (RATE_DO_EXECUTE(POSITION_ESTIMAT_RATE, tick))
		{
			positionEstimate(&sensorData, &state, POSITION_ESTIMAT_DT);
		}
			
		//目标姿态和飞行模式设定（100Hz）	
		if (RATE_DO_EXECUTE(RATE_100_HZ, tick) && getIsCalibrated()==true)
		{
			commanderGetSetpoint(&setpoint, &state);	/*目标数据和飞行模式设定*/	
		}
		
		if (RATE_DO_EXECUTE(RATE_250_HZ, tick))
		{
			fastAdjustPosZ();	/*快速调整高度*/
		}		
		
		/*读取光流数据(100Hz)*/
		if (RATE_DO_EXECUTE(RATE_100_HZ, tick))
		{
			getOpFlowData(&state, 0.01f);	
		}
		
		/*翻滚检测(500Hz) 非定点模式*/
		if (RATE_DO_EXECUTE(RATE_500_HZ, tick) && (getCommanderCtrlMode() != 0x03))
		{
			flyerFlipCheck(&setpoint, &control, &state);	
		}
		
		/*异常检测*/
		anomalDetec(&sensorData, &state, &control);			
		
		/*PID控制*/	
		
		stateControl(&control, &sensorData, &state, &setpoint, tick);
				
		
		//控制电机输出（500Hz）
		if (RATE_DO_EXECUTE(RATE_500_HZ, tick))
		{
			powerControl(&control);	
		}
		
		tick++;
	}
}

下面是该任务串行执行的函数，其中最核心的是串级 PID 控制函数stateControl（）

/*获取传感器数据*/
void sensorsAcquire(sensorData_t *sensors, const u32 tick)	
{	
	sensorsReadGyro(&sensors->gyro);
	sensorsReadAcc(&sensors->acc);
	sensorsReadMag(&sensors->mag);
	sensorsReadBaro(&sensors->baro);
}

void imuUpdate(Axis3f acc, Axis3f gyro, state_t *state , float dt)	/*数据融合 互补滤波*/
{
	float normalise;
	float ex, ey, ez;
	float halfT = 0.5f * dt;
	float accBuf[3] = {0.f};
	Axis3f tempacc = acc;
	
	gyro.x = gyro.x * DEG2RAD;	/* 度转弧度 */
	gyro.y = gyro.y * DEG2RAD;
	gyro.z = gyro.z * DEG2RAD;

	/* 加速度计输出有效时,利用加速度计补偿陀螺仪*/
	if((acc.x != 0.0f) || (acc.y != 0.0f) || (acc.z != 0.0f))
	{
		/*单位化加速计测量值*/
		normalise = invSqrt(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
		acc.x *= normalise;
		acc.y *= normalise;
		acc.z *= normalise;

		/*加速计读取的方向与重力加速计方向的差值，用向量叉乘计算*/
		ex = (acc.y * rMat[2][2] - acc.z * rMat[2][1]);
		ey = (acc.z * rMat[2][0] - acc.x * rMat[2][2]);
		ez = (acc.x * rMat[2][1] - acc.y * rMat[2][0]);
		
		/*误差累计，与积分常数相乘*/
		exInt += Ki * ex * dt ;  
		eyInt += Ki * ey * dt ;
		ezInt += Ki * ez * dt ;
		
		/*用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量*/
		gyro.x += Kp * ex + exInt;
		gyro.y += Kp * ey + eyInt;
		gyro.z += Kp * ez + ezInt;
	}
	/* 一阶近似算法，四元数运动学方程的离散化形式和积分 */
	float q0Last = q0;
	float q1Last = q1;
	float q2Last = q2;
	float q3Last = q3;
	q0 += (-q1Last * gyro.x - q2Last * gyro.y - q3Last * gyro.z) * halfT;
	q1 += ( q0Last * gyro.x + q2Last * gyro.z - q3Last * gyro.y) * halfT;
	q2 += ( q0Last * gyro.y - q1Last * gyro.z + q3Last * gyro.x) * halfT;
	q3 += ( q0Last * gyro.z + q1Last * gyro.y - q2Last * gyro.x) * halfT;
	
	/*单位化四元数*/
	normalise = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= normalise;
	q1 *= normalise;
	q2 *= normalise;
	q3 *= normalise;
	
	imuComputeRotationMatrix();	/*计算旋转矩阵*/
	
	/*计算roll pitch yaw 欧拉角*/
	state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG; 
	state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;
	state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;
	
	if (!isGravityCalibrated)	/*未校准*/
	{		
//		accBuf[0] = tempacc.x* rMat[0][0] + tempacc.y * rMat[0][1] + tempacc.z * rMat[0][2];	/*accx*/
//		accBuf[1] = tempacc.x* rMat[1][0] + tempacc.y * rMat[1][1] + tempacc.z * rMat[1][2];	/*accy*/
		accBuf[2] = tempacc.x* rMat[2][0] + tempacc.y * rMat[2][1] + tempacc.z * rMat[2][2];	/*accz*/
		calBaseAcc(accBuf);		/*计算静态加速度*/				
	}
}

void positionEstimate(sensorData_t* sensorData, state_t* state, float dt) 
{	
	static float rangeLpf = 0.f;
	static float accLpf[3] = {0.f};		/*加速度低通*/	
	float weight = wBaro;

	float relateHight = sensorData->baro.asl - startBaroAsl;	/*气压相对高度*/
	
	if(getModuleID()==OPTICAL_FLOW && isEnableVl53lxx==true)	/*光流模块可用,且使用激光*/
	{
		vl53lxxReadRange(&sensorData->zrange);	/*读取激光数据*/
	
//		rangeLpf = sensorData->zrange.distance;
		rangeLpf += (sensorData->zrange.distance - rangeLpf) * 0.1f;	/*低通 单位cm*/		
			
		float quality = sensorData->zrange.quality;

		if(quality < 0.3f)	/*低于这个可行度，激光数据不可用*/
		{
			quality = 0.f;
		}else
		{
			weight = quality;
			startBaroAsl = sensorData->baro.asl - rangeLpf;
		}
		fusedHeight = rangeLpf * quality + (1.0f - quality) * relateHight;/*融合高度*/	
	}
	else	/*无光流模块*/
	{
		fusedHeight = relateHight;	/*融合高度*/
	}
	fusedHeightLpf += (fusedHeight - fusedHeightLpf) * 0.1f;	/*融合高度 低通*/
	
	if(isRstHeight)
	{	
		isRstHeight = false;
		
		weight = 0.95f;		/*增加权重，快速调整*/	
		
		startBaroAsl = sensorData->baro.asl;
		
		if(getModuleID() == OPTICAL_FLOW)
		{
			if(sensorData->zrange.distance < VL53L0X_MAX_RANGE)
			{
				startBaroAsl -= sensorData->zrange.distance;
				fusedHeight = sensorData->zrange.distance;
			}
		}
		
		estimator.pos[Z] = fusedHeight;
	}
	else if(isRstAll)
	{
		isRstAll = false;
		
		accLpf[Z] = 0.f;	
		fusedHeight  = 0.f;
		fusedHeightLpf = 0.f;
		startBaroAsl = sensorData->baro.asl;
		
		if(getModuleID() == OPTICAL_FLOW)
		{
			if(sensorData->zrange.distance < VL53L0X_MAX_RANGE)
			{
				startBaroAsl -= sensorData->zrange.distance;
				fusedHeight = sensorData->zrange.distance;
			}
		}
		
		estimator.vel[Z] = 0.f;
		estimator.pos[Z] = fusedHeight;
	}	
	
	
	Axis3f accelBF;
	
	accelBF.x = sensorData->acc.x * GRAVITY_CMSS - estimator.accBias[X];
	accelBF.y = sensorData->acc.y * GRAVITY_CMSS - estimator.accBias[Y];
	accelBF.z = sensorData->acc.z * GRAVITY_CMSS - estimator.accBias[Z];	
	
	/* Rotate vector to Earth frame - from Forward-Right-Down to North-East-Up*/
	imuTransformVectorBodyToEarth(&accelBF);	
	
	estimator.acc[X] = applyDeadbandf(accelBF.x, estimator.vAccDeadband);/*去除死区的加速度*/
	estimator.acc[Y] = applyDeadbandf(accelBF.y, estimator.vAccDeadband);/*去除死区的加速度*/
	estimator.acc[Z] = applyDeadbandf(accelBF.z, estimator.vAccDeadband);/*去除死区的加速度*/
	
	for(u8 i=0; i<3; i++)
		accLpf[i] += (estimator.acc[i] - accLpf[i]) * 0.1f;	/*加速度低通*/
		
	bool isKeyFlightLand = ((getCommanderKeyFlight()==true)||(getCommanderKeyland()==true));	/*定高飞或者降落状态*/
	
	if(isKeyFlightLand == true)		/*定高飞或者降落状态*/
	{
		state->acc.x = constrainf(accLpf[X], -ACC_LIMIT, ACC_LIMIT);	/*加速度限幅*/
		state->acc.y = constrainf(accLpf[Y], -ACC_LIMIT, ACC_LIMIT);	/*加速度限幅*/
		state->acc.z = constrainf(accLpf[Z], -ACC_LIMIT, ACC_LIMIT);	/*加速度限幅*/
	}else
	{
		state->acc.x = constrainf(estimator.acc[X], -ACC_LIMIT_MAX, ACC_LIMIT_MAX);	/*最大加速度限幅*/
		state->acc.y = constrainf(estimator.acc[Y], -ACC_LIMIT_MAX, ACC_LIMIT_MAX);	/*最大加速度限幅*/
		state->acc.z = constrainf(estimator.acc[Z], -ACC_LIMIT_MAX, ACC_LIMIT_MAX);	/*最大加速度限幅*/
	}		

	
	float errPosZ = fusedHeight - estimator.pos[Z];
	
	/* 位置预估: Z-axis */
	inavFilterPredict(Z, dt, estimator.acc[Z]);
	/* 位置校正: Z-axis */
	inavFilterCorrectPos(Z, dt, errPosZ, weight);	

	if(getModuleID() == OPTICAL_FLOW)	/*光流模块可用*/
	{		
		float opflowDt = dt;
		
		float opResidualX = opFlow.posSum[X] - estimator.pos[X];
		float opResidualY = opFlow.posSum[Y] - estimator.pos[Y];
		float opResidualXVel = opFlow.velLpf[X] - estimator.vel[X];
		float opResidualYVel = opFlow.velLpf[Y] - estimator.vel[Y];
		
		float opWeightScaler = 1.0f;
		
		float wXYPos = wOpflowP * opWeightScaler;
		float wXYVel = wOpflowV * sq(opWeightScaler);
		
		/* 位置预估: XY-axis */
		inavFilterPredict(X, opflowDt, estimator.acc[X]);
		inavFilterPredict(Y, opflowDt, estimator.acc[Y]);
		/* 位置校正: XY-axis */
		inavFilterCorrectPos(X, opflowDt, opResidualX, wXYPos);
		inavFilterCorrectPos(Y, opflowDt, opResidualY, wXYPos);
		/* 速度校正: XY-axis */
		inavFilterCorrectVel(X, opflowDt, opResidualXVel, wXYVel);
		inavFilterCorrectVel(Y, opflowDt, opResidualYVel, wXYVel);
	}
	
	/*加速度偏置校正*/
	Axis3f accelBiasCorr = {{ 0, 0, 0}};
	
	accelBiasCorr.z -= errPosZ  * sq(wBaro);
	float accelBiasCorrMagnitudeSq = sq(accelBiasCorr.x) + sq(accelBiasCorr.y) + sq(accelBiasCorr.z);
	if (accelBiasCorrMagnitudeSq < sq(INAV_ACC_BIAS_ACCEPTANCE_VALUE)) 
	{
		/* transform error vector from NEU frame to body frame */
		imuTransformVectorEarthToBody(&accelBiasCorr);

		/* Correct accel bias */
		estimator.accBias[X] += accelBiasCorr.x * wAccBias * dt;
		estimator.accBias[Y] += accelBiasCorr.y * wAccBias * dt;
		estimator.accBias[Z] += accelBiasCorr.z * wAccBias * dt;
	}	

	if(isKeyFlightLand == true)		/*定高飞或者降落状态*/
	{
		state->velocity.x = constrainf(estimator.vel[X], -VELOCITY_LIMIT, VELOCITY_LIMIT);	/*速度限幅 VELOCITY_LIMIT*/
		state->velocity.y = constrainf(estimator.vel[Y], -VELOCITY_LIMIT, VELOCITY_LIMIT);	/*速度限幅 VELOCITY_LIMIT*/
		state->velocity.z = constrainf(estimator.vel[Z], -VELOCITY_LIMIT, VELOCITY_LIMIT);	/*速度限幅 VELOCITY_LIMIT*/
	}else
	{
		state->velocity.x = constrainf(estimator.vel[X], -VELOCITY_LIMIT_MAX, VELOCITY_LIMIT_MAX);	/*最大速度限幅 VELOCITY_LIMIT_MAX*/
		state->velocity.y = constrainf(estimator.vel[Y], -VELOCITY_LIMIT_MAX, VELOCITY_LIMIT_MAX);	/*最大速度限幅 VELOCITY_LIMIT_MAX*/
		state->velocity.z = constrainf(estimator.vel[Z], -VELOCITY_LIMIT_MAX, VELOCITY_LIMIT_MAX);	/*最大速度限幅 VELOCITY_LIMIT_MAX*/
	}
	
	state->position.x = estimator.pos[X];
	state->position.y = estimator.pos[Y];
	state->position.z = estimator.pos[Z];	
}

void commanderGetSetpoint(setpoint_t *setpoint, state_t *state)
{	
	static float maxAccZ = 0.f;
	
	ctrlDataUpdate();	/*更新控制数据*/
	
	state->isRCLocked = isRCLocked;	/*更新遥控器锁定状态*/
	
	if(commander.ctrlMode & 0x01)/*定高模式*/
	{
		if(commander.keyLand)/*一键降落*/
		{
			flyerAutoLand(setpoint, state);
		}
		else if(commander.keyFlight)/*一键起飞*/ 
		{	
			setpoint->thrust = 0;
			setpoint->mode.z = modeAbs;		
			
			if (initHigh == false)
			{
				initHigh = true;	
				isAdjustingPosXY = true;
				errorPosX = 0.f;
				errorPosY = 0.f;
				errorPosZ = 0.f;

				setFastAdjustPosParam(0, 1, 80.f);	/*一键起飞高度80cm*/															
			}		
				
			float climb = ((ctrlValLpf.thrust - 32767.f) / 32767.f);
			if(climb > 0.f) 
				climb *= MAX_CLIMB_UP;
			else
				climb *= MAX_CLIMB_DOWN;
			
			if (fabsf(climb) > 5.f)
			{
				isAdjustingPosZ = true;												
				setpoint->mode.z = modeVelocity;
				setpoint->velocity.z = climb;

				if(climb < -(CLIMB_RATE/5.f))	/*油门下拉过大*/
				{
					if(isExitFlip == true)		/*退出空翻，再检测加速度*/
					{
						if(maxAccZ < state->acc.z)
							maxAccZ = state->acc.z;
						if(maxAccZ > 250.f)		/*油门下拉过大，飞机触地停机*/
						{
							commander.keyFlight = false;
							estRstAll();	/*复位估测*/
						}
					}
				}else
				{
					maxAccZ = 0.f;
				}
			}
			else if (isAdjustingPosZ == true)
			{
				isAdjustingPosZ = false;
			
				setpoint->mode.z = modeAbs;
				setpoint->position.z = state->position.z + errorPosZ;	/*调整新位置*/									
			}
			else if(isAdjustingPosZ == false)	/*Z位移误差*/
			{
				errorPosZ = setpoint->position.z - state->position.z;
				errorPosZ = constrainf(errorPosZ, -10.f, 10.f);	/*误差限幅 单位cm*/
			}			
		}
		else/*着陆状态*/
		{
			setpoint->mode.z = modeDisable;
			setpoint->thrust = 0;
			setpoint->velocity.z = 0;
			setpoint->position.z = 0;
			initHigh = false;
			isAdjustingPosZ = false;
		}
	}
	else /*手动飞模式*/
	{
		setpoint->mode.z = modeDisable;
		setpoint->thrust = ctrlValLpf.thrust;
	}	
 	
	setpoint->attitude.roll = ctrlValLpf.roll;
	setpoint->attitude.pitch = ctrlValLpf.pitch;
	setpoint->attitude.yaw  = -ctrlValLpf.yaw;	/*摇杆方向和yaw方向相反*/
	
	if(getOpDataState() && commander.ctrlMode == 0x03)	/*光流数据可用，定点模式*/ 
	{
		setpoint->attitude.yaw *= 0.5f;	/*定点模式减慢yaw调节*/
		
		/*调整位置 速度模式*/
		if(fabsf(setpoint->attitude.roll) > 1.5f || fabsf(setpoint->attitude.pitch) > 1.5f)
		{
			adjustPosXYTime = 0;
			isAdjustingPosXY = true;
			setpoint->mode.x = modeVelocity;
			setpoint->mode.y = modeVelocity;
			setpoint->velocity.x = setpoint->attitude.pitch * 4.0f;
			setpoint->velocity.y = setpoint->attitude.roll * 4.0f;	
		}
		else if(isAdjustingPosXY == true)
		{
			if(adjustPosXYTime++ > 100)
			{
				adjustPosXYTime = 0;
				isAdjustingPosXY = false;
			}		
			setpoint->mode.x = modeAbs;
			setpoint->mode.y = modeAbs;
			setpoint->position.x = state->position.x + errorPosX;	//调整新位置
			setpoint->position.y = state->position.y + errorPosY;	//调整新位置
		}
		else if(isAdjustingPosXY == false)	/*位移误差*/
		{	
			errorPosX = setpoint->position.x - state->position.x;
			errorPosY = setpoint->position.y - state->position.y;
			errorPosX = constrainf(errorPosX, -30.0f, 30.0f);	/*误差限幅 单位cm*/
			errorPosY = constrainf(errorPosY, -30.0f, 30.0f);	/*误差限幅 单位cm*/
		}
	}
	else	/*手动模式*/
	{
		setpoint->mode.x = modeDisable;
		setpoint->mode.y = modeDisable;		
	}
	
	setpoint->mode.roll = modeDisable;	
	setpoint->mode.pitch = modeDisable;	
	
	if(commander.flightMode)/*无头模式*/
	{
		yawMode = CAREFREE;		
		rotateYawCarefree(setpoint, state);
	}		
	else	/*X飞行模式*/
	{
		yawMode = XMODE;
	}		
}

/*快速调整高度*/
static void fastAdjustPosZ(void)
{	
	if(velModeTimes > 0)
	{
		velModeTimes--;
		estRstHeight();	/*复位估测高度*/
		
		float baroVel = (sensorData.baro.asl - baroLast) / 0.004f;	/*250Hz*/
		baroLast = sensorData.baro.asl;
		baroVelLpf += (baroVel - baroVelLpf) * 0.35f;

		setpoint.mode.z = modeVelocity;
		state.velocity.z = baroVelLpf;		/*气压计融合*/
		setpoint.velocity.z = -1.0f * baroVelLpf;
		
		if(velModeTimes == 0)
		{
			if(getModuleID() == OPTICAL_FLOW)
				setHeight = getFusedHeight();
			else
				setHeight = state.position.z;
		}		
	}
	else if(absModeTimes > 0)
	{
		absModeTimes--;
		estRstAll();	/*复位估测*/
		setpoint.mode.z = modeAbs;		
		setpoint.position.z = setHeight;
	}	
}

bool getOpFlowData(state_t *state, float dt)
{
	static u8 cnt = 0;
	float height = 0.01f * getFusedHeight();/*读取高度信息 单位m*/
	
	if(opFlow.isOpFlowOk && height<4.0f)	/*4m范围内，光流可用*/
	{
		cnt= 0;
		opFlow.isDataValid = true;
		
		float coeff = RESOLUTION * height;
		float tanRoll = tanf(state->attitude.roll * DEG2RAD);
		float tanPitch = tanf(state->attitude.pitch * DEG2RAD);
		
		opFlow.pixComp[X] = 480.f * tanPitch;	/*像素补偿，负方向*/
		opFlow.pixComp[Y] = 480.f * tanRoll;
		opFlow.pixValid[X] = (opFlow.pixSum[X] + opFlow.pixComp[X]);	/*实际输出像素*/
		opFlow.pixValid[Y] = (opFlow.pixSum[Y] + opFlow.pixComp[Y]);		
		
		if(height < 0.05f)	/*光流测量范围大于5cm*/
		{
			coeff = 0.0f;
		}
		opFlow.deltaPos[X] = coeff * (opFlow.pixValid[X] - opFlow.pixValidLast[X]);	/*2帧之间位移变化量，单位cm*/
		opFlow.deltaPos[Y] = coeff * (opFlow.pixValid[Y] - opFlow.pixValidLast[Y]);	
		opFlow.pixValidLast[X] = opFlow.pixValid[X];	/*上一次实际输出像素*/
		opFlow.pixValidLast[Y] = opFlow.pixValid[Y];
		opFlow.deltaVel[X] = opFlow.deltaPos[X] / dt;	/*速度 cm/s*/
		opFlow.deltaVel[Y] = opFlow.deltaPos[Y] / dt;
		
#ifdef AVERAGE_FILTER
		velFilter(opFlow.deltaVel, opFlow.velLpf);		/*限幅均值滤波法*/
#else
		opFlow.velLpf[X] += (opFlow.deltaVel[X] - opFlow.velLpf[X]) * 0.15f;	/*速度低通 cm/s*/
		opFlow.velLpf[Y] += (opFlow.deltaVel[Y] - opFlow.velLpf[Y]) * 0.15f;	/*速度低通 cm/s*/
#endif			
		opFlow.velLpf[X] = constrainf(opFlow.velLpf[X], -VEL_LIMIT, VEL_LIMIT);	/*速度限幅 cm/s*/
		opFlow.velLpf[Y] = constrainf(opFlow.velLpf[Y], -VEL_LIMIT, VEL_LIMIT);	/*速度限幅 cm/s*/
	
		opFlow.posSum[X] += opFlow.deltaPos[X];	/*累积位移 cm*/
		opFlow.posSum[Y] += opFlow.deltaPos[Y];	/*累积位移 cm*/
	}
	else if(opFlow.isDataValid == true)
	{
		if(cnt++ > 100)	/*超过定点高度，切换为定高模式*/
		{
			cnt = 0;
			opFlow.isDataValid = false;
		}	
		resetOpFlowData();	
	}
	
	return opFlow.isOpFlowOk;	/*返回光流状态*/
}

/********************************************************
* Flyer 翻滚检测 
*********************************************************/
void flyerFlipCheck(setpoint_t* setpoint, control_t* control, state_t* state)
{
	static u16 flipThrust = 0;
	static u16 tempThrust = 0;
	static u16 reverTime = 0;
	static u16 flipTimeout = 0;	
	static float pitchTemp = 0.0;
	static float rollTemp = 0.0;
	static float yawTemp = 0.0;
	static float deltaThrust = 0.0;
	static u16 exitFlipCnt = 0;
	static u16 flipThrustMax = 0;

	fstate = (u8)flipState;
	
	switch(flipState)
	{
		case FLIP_IDLE:	/*翻滚空闲状态*/
		{
			if(flipDir!=CENTER)
			{
				if(control->thrust > 28000 && state->velocity.z > -20.f)
				{
					flipState = FLIP_SET;
					exitFlipCnt = 500;		/*空翻完成，延时1S(500Hz)退出空翻 */
					isExitFlip = false;
				}					
				else
				{
					flipDir = CENTER;					
				}					
			}else if(isExitFlip == false)
			{
				if(exitFlipCnt > 0) 
					exitFlipCnt--;
				else
					isExitFlip = true;
			}				
			break;
		}
		case FLIP_SET:	/*翻滚设置*/
		{
			currentRate = 0.f;
			maxRateCnt = 0;
			currentAngle = 0.f;
			
			flipTimeout = 0;
			control->flipDir = flipDir;
			flipThrust = -9000.0f + 1.2f * configParam.thrustBase;
			deltaThrust = configParam.thrustBase / 90.0f;
			flipThrustMax = configParam.thrustBase + 20000;
			if(flipThrustMax > 62000) flipThrustMax = 62000;
			tempThrust = flipThrust; 
					
			rollTemp = state->attitude.roll;
			pitchTemp = state->attitude.pitch;									
			yawTemp = state->attitude.yaw;
			
			flipState = FLIP_SPEED_UP;
			break;
		}
		case FLIP_SPEED_UP:	/*加速上升过程*/
		{
			if(state->velocity.z < desiredVelZ)
			{
				setpoint->mode.z = modeDisable;
				if(tempThrust < flipThrustMax)
					tempThrust += deltaThrust;
				setpoint->thrust = tempThrust;
				
				if(flipTimeout++ > SPEED_UP_TIMEOUT)	/*超时处理*/
				{
					flipTimeout = 0;
					flipState = FLIP_SLOW_DOWN;			/*直接进入下一个状态*/
				}														
			}else	
			{	
				flipTimeout = 0;				
				flipState = FLIP_SLOW_DOWN;
			}		
			break;
		}
		case FLIP_SLOW_DOWN:	/*减速过程*/
		{
			if(tempThrust > flipThrust)
			{
				tempThrust -= (6500.f - flipThrust / 10.0f);
				setpoint->mode.z = modeDisable;
				setpoint->thrust = tempThrust;
			}else
			{
				flipState = FLIP_PERIOD;
			}
		}
		case FLIP_PERIOD:	/*翻滚过程*/
		{
			if(flipTimeout++ > FLIP_TIMEOUT)	/*超时处理*/
			{
				flipTimeout = 0;
				flipState = FLIP_ERROR;
			}
			
			setpoint->mode.z = modeDisable;
			setpoint->thrust = flipThrust - 3*currentRate;
			
			currentAngle += currentRate;		/*当前角度 放大500倍*/
			
			if(currentAngle < MID_ANGLE)		/*上半圈*/
			{
				if(currentRate < MAX_FLIP_RATE)/*小于最大速率，速率继续增大*/
					currentRate += DELTA_RATE;
				else			/*大于最大速率，速率保持*/
					maxRateCnt++;					
			}else	/*下半圈*/
			{
				if(maxRateCnt > 0)
				{						
					maxRateCnt--;			
				}else
				{
					if(currentRate >= DELTA_RATE && currentAngle < 2*MID_ANGLE)
					{
						currentRate -= DELTA_RATE;	
					}																
					else							
						flipState = FLIP_FINISHED;						
				}
			}
			
			switch(control->flipDir)	
			{
				case FORWARD:	/* pitch+ */
					setpoint->attitude.pitch = currentRate;	
					setpoint->attitude.roll = state->attitude.roll = rollTemp;
					setpoint->attitude.yaw = state->attitude.yaw = yawTemp;
					break;				
				case BACK:		/* pitch- */
					setpoint->attitude.pitch = -currentRate;
					setpoint->attitude.roll = state->attitude.roll = rollTemp;
					setpoint->attitude.yaw = state->attitude.yaw = yawTemp;
					break;
				case LEFT:		/* roll- */
					setpoint->attitude.roll = -currentRate;	
					setpoint->attitude.pitch = state->attitude.pitch = pitchTemp;
					setpoint->attitude.yaw = state->attitude.yaw = yawTemp;
					break;
				case RIGHT:		/* roll+ */					
					setpoint->attitude.roll = currentRate;
					setpoint->attitude.pitch = state->attitude.pitch = pitchTemp;
					setpoint->attitude.yaw = state->attitude.yaw = yawTemp;
					break;
				default :break;
			}
			break;
		}
		case FLIP_FINISHED:	/*翻滚完成*/
		{
			setpoint->mode.z = modeDisable;
			setpoint->thrust = tempThrust;
			tempThrust = flipThrust;
			
			reverTime = 0;
			flipTimeout = 0;
			flipDir = CENTER;	
			control->flipDir = flipDir;

			flipState = REVER_SPEED_UP;
			break;
		}
		case REVER_SPEED_UP:	/*翻滚完成后 反向加速*/
		{			
			if(reverTime++<REVER_SPEEDUP_TIME)	
			{
				if(tempThrust < flipThrustMax)
					tempThrust += 2.0f * deltaThrust;
				setpoint->mode.z = modeDisable;
				setpoint->thrust = tempThrust;
			}else
			{				
				flipTimeout = 0;
				flipState = FLIP_IDLE;					
//				if(getCommanderKeyFlight())	/*定高模式*/
//				{
//					setpoint->thrust = 0;
//					setpoint->mode.z = modeAbs;	
//				}
			}
			break;
		}
		case FLIP_ERROR:
		{
			reverTime = 0;
			flipDir = CENTER;	
			control->flipDir = CENTER;
			
			setpoint->mode.z = modeDisable;
			setpoint->thrust = 0;
			if(flipTimeout++ > 1)
			{
				flipTimeout = 0;
				
				if(getCommanderKeyFlight())	/*定高模式*/
				{
					setpoint->thrust = 0;
					setpoint->mode.z = modeAbs;				
				}
				
				flipState = FLIP_IDLE;
			}
			break;
		}
		default : break;
	}			
}

/*异常检测*/
void anomalDetec(const sensorData_t *sensorData, const state_t *state, const control_t *control)
{
#if defined(DETEC_ENABLED)
	
	if(control->flipDir != CENTER) 
	{
		outFlipCnt = 1000;
		return;
	}	

	if(state->isRCLocked == false && 		//遥控器解锁状态
	getCommanderKeyFlight() == false &&		//未飞行状态
	(getCommanderCtrlMode() & 0x01) == 0x01)//定高模式
	{
		float accMAG = (sensorData->acc.x*sensorData->acc.x) +
						(sensorData->acc.y*sensorData->acc.y) +
						(sensorData->acc.z*sensorData->acc.z);

		if(detecFreeFall(state->acc.z/980.f, accMAG) == true)/*自由落体检测*/
		{				
			setCommanderKeyFlight(true);
			setFastAdjustPosParam(35, 10, 0.f);	/*设置快速调整位置参数*/
		}
	}
	
	if(outFlipCnt > 0)	
	{
		outFlipCnt--;
	}
	if(outFlipCnt == 0 && detecTumbled(state)==true)/*碰撞检测*/
	{
		setCommanderKeyFlight(false);
		setCommanderKeyland(false);
	}			

#endif
}

void stateControl(control_t *control, sensorData_t *sensors, state_t *state, setpoint_t *setpoint, const u32 tick)
{
	static u16 cnt = 0;
	
	if (RATE_DO_EXECUTE(POSITION_PID_RATE, tick))
	{
		if (setpoint->mode.x != modeDisable || setpoint->mode.y != modeDisable || setpoint->mode.z != modeDisable)
		{
			positionController(&actualThrust, &attitudeDesired, setpoint, state, POSITION_PID_DT);
		}
	}
	
	//角度环（外环）
	if (RATE_DO_EXECUTE(ANGEL_PID_RATE, tick))
	{
		if (setpoint->mode.z == modeDisable)
		{
			actualThrust = setpoint->thrust;
		}
		if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) 
		{
			attitudeDesired.roll = setpoint->attitude.roll;
			attitudeDesired.pitch = setpoint->attitude.pitch;
		}
		
		if(control->flipDir == CENTER)
		{
			attitudeDesired.yaw += setpoint->attitude.yaw/ANGEL_PID_RATE; /*期望YAW 速率模式*/
			if(attitudeDesired.yaw > 180.0f) 
				attitudeDesired.yaw -= 360.0f;
			if(attitudeDesired.yaw < -180.0f) 
				attitudeDesired.yaw += 360.0f;
		}
			
		attitudeDesired.roll += configParam.trimR;	//叠加微调值
		attitudeDesired.pitch += configParam.trimP;		
		
		attitudeAnglePID(&state->attitude, &attitudeDesired, &rateDesired);
	}
	
	//角速度环（内环）
	if (RATE_DO_EXECUTE(RATE_PID_RATE, tick))
	{
		if (setpoint->mode.roll == modeVelocity)
		{
			rateDesired.roll = setpoint->attitudeRate.roll;
			attitudeControllerResetRollAttitudePID();
		}
		if (setpoint->mode.pitch == modeVelocity)
		{
			rateDesired.pitch = setpoint->attitudeRate.pitch;
			attitudeControllerResetPitchAttitudePID();
		}
		extern u8 fstate;
		if (control->flipDir != CENTER && fstate == 4)	/*空翻过程只使用内环PID*/
		{
			rateDesired.pitch = setpoint->attitude.pitch;
			rateDesired.roll = setpoint->attitude.roll;
		}
		
		attitudeRatePID(&sensors->gyro, &rateDesired, control);
	}

	control->thrust = actualThrust;	
	
	if (control->thrust < 5.f)
	{			
		control->roll = 0;
		control->pitch = 0;
		control->yaw = 0;
		
		attitudeResetAllPID();	/*复位姿态PID*/	
		positionResetAllPID();	/*复位位置PID*/
		attitudeDesired.yaw = state->attitude.yaw;		/*复位计算的期望yaw值*/
		
		if(cnt++ > 1500)
		{
			cnt = 0;
			configParamGiveSemaphore();
		}
	}else
	{
		cnt = 0;
	}
}

9.控制马达输出

void powerControl(control_t *control)	/*功率输出控制*/
{
	s16 r = control->roll / 2.0f;
	s16 p = control->pitch / 2.0f;
	
	motorPWM.m1 = limitThrust(control->thrust - r - p + control->yaw);
	motorPWM.m2 = limitThrust(control->thrust - r + p - control->yaw);
	motorPWM.m3 = limitThrust(control->thrust + r + p + control->yaw);
	motorPWM.m4 = limitThrust(control->thrust + r - p - control->yaw);		

	if (motorSetEnable)
	{
		motorPWM = motorPWMSet;
	}
	motorsSetRatio(MOTOR_M1, motorPWM.m1);	/*控制电机输出百分比*/
	motorsSetRatio(MOTOR_M2, motorPWM.m2);
	motorsSetRatio(MOTOR_M3, motorPWM.m3);
	motorsSetRatio(MOTOR_M4, motorPWM.m4);
}

void getMotorPWM(motorPWM_t* get)
{
	*get = motorPWM;
}

void setMotorPWM(bool enable, u32 m1_set, u32 m2_set, u32 m3_set, u32 m4_set)
{
	motorSetEnable = enable;
	motorPWMSet.m1 = m1_set;
	motorPWMSet.m2 = m2_set;
	motorPWMSet.m3 = m3_set;	
	motorPWMSet.m4 = m4_set;
}
盼望孤独的二进制
关注
  • 5
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
预备篇：二.飞控代码下载与编译
无人机开发
03-30 5285
[深入浅出多旋翼飞控开发]预备篇][四][飞控代码下载与编译] 作者：BlueSky QQ : 352707983 Github 因论坛关闭，迁移该文章至博客。 系统环境：WIN7/WIN8/WIN10 编译工具：Keil MDK 5.25 1.安装git并克隆项目到本地 首先到https://gitforwindows.org下载最新版git工具并进行安装，一路next就好了。安装完git以...
1，ATK-MiniFly微型四轴原理图.rar
06-05
ATK-Minifly微型四轴原理图及PCB封装，AD打开PCB封装。
【开源飞控】匿名飞控TI版解析（2）
深思深练 厚积薄发
08-02 3180
因为电赛，买来匿名飞控研究一下，感觉相比其他的一下开源飞控，易开发性和稳定性都是比较好的，但就是比较贵…匿名TI版飞控是从32版改过来的，硬件上就换了个芯片，程序里也有些东西都没删干净。整体上来说开源程度很高了，自己想改啥就改啥，不过就是光流内部是不开源的。这里整理里一下飞控框架，也是学习一下一个飞控从头到尾应该怎么写。 一、飞控框架 框架中比较关键的几个部分都用颜色标了一下，具体内...
正点原子飞控——代码学习笔记（一）
m0_57332128的博客
10-08 1033
当要使用JTAG占用的IO口但还要使用SWD时，也就是要使用PB3当做IO口，但还要使用SWD下载或者调试时，就需要禁用JTAG，那么随之产生的副作用就是，我们不能使用JTAG功能。
五、PX4(v1.14)多旋翼位置控制代码(mc_pos_control)分析
最新发布
Vincent的博客
03-24 851
PX4(v1.14)多旋翼位置控制代码(mc_pos_control)分析
开源飞控初探（四）ArduPilot::Copter固件源码分析
hursing的博客
01-12 3560
2022.5.7，基于v4.0.5的分析。官网文档没及时更新，本文对当前版本源码的描述可能和官网不一样。
匿名飞控代码理解（自己理解的，自用，大家可以借鉴）
yt09281102的博客
04-29 1734
就是先执行的一个里获得传感器数据，另外一个进行数据处理。*（奇学科技的后辈，都自己回去学，这不是正途）顺便有没有路过的大神给我发个定高的程序。//系统任务配置，创建不同执行频率的“线程”这个思路看这个视频，博主很牛逼；
ArduPilot开源代码之AP_VideoTX
lida2003的专栏
06-14 849
如能统一，从设计的角度看就比较整齐美观了，当然历史问题导致了当前的设计，能用就是最好的。结合ArduPilot系统，模拟图传部分抽象为AP_VideoTX模块，这里就这个展开一些简单的功能和代码介绍，以便更加理解在实际应用过程中需要注意的一些问题。虽然目前逐步的被高清数字图传所替代，但是模拟图传作为高性价比，尤其是作为经济实惠的产品，为大众所喜爱。注：这里AP_SmartAudio在init过程中会创建线程来处理类似AP_Tramp::update的工作。飞控与VTX图传上电时间可能存在不同步问题，所以。
ATK-MiniFly微型四轴固件说明_V1.0
12-19
STM32 USB STM32 USB STM32 USB STM32 USBSTM32 USB虚拟 串口驱动 串口驱动 串口驱动 路径 ：“ATKATK -MiniFly MiniFly MiniFlyMiniFlyMiniFly微型四轴资料 微型四轴资料 \3，配套软件 ，配套软件 ，配套软件 ，...
ATK-MiniFly微型四轴原理图
11-14
ATK-MiniFly微型四轴原理图，ATK-MiniFly微型四轴原理图
ATK-LORA-01模块资料及源码.rar_ATK lora模块_ATK-LORA-01_NB-IOT 低功耗_atk-lor
07-15
ATK-LORA-01模块资料及源码,物联网的端点网通传输技术，有许多技术方案可以选择，除了lora以外，还有NB-IoT、WiFi、蓝牙、zigbee、Dustl和sub-1GHz等，不同的通讯技术有着不同的特点，也各有适合自己的应用场景。...
STM32飞控代码
09-05
采用mpu6050的飞控代码，非常好理解，可以学习学习
STM32无人机飞控源码（含思路解析）
10-09
无人机飞控源码的视频讲解：https://www.bilibili.com/video/BV1NV411H7sh?
飞控解析sbus的代码
01-22
飞控解析sbus的代码
匿名飞控代码源码-2015
03-01
此代码为匿名飞控代码2015版 适合学习飞控
飞控，源码
02-26
AeroQuad-master.zip　飞控源码
ATK-NEO-6M GPS模块_ATK-NEO-6MGPS模块_
09-28
ATK-NEO-6M GPS模块单片机单码上位机代码
ubantu上跑通无人机飞控代码（ZJU-FAST-Lab/GCOPTER）教程
weixin_53665577的博客
04-29 1235
最近接触到一些无人机轨迹规划的知识，学习了浙江大学FAST实验室做的一些东西，不得不说，突然接触到这么深奥的知识是很痛苦的，尤其是在读王哲培老师的博士论文的时候，读了一个寒假只读了一点点表皮，不，一点表皮也没有（苦涩）。所以想直接跑代码。我首先在unbuntu18.04上面跑的，然后遇到了各种错误，最终确实错误搞不定，然后重装了一个20.04，竟然直接跑通了。所以写下这篇文章给大家一些建议。
atk-lora模块arduino代码
10-04
atk-lora模块是一款基于Arduino的无线通信模块，它采用LoRa无线技术，能够实现长距离、低功耗、低速率的数据传输。下面是一个基本的atk-lora模块的Arduino代码示例。 首先，我们需要引入LoRa库以便使用相关函数。在Arduino IDE中，点击Sketch -> 包含库 -> 管理库，搜索"LoRa"并安装。 然后，我们需要定义配置参数，如频段、传输功率等。例如： #include <LoRa.h> #define BAND 915E6 // 频段，根据所使用的模块和地区进行设置 #define POWER 20 // 传输功率，单位为 dBm，可根据需求进行调整 void setup() { Serial.begin(9600); // 初始化串口，用于调试输出 LoRa.setPins(ss, rst, dio0); // 设置SS引脚，复位引脚，中断引脚（根据模块的实际连接情况进行修改） if (!LoRa.begin(BAND)) { // 初始化LoRa模块 Serial.println("LoRa init failed. Check your wiring."); while(true); // 若初始化失败，则进入死循环 } LoRa.setTxPower(POWER); // 设置传输功率 } void loop() { // 在这里编写你的代码 } 在setup()函数中，我们首先初始化了串口，方便我们在调试时输出信息。然后通过调用LoRa.setPins()函数设置LoRa模块相关的引脚。接着，通过调用LoRa.begin()函数初始化LoRa模块。如果初始化失败，会打印错误信息，并进入死循环。 在loop()函数中，你可以编写你自己的代码。例如，可以使用LoRa库提供的函数发送和接收数据。例如，使用LoRa.beginPacket()函数开始一个数据包的传输，使用LoRa.write()函数写入数据，最后通过调用LoRa.endPacket()函数完成数据包的传输。 这只是一个基本的atk-lora模块的Arduino示例代码。你可以根据你的具体需求进行修改和扩展。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值