RoboFlyDEMO_源码分享

1万 · 2024-1-23 16:35

RoboFlyDEMO_源码分享

1万 · 2024-1-23 16:44

#include "main.h"

int main(void)
{
System_Init(); //系统初始化

while(1)
{
Task_Schedule(); //任务调度
}
}

1万 · 2024-1-23 16:44

/*******************************************************************************************
      声明
本项目代码仅供个人学习使用，可以自由移植修改，但必须保留此声明信息。移植过程中出现其他

不可估量的BUG，天际智联不负任何责任。请勿商用！

* 程序版本：V1.01
* 程序日期：2018-8-18
* 程序作者：愤怒的小孩
* 版权所有：西安天际智联信息技术有限公司
*******************************************************************************************/
#include "stm32f10x.h"
#include "pid.h"
#include "led.h"
#include "ParamSave.h"
#include "structconfig.h"

/*****************************************************************************
* 函  数：void PID_Postion_Cal(PID_TYPE*PID,float target,float measure)
* 功  能：位置式PID算法
* 参  数：PID: 算法P I D参数的结构体
*       target: 目标值
*       measure: 测量值
* 返回值：无
* 备  注: 角度环和角速度环共用此函数
*****************************************************************************/
void PID_Postion_Cal(PID_TYPE*PID,float target,float measure)
{

PID->Error  = target - measure;             //误差
PID->Differ = PID->Error - PID->PreError; //微分量

PID->Pout = PID->P * PID->Error;                      //比例控制
PID->Iout = PID->Ilimit_flag * PID->I * PID->Integral;  //积分控制
PID->Dout = PID->D * PID->Differ;                      //微分控制

PID->OutPut =  PID->Pout + PID->Iout + PID->Dout;    //比例 + 积分 + 微分总控制

if(Airplane_Enable == 1&&RC_Control.THROTTLE >= 180) //飞机解锁之后再加入积分,防止积分过调
{
if(measure > (PID->Ilimit)||measure < -PID->Ilimit) //积分分离
{PID->Ilimit_flag = 0;}
else
{
PID->Ilimit_flag = 1;                            //加入积分(只有测量值在-PID->Ilimit~PID->Ilimit 范围内时才加入积分)
PID->Integral += PID->Error;                      //对误差进行积分
if(PID->Integral > PID->Irang)                   //积分限幅
PID->Integral = PID->Irang;
if(PID->Integral < -PID->Irang)                   //积分限幅
      PID->Integral = -PID->Irang;
}
}else
{PID->Integral = 0;}
PID->PreError = PID->Error ;                         //前一个误差值
}

/*****************************************************************************
* 函  数：void PidParameter_init(void)
* 功  能：初始化PID结构体里的一些成员值
* 参  数：无
* 返回值：无
* 备  注: 由于PID参数我们都保存在FLASH中，所以此函数初始化时不用初始化这些参数，
*       但是Flash中的参数有可能因为误操作被擦除，如果Flash读取参数失败，则就
*       初始化为默认参数。
*****************************************************************************/
void PidParameter_init(void)
{
//ROLL轴
PID_ROL_Rate.Ilimit_flag = 0; //Roll轴角速度积分的分离标志
PID_ROL_Rate.Ilimit = 150; //Roll轴角速度积分范围
PID_ROL_Rate.Irang = 1200; //Roll轴角速度积分限幅度（由于电机输出有限，所以积分输出也是有限的）
PID_ROL_Angle.Ilimit_flag = 0;//Roll轴角度积分的分离标志
PID_ROL_Angle.Ilimit = 35; //Roll轴角度积分范围
PID_ROL_Angle.Irang = 200; //Roll轴角度积分限幅度（由于电机输出有限，所以积分输出也是有限的）

//PITCH轴
PID_PIT_Rate.Ilimit_flag = 0; //Pitch轴角速度积分的分离标志
PID_PIT_Rate.Ilimit = 150; //Pitch轴角速度积分范围
PID_PIT_Rate.Irang = 1200; //Pitch轴角速度积分限幅度（由于电机输出有限，所以积分输出也是有限的）
PID_PIT_Angle.Ilimit_flag = 0;//Roll轴角度积分的分离标志
PID_PIT_Angle.Ilimit = 35; //Roll轴角度积分范围
PID_PIT_Angle.Irang = 200; //Roll轴角度积分限幅度（由于电机输出有限，所以积分输出也是有限的）

//YAW轴
// PID_YAW_Angle.P = 5;
// PID_YAW_Rate.P = 2;
// PID_YAW_Rate.I = 0.07;
// PID_YAW_Rate.D = 0.7;
PID_YAW_Rate.Ilimit_flag = 0; //Yaw轴角速度积分的分离标志
PID_YAW_Rate.Ilimit = 150; //Yaw轴角速度积分范围
PID_YAW_Rate.Irang = 1200; //Yaw轴角速度积分限幅度（由于电机输出有限，所以积分输出也是有限的）
PID_YAW_Angle.Ilimit_flag = 0;//Yaw轴角度积分的分离标志
PID_YAW_Angle.Ilimit = 35; //Yaw轴角度积分范围
PID_YAW_Angle.Irang = 200; //Yaw轴角度积分限幅度（由于电机输出有限，所以积分输出也是有限的）


//高度环
PID_ALT_Rate.Ilimit_flag = 0;
PID_ALT_Rate.Ilimit = 0;
PID_ALT_Rate.Irang = 0;
PID_ALT.Ilimit_flag = 0;
PID_ALT.Ilimit = 100;
PID_ALT.Irang = 200;
}

