笔触狂放9

这个屌丝很懒，什么也没留下！

热门标签

正点原子无人机飞控原理（基于源码分析）

作者：笔触狂放9 | 2024-06-13 07:20:48

踩

正点原子无人机

首先可以去看正点原子给的

这里面对他的飞控的基本原理如控制算法等做了介绍。

感觉他们就弄了个最基本的姿态环加上一个高度环，就完了，其实也没什么复杂的对吧，都没什么速度环，位置环，不过加了光流模块应该是有位置环的我觉得。

不过我看1.3版本的飞控代码里面是有位置控制和速度控制的，位置控制包含XYZ三个方向的位置控制

在position_pid.c这个文件里


#include <math.h>
#include "pid.h"
#include "commander.h"
#include "config_param.h"
#include "position_pid.h"
#include "remoter_ctrl.h"
#include "maths.h"
 
/********************************************************************************	 
 * 本程序只供学习使用，未经作者许可，不得用于其它任何用途
 * ALIENTEK MiniFly
 * 位置PID控制代码	
 * 正点原子@ALIENTEK
 * 技术论坛:www.openedv.com
 * 创建日期:2017/5/12
 * 版本：V1.3
 * 版权所有，盗版必究。
 * Copyright(C) 广州市星翼电子科技有限公司 2014-2024
 * All rights reserved
 *
 * 修改说明:
 * 版本V1.3 水平定点PID输出较大，所以在位置环输出设置0.1的系数，
	速率环输出设置0.15系数，从而增加PID的可调性。
********************************************************************************/
 
#define THRUST_BASE  		(20000)	/*基础油门值*/
 
#define PIDVX_OUTPUT_LIMIT	120.0f	//ROLL限幅	(单位°带0.15的系数)
#define PIDVY_OUTPUT_LIMIT	120.0f 	//PITCH限幅	(单位°带0.15的系数)
#define PIDVZ_OUTPUT_LIMIT	(40000)	/*PID VZ限幅*/
 
#define PIDX_OUTPUT_LIMIT	1200.0f	//X轴速度限幅(单位cm/s 带0.1的系数)
#define PIDY_OUTPUT_LIMIT	1200.0f	//Y轴速度限幅(单位cm/s 带0.1的系数)
#define PIDZ_OUTPUT_LIMIT	120.0f	//Z轴速度限幅(单位cm/s)
 
 
static float thrustLpf = THRUST_BASE;	/*油门低通*/
 
PidObject pidVX;
PidObject pidVY;
PidObject pidVZ;
 
PidObject pidX;
PidObject pidY;
PidObject pidZ;
 
void positionControlInit(float velocityPidDt, float posPidDt)
{
	pidInit(&pidVX, 0, configParam.pidPos.vx, velocityPidDt);	/*vx PID初始化*/
	pidInit(&pidVY, 0, configParam.pidPos.vy, velocityPidDt);	/*vy PID初始化*/
	pidInit(&pidVZ, 0, configParam.pidPos.vz, velocityPidDt);	/*vz PID初始化*/
	pidSetOutputLimit(&pidVX, PIDVX_OUTPUT_LIMIT);		/* 输出限幅 */
	pidSetOutputLimit(&pidVY, PIDVY_OUTPUT_LIMIT);		/* 输出限幅 */
	pidSetOutputLimit(&pidVZ, PIDVZ_OUTPUT_LIMIT);		/* 输出限幅 */
	
	pidInit(&pidX, 0, configParam.pidPos.x, posPidDt);			/*x PID初始化*/
	pidInit(&pidY, 0, configParam.pidPos.y, posPidDt);			/*y PID初始化*/
	pidInit(&pidZ, 0, configParam.pidPos.z, posPidDt);			/*z PID初始化*/
	pidSetOutputLimit(&pidX, PIDX_OUTPUT_LIMIT);		/* 输出限幅 */
	pidSetOutputLimit(&pidY, PIDY_OUTPUT_LIMIT);		/* 输出限幅 */
	pidSetOutputLimit(&pidZ, PIDZ_OUTPUT_LIMIT);		/* 输出限幅 */
}
 
