首先可以去看正點原子給的
這裏面對他的飛控的基本原理如控制算法等做了介紹。
感覺他們就弄了個最基本的姿態環加上一個高度環,就完了,其實也沒什麼複雜的對吧,都沒什麼速度環,位置環,不過加了光流模塊應該是有位置環的我覺得。
不過我看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就可以發現,前面包含速度環和位置環,後面包含角速度環和角度環。
這麼串起來的話內外環的頻率如何。
還有內外環頻率不一樣的話數據如何保持同步,比較外環的輸入是內環的期望。
不過本身他們傳感器的更新速率就不一樣啊,氣壓計更新速率應該是沒有三軸加速度傳感器那麼快的,那麼自然也就導致外環速率應該沒有內環速率那麼高啊。
這種小型飛控的源碼就清晰明瞭許多。