1万 · 2024-1-23 16:52

/*******************************************************************************************
      声明
本项目代码仅供个人学习使用，可以自由移植修改，但必须保留此声明信息。移植过程中出现其他

不可估量的BUG，天际智联不负任何责任。请勿商用！

* 程序版本：V1.01
* 程序日期：2018-8-18
* 程序作者：愤怒的小孩
* 版权所有：西安天际智联信息技术有限公司
*******************************************************************************************/
#include "main.h"
#include "control.h"
#include "pid.h"
#include "led.h"
#include "motor.h"
#include "math.h"
#include "structconfig.h"
#include "stdio.h"

//角度环PID
PID_TYPE PID_ROL_Angle;
PID_TYPE PID_PIT_Angle;
PID_TYPE PID_YAW_Angle;
//角速度环PID
PID_TYPE PID_ROL_Rate;
PID_TYPE PID_PIT_Rate;
PID_TYPE PID_YAW_Rate;
//高度环PID
PID_TYPE PID_ALT_Rate;
PID_TYPE PID_ALT;

float Pre_THROTTLE,THROTTLE;
float Moto_PWM_1=0.0f,Moto_PWM_2=0.0f,Moto_PWM_3=0.0f,Moto_PWM_4=0.0f;
uint8_t SI24R1_Controlflag = 1,Airplane_Enable;

/******************************************************************************************
*函  数：void Control(FLOAT_ANGLE *att_in,FLOAT_XYZ *gyr_in, RC_TYPE *rc_in, uint8_t armed)
*功  能：姿态控制,角度环控制和角速度环控制
*参  数：att_in：测量值
*       gry_in: MPU6050读取的角速度值
*       rc_in : 遥控器设定值
*       armed记录命令
*返回值：无
*备  注：RoboFly 小四轴机头与电机示意图
   机头(Y+)

   M1 ↑ M2
\ | /
   \  |  /
   \ | /
      ————————+————————>X+
   / | \
   /  |  \
/ | \
   M4 | M3

1. M1 M3电机逆时针旋转，M2 M4电机顺时针旋转
2. X:是MPU6050的 X 轴，Y:是MPU6050的 Y 轴，Z轴正方向垂直 X-Y 面，竖直向上
3. 绕 X 轴旋转为PITCH 角
   绕 Y 轴旋转为 ROLL 角
   绕 Z 轴旋转为 YAW  角
4. 自己DIY时进行动力分配可以一个轴一个轴的分配，切勿三个轴同时分配。
*******************************************************************************************/
void Control(FLOAT_ANGLE *att_in,FLOAT_XYZ *gyr_in, RC_TYPE *rc_in, uint8_t armed)
{
FLOAT_ANGLE Measure_Angle,Target_Angle;
Measure_Angle.rol = att_in->rol;
Measure_Angle.pit = att_in->pit;
Measure_Angle.yaw = att_in->yaw;
Target_Angle.rol = (float)((rc_in->ROLL-1500)/12.0f);
Target_Angle.pit = (float)((rc_in->PITCH-1500)/12.0f);
Target_Angle.yaw = (float)((1500-rc_in->YAW)/12.0f);

if(!SI24R1_Controlflag)
{
Target_Angle.yaw = 0;
}else if(fabs(Target_Angle.yaw )<4)
{
Target_Angle.yaw = 0;
}
if(1 == (GET_FLAG(FLY_MODE))) //无头模式
{
Yaw_Carefree(&Target_Angle,&Measure_Angle);
}

//角度环
PID_Postion_Cal(&PID_ROL_Angle,Target_Angle.rol,Measure_Angle.rol);//ROLL角度环PID （输入角度输出角速度）
PID_Postion_Cal(&PID_PIT_Angle,Target_Angle.pit,Measure_Angle.pit);//PITH角度环PID （输入角度输出角速度）
// PID_Postion_Cal(&PID_YAW_Angle,Target_Angle.yaw,Measure_Angle.yaw);//YAW角度环PID  （输入角度输出角速度）

//角速度环
PID_Postion_Cal(&PID_ROL_Rate,PID_ROL_Angle.OutPut,(gyr_in->Y*RadtoDeg)); //ROLL角速度环PID （输入角度环的输出，输出电机控制量）
PID_Postion_Cal(&PID_PIT_Rate,PID_PIT_Angle.OutPut,-(gyr_in->X*RadtoDeg)); //PITH角速度环PID （输入角度环的输出，输出电机控制量）
PID_Postion_Cal(&PID_YAW_Rate,Target_Angle.yaw*PID_YAW_Angle.P,gyr_in->Z*RadtoDeg); //YAW角速度环PID （输入角度，输出电机控制量）

//动力分配（自己DIY是动力分配一定要好好研究，动力分配搞错飞机肯定飞不起来!!!）
if(rc_in->THROTTLE>180&&armed)//当油门大于150时和飞机解锁时动力分配才生效
{
Moto_PWM_1 = rc_in->THROTTLE + PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;
Moto_PWM_2 = rc_in->THROTTLE - PID_ROL_Rate.OutPut - PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;
Moto_PWM_3 = rc_in->THROTTLE - PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut - PID_YAW_Rate.OutPut;
Moto_PWM_4 = rc_in->THROTTLE + PID_ROL_Rate.OutPut + PID_PIT_Rate.OutPut + PID_YAW_Rate.OutPut;
}
else
{
Moto_PWM_1 = 0;
Moto_PWM_2 = 0;
Moto_PWM_3 = 0;
Moto_PWM_4 = 0;
}
Moto_Pwm(Moto_PWM_1,Moto_PWM_2,Moto_PWM_3,Moto_PWM_4); //将此数值分配到定时器，输出对应占空比的PWM波
}