static void velocityController(float* thrust, attitude_t *attitude, setpoint_t *setpoint, const state_t *state)   //maxi:这就是速度控制，说明有速度环                                                      
{	
	static u16 altholdCount = 0;
	
	// Roll and Pitch
	attitude->pitch = 0.15f * pidUpdate(&pidVX, setpoint->velocity.x - state->velocity.x);    //maxi:说明速度环的输出给了角度环，等号右边很明显就是期望值减去测量值嘛！！！
	attitude->roll = 0.15f * pidUpdate(&pidVY, setpoint->velocity.y - state->velocity.y);
	
	// Thrust
	float thrustRaw = pidUpdate(&pidVZ, setpoint->velocity.z - state->velocity.z);
	
	*thrust = constrainf(thrustRaw + THRUST_BASE, 1000, 60000);	/*油门限幅*/
	
	thrustLpf += (*thrust - thrustLpf) * 0.003f;
	
	if(getCommanderKeyFlight())	/*定高飞行状态*/
	{
		if(fabs(state->acc.z) < 35.f)
		{
			altholdCount++;
			if(altholdCount > 1000)
			{
				altholdCount = 0;
				if(fabs(configParam.thrustBase - thrustLpf) > 1000.f)	/*更新基础油门值*/
					configParam.thrustBase = thrustLpf;
			}
		}else
		{
			altholdCount = 0;
		}
	}else if(getCommanderKeyland() == false)	/*降落完成，油门清零*/
	{
		*thrust = 0;
	}
}
 
void positionController(float* thrust, attitude_t *attitude, setpoint_t *setpoint, const state_t *state, float dt)                                                
{	
	if (setpoint->mode.x == modeAbs || setpoint->mode.y == modeAbs)
	{
		setpoint->velocity.x = 0.1f * pidUpdate(&pidX, setpoint->position.x - state->position.x);    // maxi:说明位置环的输出给了速度环
		setpoint->velocity.y = 0.1f * pidUpdate(&pidY, setpoint->position.y - state->position.y);
	}
	
	if (setpoint->mode.z == modeAbs)
	{
		setpoint->velocity.z = pidUpdate(&pidZ, setpoint->position.z - state->position.z);
	}
	
	velocityController(thrust, attitude, setpoint, state);
}
 
/*获取定高油门值*/
float getAltholdThrust(void)
{
	return thrustLpf;
}
 
void positionResetAllPID(void)
{
	pidReset(&pidVX);
	pidReset(&pidVY);
	pidReset(&pidVZ);
 
	pidReset(&pidX);
	pidReset(&pidY);
	pidReset(&pidZ);
}
 
void positionPIDwriteToConfigParam(void)
{
	configParam.pidPos.vx.kp  = pidVX.kp;
	configParam.pidPos.vx.ki  = pidVX.ki;
	configParam.pidPos.vx.kd  = pidVX.kd;
	
	configParam.pidPos.vy.kp  = pidVY.kp;
	configParam.pidPos.vy.ki  = pidVY.ki;
	configParam.pidPos.vy.kd  = pidVY.kd;
	
	configParam.pidPos.vz.kp  = pidVZ.kp;
	configParam.pidPos.vz.ki  = pidVZ.ki;
	configParam.pidPos.vz.kd  = pidVZ.kd;
	
	configParam.pidPos.x.kp  = pidX.kp;
	configParam.pidPos.x.ki  = pidX.ki;
	configParam.pidPos.x.kd  = pidX.kd;
	
	configParam.pidPos.y.kp  = pidY.kp;
	configParam.pidPos.y.ki  = pidY.ki;
	configParam.pidPos.y.kd  = pidY.kd;
	
	configParam.pidPos.z.kp  = pidZ.kp;
	configParam.pidPos.z.ki  = pidZ.ki;
	configParam.pidPos.z.kd  = pidZ.kd;
}

下面是attitude_pid.c


#include <stdbool.h>
#include "pid.h"
#include "sensors.h"
#include "attitude_pid.h"
 
/********************************************************************************	 
 * 本程序只供学习使用，未经作者许可，不得用于其它任何用途
 * ALIENTEK MiniFly
 * 姿态PID控制代码	
 * 正点原子@ALIENTEK
 * 技术论坛:www.openedv.com
 * 创建日期:2017/5/12
 * 版本：V1.3
 * 版权所有，盗版必究。
 * Copyright(C) 广州市星翼电子科技有限公司 2014-2024
 * All rights reserved
 *
 * 修改说明:
 * 版本V1.3 纠正角度环和角速度环积分时间参数错误的bug。
********************************************************************************/
 
