正点原子MiniFly Firmware V1.5开源四轴代码分析一:pid.c,attitude_pid.c。
正点原子MiniFly Firmware V1.5开源四轴代码分析一:pid.c,attitude_pid.c。
·
补充关于pid的知识:
PID控制应该算是应用非常广泛的控制算法了。小到控制一个元件的温度,大到控制无人机的飞行姿态和飞行速度等等,都可以使用PID控制。这里我们从原理上来理解PID控制。
PID(proportion integration differentiation)其实就是指比例,积分,微分控制。
来自:PID控制算法原理(抛弃公式,从本质上真正理解PID控制) - 知乎 (zhihu.com)
pid.c
这个文件主要设置关于pid的初始化函数,设置参数函数。具体可看里边注释
#include "pid.h"
//此函数用作初始化pid
void pidInit(PidObject* pid, const float desired, const pidInit_t pidParam, const float dt)
{
pid->error = 0; //误差
pid->prevError = 0; //上一时刻的误差
pid->integ = 0; //积分值
pid->deriv = 0; //微分值
pid->desired = desired;//目标值
pid->kp = pidParam.kp; //kp
pid->ki = pidParam.ki;//ki
pid->kd = pidParam.kd;//kd
pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT;//积分限幅
pid->outputLimit = DEFAULT_PID_OUTPUT_LIMIT;//默认pid输出限幅,0为不限幅
pid->dt = dt;//采样时间间隔
}
//此函数用作更新pid
float pidUpdate(PidObject* pid, const float error)
{
float output;//输出
pid->error = error; //误差
pid->integ += pid->error * pid->dt; //积分计算
//积分限幅
if (pid->integ > pid->iLimit) //若大于,就积分=积分限幅
{
pid->integ = pid->iLimit;
}
else if (pid->integ < -pid->iLimit)
{
pid->integ = -pid->iLimit;
}
pid->deriv = (pid->error - pid->prevError) / pid->dt; //微分计算公式
pid->outP = pid->kp * pid->error; //kp的输出值 kp*error
pid->outI = pid->ki * pid->integ; //ki的输出值 ki*error
pid->outD = pid->kd * pid->deriv; //kd的输出值 kd*error
output = pid->outP + pid->outI + pid->outD; //总输出
//输出限幅,此处设置outputLimit = 0,没有输出限幅,跳过此函数。
if (pid->outputLimit != 0)
{
if (output > pid->outputLimit)
output = pid->outputLimit;
else if (output < -pid->outputLimit)
output = -pid->outputLimit;
}
pid->prevError = pid->error; //更新误差
pid->out = output; //更新输出
return output; //返回值
}
//下列函数都是设置一些参数设置函数,从字面意思应该很好理解。
void pidSetIntegralLimit(PidObject* pid, const float limit)
{
pid->iLimit = limit;
}
void pidSetOutputLimit(PidObject* pid, const float limit)
{
pid->outputLimit = limit;
}
void pidSetError(PidObject* pid, const float error)
{
pid->error = error;
}
void pidSetDesired(PidObject* pid, const float desired)
{
pid->desired = desired;
}
float pidGetDesired(PidObject* pid)
{
return pid->desired;
}
void pidSetKp(PidObject* pid, const float kp)
{
pid->kp = kp;
}
void pidSetKi(PidObject* pid, const float ki)
{
pid->ki = ki;
}
void pidSetKd(PidObject* pid, const float kd)
{
pid->kd = kd;
}
void pidSetDt(PidObject* pid, const float dt)
{
pid->dt = dt;
}
void pidReset(PidObject* pid)
{
pid->error = 0;
pid->prevError = 0;
pid->integ = 0;
pid->deriv = 0;
}
attitude_pid.c
角度环和角速度环pid,主要用到pid.c的函数,这里有点像c++里面的继承的意思,attitude_pid.c继承了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));
}
/*
入口参数为三个attitude_t结构体指针,attitude_t即姿态数据结构类型。
第一个结构体为数据融合之后的输出值,也就是日志一当中说的将处理过的加速计数据与陀螺仪数据结合之后得到的姿态数据,即测量的当前角度。
第二个结构体为期望的角度值,来自WIFI/遥控RC。
第三个结构体对象即PID后的输出。三者都含有姿态数据的roll/pitch/yaw三个参数,前两个参数只要调用限幅位置式的pid校正即可,
yaw由于代表的偏航角度,要限制其范围在-180°-180°之后再进行限幅位置式pid校正。
*/
void attitudeAnglePID(attitude_t *actualAngle,attitude_t *desiredAngle,attitude_t *outDesiredRate) /* 角度环PID */
{
outDesiredRate->roll = pidUpdate(&pidAngleRoll, desiredAngle->roll - actualAngle->roll); //计算出期望的输出
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
#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)
{
static u16 altholdCount = 0;
// Roll and Pitch
attitude->pitch = 0.15f * pidUpdate(&pidVX, setpoint->velocity.x - state->velocity.x);
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) //fabs取绝对值
{
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);
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;
}
整个控制系统框图
更多推荐
已为社区贡献2条内容
所有评论(0)