/******************************************************************************************
* 函  数：int16_t Yaw_Control(float TARGET_YAW)
* 功  能：航向角不回中控制
* 参  数：TARGET_YAW 目标航向角
* 返回值：针对目标航向角计算出的航向角
* 备  注：由于遥控器航向舵会自动回中，所以需要对目标航向角进行不回中处理;
*******************************************************************************************/
int16_t Yaw_Control(float TARGET_YAW)
{
static int16_t YAW=0; //根据目标航向角计算出的不回中角度
if(Airplane_Enable)
{
if(SI24R1_Controlflag) //遥控器控制航向角
{
if(TARGET_YAW>2) //目标航向角为正时YAW增大
YAW +=2;
if(TARGET_YAW<-2) //目标航向角为负时YAW减小
YAW -=2;
}
}
return YAW;
}

/******************************************************************************************
* 函  数：void Yaw_Carefree(FLOAT_ANGLE *Target_Angle, const FLOAT_ANGLE *Measure_Angle)
* 功  能：无头角度控制
* 参  数：*Target_Angle 指向目标姿态角的指针
*       *Measure_Angle 测量姿态角的指针
* 返回值：针对目标航向角计算出的航向角
* 备  注：无头模式需调用此函数
*******************************************************************************************/
void Yaw_Carefree(FLOAT_ANGLE *Target_Angle, const FLOAT_ANGLE *Measure_Angle)
{
float yawRad = fabs(Measure_Angle->yaw) * DegtoRad;
float cosy = cosf(yawRad);
float siny = sinf(yawRad);
float originalRoll = Target_Angle->rol;
float originalPitch = Target_Angle->pit;

Target_Angle->rol = originalRoll * cosy + originalPitch * siny;
Target_Angle->pit = originalPitch * cosy - originalRoll * siny;
}
/******************************************************************************************
* 函  数：void Safety_Check(void)
* 功  能：飞机姿态安全监测
* 参  数：无
* 返回值：无
* 备  注：如果飞机角度和加速度异常就将飞机上锁并停止电机，防止电机狂转打坏桨叶
*******************************************************************************************/
void Safety_Check(void)
{
  if((fabs(Att_Angle.pit)>45.0f||fabs(Att_Angle.rol)>45.0f) && (fabs(Acc_filt.X)>9.0f||fabs(Acc_filt.Y)>9.0f))
{
   Airplane_Enable = 0;
   Moto_PWM_1 = 0;
   Moto_PWM_2 = 0;
   Moto_PWM_3 = 0;
   Moto_PWM_4 = 0;
}
}