/*角度环积分限幅*/
#define PID_ANGLE_ROLL_INTEGRATION_LIMIT    30.0
#define PID_ANGLE_PITCH_INTEGRATION_LIMIT   30.0
#define PID_ANGLE_YAW_INTEGRATION_LIMIT     180.0
 
/*角速度环积分限幅*/
#define PID_RATE_ROLL_INTEGRATION_LIMIT		500.0
#define PID_RATE_PITCH_INTEGRATION_LIMIT	500.0
#define PID_RATE_YAW_INTEGRATION_LIMIT		50.0
 
PidObject pidAngleRoll;
PidObject pidAnglePitch;
PidObject pidAngleYaw;
PidObject pidRateRoll;
PidObject pidRatePitch;
PidObject pidRateYaw;
 
static inline int16_t pidOutLimit(float in)
{
	if (in > INT16_MAX)
		return INT16_MAX;
	else if (in < -INT16_MAX)
		return -INT16_MAX;
	else
		return (int16_t)in;
}
 
void attitudeControlInit(float ratePidDt, float anglePidDt)
{
	pidInit(&pidAngleRoll, 0, configParam.pidAngle.roll, anglePidDt);			/*roll  角度PID初始化*/
	pidInit(&pidAnglePitch, 0, configParam.pidAngle.pitch, anglePidDt);			/*pitch 角度PID初始化*/
	pidInit(&pidAngleYaw, 0, configParam.pidAngle.yaw, anglePidDt);				/*yaw   角度PID初始化*/
	pidSetIntegralLimit(&pidAngleRoll, PID_ANGLE_ROLL_INTEGRATION_LIMIT);		/*roll  角度积分限幅设置*/
	pidSetIntegralLimit(&pidAnglePitch, PID_ANGLE_PITCH_INTEGRATION_LIMIT);		/*pitch 角度积分限幅设置*/
	pidSetIntegralLimit(&pidAngleYaw, PID_ANGLE_YAW_INTEGRATION_LIMIT);			/*yaw   角度积分限幅设置*/
	
	pidInit(&pidRateRoll, 0, configParam.pidRate.roll, ratePidDt);				/*roll  角速度PID初始化*/
	pidInit(&pidRatePitch, 0, configParam.pidRate.pitch, ratePidDt);			/*pitch 角速度PID初始化*/
	pidInit(&pidRateYaw, 0, configParam.pidRate.yaw, ratePidDt);				/*yaw   角速度PID初始化*/
	pidSetIntegralLimit(&pidRateRoll, PID_RATE_ROLL_INTEGRATION_LIMIT);			/*roll  角速度积分限幅设置*/
	pidSetIntegralLimit(&pidRatePitch, PID_RATE_PITCH_INTEGRATION_LIMIT);		/*pitch 角速度积分限幅设置*/
	pidSetIntegralLimit(&pidRateYaw, PID_RATE_YAW_INTEGRATION_LIMIT);			/*yaw   角速度积分限幅设置*/
}
 
bool attitudeControlTest()
{
	return true;
}
 
void attitudeRatePID(Axis3f *actualRate,attitude_t *desiredRate,control_t *output)	/* 角速度环PID */
{
	output->roll = pidOutLimit(pidUpdate(&pidRateRoll, desiredRate->roll - actualRate->x));
	output->pitch = pidOutLimit(pidUpdate(&pidRatePitch, desiredRate->pitch - actualRate->y));
	output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, desiredRate->yaw - actualRate->z));
}
 
void attitudeAnglePID(attitude_t *actualAngle,attitude_t *desiredAngle,attitude_t *outDesiredRate)	/* 角度环PID */
{
	outDesiredRate->roll = pidUpdate(&pidAngleRoll, desiredAngle->roll - actualAngle->roll);      //maxi:这成功地说明了外环的输出就内环的输入也就是内环的期望值。
	outDesiredRate->pitch = pidUpdate(&pidAnglePitch, desiredAngle->pitch - actualAngle->pitch);
 
	float yawError = desiredAngle->yaw - actualAngle->yaw ;
	if (yawError > 180.0f) 
		yawError -= 360.0f;
	else if (yawError < -180.0) 
		yawError += 360.0f;
	outDesiredRate->yaw = pidUpdate(&pidAngleYaw, yawError);
}
 
void attitudeControllerResetRollAttitudePID(void)
{
    pidReset(&pidAngleRoll);
}
 
void attitudeControllerResetPitchAttitudePID(void)
{
    pidReset(&pidAnglePitch);
}
 
void attitudeResetAllPID(void)	/*复位PID*/
{
	pidReset(&pidAngleRoll);
	pidReset(&pidAnglePitch);
	pidReset(&pidAngleYaw);
	pidReset(&pidRateRoll);
	pidReset(&pidRatePitch);
	pidReset(&pidRateYaw);
}
 
void attitudePIDwriteToConfigParam(void)
{
	configParam.pidAngle.roll.kp = pidAngleRoll.kp;
	configParam.pidAngle.roll.ki = pidAngleRoll.ki;
	configParam.pidAngle.roll.kd = pidAngleRoll.kd;
	
	configParam.pidAngle.pitch.kp = pidAnglePitch.kp;
	configParam.pidAngle.pitch.ki = pidAnglePitch.ki;
	configParam.pidAngle.pitch.kd = pidAnglePitch.kd;
	
	configParam.pidAngle.yaw.kp = pidAngleYaw.kp;
	configParam.pidAngle.yaw.ki = pidAngleYaw.ki;
	configParam.pidAngle.yaw.kd = pidAngleYaw.kd;
	
	configParam.pidRate.roll.kp = pidRateRoll.kp;
	configParam.pidRate.roll.ki = pidRateRoll.ki;
	configParam.pidRate.roll.kd = pidRateRoll.kd;
	
	configParam.pidRate.pitch.kp = pidRatePitch.kp;
	configParam.pidRate.pitch.ki = pidRatePitch.ki;
	configParam.pidRate.pitch.kd = pidRatePitch.kd;
	
	configParam.pidRate.yaw.kp = pidRateYaw.kp;
	configParam.pidRate.yaw.ki = pidRateYaw.ki;
	configParam.pidRate.yaw.kd = pidRateYaw.kd;
}

我现在发现正点原子也是四环串级，也就是角速度环+角度环+速度环+位置环！！！！！！和无名的一样了那就。

每一个外环的输出就是内环的期望值，这在它的代码里面体现得很明显了。

你去读position_pid.c和attitude_pid.c就可以发现，前面包含速度环和位置环，后面包含角速度环和角度环。

这么串起来的话内外环的频率如何。

还有内外环频率不一样的话数据如何保持同步，比较外环的输入是内环的期望。

不过本身他们传感器的更新速率就不一样啊，气压计更新速率应该是没有三轴加速度传感器那么快的，那么自然也就导致外环速率应该没有内环速率那么高啊。

看下面角度和角速度的频率是500Hz，光流模块本质是摄像头，我之前完ov6070的时候，我记得频率应该是30Hz左右，摄像头的频率肯定达不到500Hz那么高，更何况还是个低配摄像头，可能连ov6070都不如。测速度的，有的靠GPS，那频率更低了。

这种小型飞控的源码就清晰明了许多。

===============================================================================================

我们来进一步分析解读正点原子的飞控源码，正点原子飞控基于freertos操作系统来的，所以它是写成一个一个任务的。

这些其实在正点原子的源码的main.c里面就有体现，main.c其实就是写创建几个任务，就完了。具体任务细节应该就是去每个任务对应的执行函数里面去看。

最核心的任务莫过于stabilizerTask，

xTaskCreate(stabilizerTask, "STABILIZER", 450, NULL, 5, NULL);

它的执行函数就是stabilizerTask，我们可以右键选择跳转到这个函数的定义上去，你就会发现跳转到了stabilizer.c文件里面，stabilizerTask函数的定义就在stabilizer.c里面，而且就是一个while(1)，非常符合对操作系统中任务的定义。

找到stabilizerTask函数你就会发现飞控的主要代码都包含在这里面了，包括你之前一直想找的翻滚，一直不知道是在哪里调用了翻滚的函数实现翻滚的，这下一下子就找到了！！！！都在stabilizerTask函数里面！！！！！！！！！！我想这下子你对正点原子飞控的整个代码体系有了更为清晰的认识了吧！包括你之前不知道翻滚里面对电机的控制量最后怎么给到电机的呀，看了stabilizerTask函数也清楚了，因为翻滚函数flyerFlipCheck(&setpoint, &control, &state); 和电机控制函数powerControl(&control);操作的是同一套数据，而且这两个函数输入的是地质，也就意味着改变形参可以改变实参的值，怪不得是&control。当然我也好奇，函数输入能不能直接写结构体？作为一个疑问，这应该是基础的C语法。


void stabilizerTask(void* param)
{
	u32 tick = 0;
	u32 lastWakeTime = getSysTickCnt();
	
	ledseqRun(SYS_LED, seq_alive);
	
	while(!sensorsAreCalibrated())
	{
		vTaskDelayUntil(&lastWakeTime, MAIN_LOOP_DT);
	}
	
	while(1)      //maxi:果然，一个任务里面就是一个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++;
	}
}

===============================================================================================

最重要的应该是这几个C文件

anomal_detec.c 异常检测

attitude_pid.c 姿态环也就是角度环和角速度环

flip.c 翻滚的代码

pid.c 通用的PID计算函数的定义在这里面，其他地方只要算PID的只需要调一个函数即可

position_pid.c 外环也就是位置环和速度环

power_control.c 把PID的输出，升力等等最后分配到各个电机上

sensfusion.c 六轴数据融合，好像用的互补滤波。

sensors.c

stabilizer.c freertos是分成一个个任务（进程）的，四轴最核心的一个任务 stabilizerTask它的执行函数就是stabilizerTask就在这个文件里面被定义的。讲道理这几个C文件应该先看这个。

state_control.c

state_estimator.c 根据传感器估测你的位置和速度

=============================================================================================

无人机的空翻

我们先看下开发指南说的

我刚刚又在state_control.c里面看到一行注释，空翻过程值使用内环PID，它这里的内环指的角速度环。我之前以为它没用PID，还是用了的，就用的角速度环。

还有我看flip.c的代码我感觉它的期望角速度是渐变的，逐渐变到最大值然后保持不变，而不是突变，一下子把角速度期望值变为某一个值，可能这样是为了无人机姿态稳定些？

还有注意空翻是360度，不是180度。


case FLIP_PERIOD:	/*翻滚过程*/
		{
			if(flipTimeout++ > FLIP_TIMEOUT)	/*超时处理*/
			{
				flipTimeout = 0;
				flipState = FLIP_ERROR;
			}
			
			setpoint->mode.z = modeDisable;
			setpoint->thrust = flipThrust - 3*currentRate;
			
			currentAngle += currentRate;		/*当前角度 放大500倍*/
			                               //maxi:当前角度是当前速率累加起来的,
			                               //maxi:(这是开发指南里面的话）FLIP_PERIOD 状态下，我们按照设定方向设置四轴角速度的值，由于只是用了内环， 没使用外环角度环，四轴就没有自稳模式，四轴就会按照设定方向以设定角速度转动，同时 我们对角速度积分，当这个积分值达到 360°，说明四轴绕某个轴转动一圈了，然后状态切 换到 FLIP_FINISHED。这就是实际空翻过程。 
			if(currentAngle < MID_ANGLE)		/*上半圈*/
			{
				if(currentRate < MAX_FLIP_RATE)/*小于最大速率，速率继续增大*/
					currentRate += DELTA_RATE;               //maxi:DELTA_RATE是递增速率，感觉翻滚没有用PID(用了），cureenrRate应该控制着电机的输出，看189行，赋值给。还有我感觉它的期望角速度是渐变的，逐渐变到最大值然后保持不变，而不是突变，一下子把角速度期望值变为某一个值，可能这样是为了无人机姿态稳定些？
				else			/*大于最大速率，速率保持*/
					maxRateCnt++;					//maxi:maxRateCnt的值原本是0，这个的值好像要么是0要么是1，用来判断是否达到最大速率。
			}else	/*下半圈*/
			{
				if(maxRateCnt > 0)
				{						
					maxRateCnt--;			
				}else
				{
					if(currentRate >= DELTA_RATE && currentAngle < 2*MID_ANGLE)
					{
						currentRate -= DELTA_RATE;	  //maxi:下半圈就让叫嘟嘟降低
					}																
					else							
						flipState = FLIP_FINISHED;						
				}
			}

COMMUNICATE文件下下面的这些应该是和无线连接相关的，遥控相关的，communicate本身是交流的意思。而且main函数里面创建的第一个任务就是创建无线连接任务，而这个任务的执行函数radiolinkTask就是在COMMUNICATE这个文件夹下的radiolink.c文件里面定义的。

综上来看，正点原子的飞控程序里面，最主要的应该是这三个文件夹。当然也可以说两个，FLIGHT和COMMUNICATE，当然也可以说就一个，那就是FLIGHT。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/笔触狂放9/article/detail/711380