Skip to content

Instantly share code, notes, and snippets.

@skyzh

skyzh/bsp_can.c

Last active November 8, 2018 05:50
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save skyzh/ac98d2143bb88d04cb6556a08aede814 to your computer and use it in GitHub Desktop.
Save skyzh/ac98d2143bb88d04cb6556a08aede814 to your computer and use it in GitHub Desktop.
Download ZIP
Dajiang C620 PID from official website
Raw
bsp_can.c
/******************************************************************************
/// @brief
/// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.>
/// @license MIT License
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction,including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell
/// copies of the Software, and to permit persons to whom the Software is furnished
/// to do so,subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
/// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
*******************************************************************************/
#include "can.h"
#include "bsp_can.h"
#include "cmsis_os.h"
//moto_measure_t moto_pit;
//moto_measure_t moto_yaw;
//moto_measure_t moto_poke; //拨单电机
moto_measure_t moto_chassis[4] = {0};//4 chassis moto
moto_measure_t moto_info;
void get_total_angle(moto_measure_t *p);
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);
/*******************************************************************************************
* @Func my_can_filter_init
* @Brief CAN1和CAN2滤波器配置
* @Param CAN_HandleTypeDef* hcan
* @Retval None
* @Date 2015/11/30
*******************************************************************************************/
void my_can_filter_init_recv_all(CAN_HandleTypeDef* _hcan)
{
//can1 &can2 use same filter config
CAN_FilterConfTypeDef CAN_FilterConfigStructure;
static CanTxMsgTypeDef Tx1Message;
static CanRxMsgTypeDef Rx1Message;
static CanTxMsgTypeDef Tx2Message;
static CanRxMsgTypeDef Rx2Message;
CAN_FilterConfigStructure.FilterNumber = 0;
CAN_FilterConfigStructure.FilterMode = CAN_FILTERMODE_IDMASK;
CAN_FilterConfigStructure.FilterScale = CAN_FILTERSCALE_32BIT;
CAN_FilterConfigStructure.FilterIdHigh = 0x0000;
CAN_FilterConfigStructure.FilterIdLow = 0x0000;
CAN_FilterConfigStructure.FilterMaskIdHigh = 0x0000;
CAN_FilterConfigStructure.FilterMaskIdLow = 0x0000;
CAN_FilterConfigStructure.FilterFIFOAssignment = CAN_FilterFIFO0;
CAN_FilterConfigStructure.BankNumber = 14;//can1(0-13)和can2(14-27)分别得到一半的filter
CAN_FilterConfigStructure.FilterActivation = ENABLE;
if(HAL_CAN_ConfigFilter(_hcan, &CAN_FilterConfigStructure) != HAL_OK)
{
//err_deadloop(); //show error!
}
//filter config for can2
//can1(0-13)和can2(14-27)分别得到一半的filter
CAN_FilterConfigStructure.FilterNumber = 14;
if(HAL_CAN_ConfigFilter(_hcan, &CAN_FilterConfigStructure) != HAL_OK)
{
//err_deadloop();
}
if(_hcan == &hcan1){
_hcan->pTxMsg = &Tx1Message;
_hcan->pRxMsg = &Rx1Message;
}
if(_hcan == &hcan2){
_hcan->pTxMsg = &Tx2Message;
_hcan->pRxMsg = &Rx2Message;
}
}
/*******************************************************************************************
* @Func void can_filter_recv_special(CAN_HandleTypeDef* _hcan, s16 id)
* @Brief 待测试！！！
* @Param 只接收filtered id，其他的全屏蔽。
* @Retval eg： CAN1_FilterConfiguration(0, HOST_CONTROL_ID);
CAN1_FilterConfiguration(1, SET_CURRENT_ID);
CAN1_FilterConfiguration(2, SET_VOLTAGE_ID);
CAN1_FilterConfiguration(3, ESC_CAN_DEVICE_ID);
CAN1_FilterConfiguration(4, SET_POWER_ID);
CAN1_FilterConfiguration(5, SET_LIMIT_RECOVER_ID);
* @Date 2016年11月11日
*******************************************************************************************/
void can_filter_recv_special(CAN_HandleTypeDef* hcan, uint8_t filter_number, uint16_t filtered_id)
{
CAN_FilterConfTypeDef cf;
cf.FilterNumber = filter_number; //过滤器组编号
cf.FilterMode = CAN_FILTERMODE_IDMASK; //id屏蔽模式
cf.FilterScale = CAN_FILTERSCALE_32BIT; //32bit 滤波
cf.FilterIdHigh = (filtered_id<<21) >> 16; //high 16 bit 其实这两个结构体成员变量是16位宽
cf.FilterIdLow = filtered_id<<21; //low 16bit
cf.FilterMaskIdHigh = 0xFFFF;
cf.FilterMaskIdLow = 0xFFF8; //IDE[2], RTR[1] TXRQ[0] 低三位不考虑。
cf.FilterFIFOAssignment = CAN_FilterFIFO0;
cf.BankNumber = 14; //can1(0-13)和can2(14-27)分别得到一半的filter
cf.FilterActivation = ENABLE;
HAL_CAN_ConfigFilter(hcan, &cf);
}
HAL_StatusTypeDef can_send_msg()
{
// if(_hcan->Instance->ESR){
// //can error occured, sleep can and reset!
// _hcan->Instance->MCR |= 0x02;
// _hcan->Instance->MCR &= ~(0x02);
// }//这个是zw试过的可以解决can错误 有待验证！
return HAL_OK;
}
float ZGyroModuleAngle;
/*******************************************************************************************
* @Func void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* _hcan)
* @Brief 这是一个回调函数,都不用声明
* @Param
* @Retval None
* @Date 2015/11/24
*******************************************************************************************/
void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* _hcan)
{
//ignore can1 or can2.
switch(_hcan->pRxMsg->StdId){
case CAN_3510Moto1_ID:
case CAN_3510Moto2_ID:
case CAN_3510Moto3_ID:
case CAN_3510Moto4_ID:
{
static u8 i;
i = _hcan->pRxMsg->StdId - CAN_3510Moto1_ID;
moto_chassis[i].msg_cnt++ <= 50 ? get_moto_offset(&moto_chassis[i], _hcan) : get_moto_measure(&moto_chassis[i], _hcan);
get_moto_measure(&moto_info, _hcan);
//get_moto_measure(&moto_chassis[i], _hcan);
}
break;
}
//hcan1.Instance->IER|=0x00008F02;
/*#### add enable can it again to solve can receive only one ID problem!!!####**/
__HAL_CAN_ENABLE_IT(&hcan1, CAN_IT_FMP0);
__HAL_CAN_ENABLE_IT(&hcan2, CAN_IT_FMP0);
/*#### add enable can it again to solve can receive only one ID problem!!!####**/
}
/*******************************************************************************************
* @Func void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
* @Brief 接收云台电机,3510电机通过CAN发过来的信息
* @Param
* @Retval None
* @Date 2015/11/24
*******************************************************************************************/
void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
// u32 sum=0;
// u8 i = FILTER_BUF_LEN;
/*BUG!!! dont use this para code*/
// ptr->angle_buf[ptr->buf_idx] = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
// ptr->buf_idx = ptr->buf_idx++ > FILTER_BUF_LEN ? 0 : ptr->buf_idx;
// while(i){
// sum += ptr->angle_buf[--i];
// }
// ptr->fited_angle = sum / FILTER_BUF_LEN;
ptr->last_angle = ptr->angle;
ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
ptr->real_current = (int16_t)(hcan->pRxMsg->Data[2]<<8 | hcan->pRxMsg->Data[3]);
ptr->speed_rpm = ptr->real_current; //这里是因为两种电调对应位不一样的信息
ptr->given_current = (int16_t)(hcan->pRxMsg->Data[4]<<8 | hcan->pRxMsg->Data[5])/-5;
ptr->hall = hcan->pRxMsg->Data[6];
if(ptr->angle - ptr->last_angle > 4096)
ptr->round_cnt --;
else if (ptr->angle - ptr->last_angle < -4096)
ptr->round_cnt ++;
ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
}
/*this function should be called after system+can init */
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
ptr->offset_angle = ptr->angle;
}
#define ABS(x) ( (x>0) ? (x) : (-x) )
/**
*@bref 电机上电角度=0， 之后用这个函数更新3510电机的相对开机后（为0）的相对角度。
*/
void get_total_angle(moto_measure_t *p){
int res1, res2, delta;
if(p->angle < p->last_angle){ //可能的情况
res1 = p->angle + 8192 - p->last_angle; //正转，delta=+
res2 = p->angle - p->last_angle; //反转 delta=-
}else{ //angle > last
res1 = p->angle - 8192 - p->last_angle ;//反转 delta -
res2 = p->angle - p->last_angle; //正转 delta +
}
//不管正反转，肯定是转的角度小的那个是真的
if(ABS(res1)<ABS(res2))
delta = res1;
else
delta = res2;
p->total_angle += delta;
p->last_angle = p->angle;
}
void set_moto_current(CAN_HandleTypeDef* hcan, s16 iq1, s16 iq2, s16 iq3, s16 iq4){
hcan->pTxMsg->StdId = 0x200;
hcan->pTxMsg->IDE = CAN_ID_STD;
hcan->pTxMsg->RTR = CAN_RTR_DATA;
hcan->pTxMsg->DLC = 0x08;
hcan->pTxMsg->Data[0] = iq1 >> 8;
hcan->pTxMsg->Data[1] = iq1;
hcan->pTxMsg->Data[2] = iq2 >> 8;
hcan->pTxMsg->Data[3] = iq2;
hcan->pTxMsg->Data[4] = iq3 >> 8;
hcan->pTxMsg->Data[5] = iq3;
hcan->pTxMsg->Data[6] = iq4 >> 8;
hcan->pTxMsg->Data[7] = iq4;
HAL_CAN_Transmit(hcan, 1000);
}
Raw
bsp_can.h
/******************************************************************************
/// @brief
/// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.>
/// @license MIT License
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction,including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell
/// copies of the Software, and to permit persons to whom the Software is furnished
/// to do so,subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
/// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
*******************************************************************************/
#ifndef __BSP_CAN
#define __BSP_CAN
#ifdef STM32F4
#include "stm32f4xx_hal.h"
#elif defined STM32F1
#include "stm32f1xx_hal.h"
#endif
#include "mytype.h"
#include "can.h"
/*CAN发送或是接收的ID*/
typedef enum
{
CAN_TxPY12V_ID = 0x200, //云台12V发送ID
CAN_TxPY24V_ID = 0x1FF, //云台12V发送ID
// CAN_Pitch_ID = 0x201, //云台Pitch
// CAN_Yaw_ID = 0x203, //云台Yaw
CAN_YAW_FEEDBACK_ID = 0x205, //云台Yaw24v
CAN_PIT_FEEDBACK_ID = 0x206, //云台Yaw24v
CAN_POKE_FEEDBACK_ID = 0x207,
CAN_ZGYRO_RST_ID = 0x404,
CAN_ZGYRO_FEEDBACK_MSG_ID = 0x401,
CAN_MotorLF_ID = 0x041, //左前
CAN_MotorRF_ID = 0x042, //右前
CAN_MotorLB_ID = 0x043, //左后
CAN_MotorRB_ID = 0x044, //右后
CAN_EC60_four_ID = 0x200, //EC60接收程序
CAN_backLeft_EC60_ID = 0x203, //ec60
CAN_frontLeft_EC60_ID = 0x201, //ec60
CAN_backRight_EC60_ID = 0x202, //ec60
CAN_frontRight_EC60_ID = 0x204, //ec60
//add by langgo
CAN_3510Moto_ALL_ID = 0x200,
CAN_3510Moto1_ID = 0x201,
CAN_3510Moto2_ID = 0x202,
CAN_3510Moto3_ID = 0x203,
CAN_3510Moto4_ID = 0x204,
CAN_DriverPower_ID = 0x80,
CAN_HeartBeat_ID = 0x156,
}CAN_Message_ID;
#define FILTER_BUF_LEN 5
/*接收到的云台电机的参数结构体*/
typedef struct{
int16_t speed_rpm;
int16_t real_current;
int16_t given_current;
uint8_t hall;
uint16_t angle; //abs angle range:[0,8191]
uint16_t last_angle; //abs angle range:[0,8191]
uint16_t offset_angle;
int32_t round_cnt;
int32_t total_angle;
u8 buf_idx;
u16 angle_buf[FILTER_BUF_LEN];
u16 fited_angle;
u32 msg_cnt;
}moto_measure_t;
/* Extern ------------------------------------------------------------------*/
extern moto_measure_t moto_chassis[];
extern moto_measure_t moto_yaw,moto_pit,moto_poke,moto_info;
extern float real_current_from_judgesys; //unit :mA
extern float dynamic_limit_current; //unit :mA,; //from judge_sys
extern float ZGyroModuleAngle,yaw_zgyro_angle;
void my_can_filter_init(CAN_HandleTypeDef* hcan);
void my_can_filter_init_recv_all(CAN_HandleTypeDef* _hcan);
void can_filter_recv_special(CAN_HandleTypeDef* hcan, uint8_t filter_number, uint16_t filtered_id);
void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);
void can_receive_onetime(CAN_HandleTypeDef* _hcan, int time);
void set_moto_current(CAN_HandleTypeDef* hcan, s16 iq1, s16 iq2, s16 iq3, s16 iq4);
#endif
Raw
mytype.h
/******************************************************************************
/// @brief
/// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.>
/// @license MIT License
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction,including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell
/// copies of the Software, and to permit persons to whom the Software is furnished
/// to do so,subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
/// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
*******************************************************************************/
//#pragma once
#ifndef __MYTYPE__
#define __MYTYPE__
#include "stm32f4xx_hal.h"
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <stdbool.h>
typedef uint8_t u8;
typedef uint16_t u16;
typedef uint32_t u32;
typedef int8_t s8;
typedef int16_t s16;
typedef int32_t s32;
typedef volatile uint8_t vu8;
typedef volatile uint16_t vu16;
typedef volatile uint32_t vu32;
typedef volatile int8_t vs8;
typedef volatile int16_t vs16;
typedef volatile int32_t vs32;
typedef union{
s8 s8_fmt[8]; //for angle and omega
u8 u8_fmt[8]; //for angle and omega
char ch_fmt[8]; //
s16 s16_fmt[4];
u16 u16_fmt[4];
s32 s32_fmt[2];
u32 u32_fmt[2];
float f32_fmt[2];
double d64_fmt;
}data_convert_ut; //for diaobi gyro
#endif
Raw
pid.c
/******************************************************************************
/// @brief
/// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.>
/// @license MIT License
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction,including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell
/// copies of the Software, and to permit persons to whom the Software is furnished
/// to do so,subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
/// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
*******************************************************************************/
/**
******************************************************************************
* @file pid.c
* @version V1.0.0
* @date 2016年11月11日17:21:36
* @brief 对于PID， 反馈/测量习惯性叫get/measure/real/fdb,
期望输入一般叫set/target/ref
*******************************************************************************/
/* Includes ------------------------------------------------------------------*/
#include "pid.h"
#include "mytype.h"
#include <math.h>
//#include "cmsis_os.h"
#define ABS(x) ((x>0)? (x): (-x))
void abs_limit(float *a, float ABS_MAX){
if(*a > ABS_MAX)
*a = ABS_MAX;
if(*a < -ABS_MAX)
*a = -ABS_MAX;
}
/*参数初始化--------------------------------------------------------------*/
static void pid_param_init(
pid_t *pid,
uint32_t mode,
uint32_t maxout,
uint32_t intergral_limit,
float kp,
float ki,
float kd)
{
pid->IntegralLimit = intergral_limit;
pid->MaxOutput = maxout;
pid->pid_mode = mode;
pid->p = kp;
pid->i = ki;
pid->d = kd;
}
/*中途更改参数设定(调试)------------------------------------------------------------*/
static void pid_reset(pid_t *pid, float kp, float ki, float kd)
{
pid->p = kp;
pid->i = ki;
pid->d = kd;
}
/**
*@bref. calculate delta PID and position PID
*@param[in] set： target
*@param[in] real measure
*/
float pid_calc(pid_t* pid, float get, float set){
pid->get[NOW] = get;
pid->set[NOW] = set;
pid->err[NOW] = set - get; //set - measure
if (pid->max_err != 0 && ABS(pid->err[NOW]) > pid->max_err )
return 0;
if (pid->deadband != 0 && ABS(pid->err[NOW]) < pid->deadband)
return 0;
if(pid->pid_mode == POSITION_PID) //位置式p
{
pid->pout = pid->p * pid->err[NOW];
pid->iout += pid->i * pid->err[NOW];
pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST] );
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->pos_out = pid->pout + pid->iout + pid->dout;
abs_limit(&(pid->pos_out), pid->MaxOutput);
pid->last_pos_out = pid->pos_out; //update last time
}
else if(pid->pid_mode == DELTA_PID)//增量式P
{
pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
pid->iout = pid->i * pid->err[NOW];
pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->delta_u = pid->pout + pid->iout + pid->dout;
pid->delta_out = pid->last_delta_out + pid->delta_u;
abs_limit(&(pid->delta_out), pid->MaxOutput);
pid->last_delta_out = pid->delta_out; //update last time
}
pid->err[LLAST] = pid->err[LAST];
pid->err[LAST] = pid->err[NOW];
pid->get[LLAST] = pid->get[LAST];
pid->get[LAST] = pid->get[NOW];
pid->set[LLAST] = pid->set[LAST];
pid->set[LAST] = pid->set[NOW];
return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
//
}
/**
*@bref. special calculate position PID @attention @use @gyro data!!
*@param[in] set： target
*@param[in] real measure
*/
float pid_sp_calc(pid_t* pid, float get, float set, float gyro){
pid->get[NOW] = get;
pid->set[NOW] = set;
pid->err[NOW] = set - get; //set - measure
if(pid->pid_mode == POSITION_PID) //位置式p
{
pid->pout = pid->p * pid->err[NOW];
if(fabs(pid->i) >= 0.001f)
pid->iout += pid->i * pid->err[NOW];
else
pid->iout = 0;
pid->dout = -pid->d * gyro/100.0f;
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->pos_out = pid->pout + pid->iout + pid->dout;
abs_limit(&(pid->pos_out), pid->MaxOutput);
pid->last_pos_out = pid->pos_out; //update last time
}
else if(pid->pid_mode == DELTA_PID)//增量式P
{
// pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
// pid->iout = pid->i * pid->err[NOW];
// pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
//
// abs_limit(&(pid->iout), pid->IntegralLimit);
// pid->delta_u = pid->pout + pid->iout + pid->dout;
// pid->delta_out = pid->last_delta_out + pid->delta_u;
// abs_limit(&(pid->delta_out), pid->MaxOutput);
// pid->last_delta_out = pid->delta_out; //update last time
}
pid->err[LLAST] = pid->err[LAST];
pid->err[LAST] = pid->err[NOW];
pid->get[LLAST] = pid->get[LAST];
pid->get[LAST] = pid->get[NOW];
pid->set[LLAST] = pid->set[LAST];
pid->set[LAST] = pid->set[NOW];
return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
//
}
/*pid总体初始化-----------------------------------------------------------------*/
void PID_struct_init(
pid_t* pid,
uint32_t mode,
uint32_t maxout,
uint32_t intergral_limit,
float kp,
float ki,
float kd)
{
/*init function pointer*/
pid->f_param_init = pid_param_init;
pid->f_pid_reset = pid_reset;
// pid->f_cal_pid = pid_calc;
// pid->f_cal_sp_pid = pid_sp_calc; //addition
/*init pid param */
pid->f_param_init(pid, mode, maxout, intergral_limit, kp, ki, kd);
}
//pid_t pid_rol = {0};
//pid_t pid_pit = {0};
//pid_t pid_yaw = {0};
//pid_t pid_yaw_omg = {0};//角速度环
//pid_t pid_pit_omg = {0};//角速度环
//pid_t pid_yaw_alfa = {0}; //angle acce
//pid_t pid_chassis_angle={0};
//pid_t pid_poke = {0};
//pid_t pid_poke_omg = {0};
//pid_t pid_imu_tmp;
//pid_t pid_x;
//pid_t pid_cali_bby;
//pid_t pid_cali_bbp;
pid_t pid_omg;
pid_t pid_pos;
pid_t pid_spd[4];
void pid_test_init(){
//为了解决上位机调参的时候第一次赋值的时候清零其他参数， 应该提前把参数表填充一下！
}
Raw
pid.h
/******************************************************************************
/// @brief
/// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.>
/// @license MIT License
/// Permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to deal
/// in the Software without restriction,including without limitation the rights
/// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell
/// copies of the Software, and to permit persons to whom the Software is furnished
/// to do so,subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions of the Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
/// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
/// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
/// THE SOFTWARE.
*******************************************************************************/
#ifndef __pid_H
#define __pid_H
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
enum{
LLAST = 0,
LAST = 1,
NOW = 2,
POSITION_PID,
DELTA_PID,
};
typedef struct __pid_t
{
float p;
float i;
float d;
float set[3]; //目标值,包含NOW， LAST， LLAST上上次
float get[3]; //测量值
float err[3]; //误差
float pout; //p输出
float iout; //i输出
float dout; //d输出
float pos_out; //本次位置式输出
float last_pos_out; //上次输出
float delta_u; //本次增量值
float delta_out; //本次增量式输出 = last_delta_out + delta_u
float last_delta_out;
float max_err;
float deadband; //err < deadband return
uint32_t pid_mode;
uint32_t MaxOutput; //输出限幅
uint32_t IntegralLimit; //积分限幅
void (*f_param_init)(struct __pid_t *pid, //PID参数初始化
uint32_t pid_mode,
uint32_t maxOutput,
uint32_t integralLimit,
float p,
float i,
float d);
void (*f_pid_reset)(struct __pid_t *pid, float p, float i, float d); //pid三个参数修改
}pid_t;
void PID_struct_init(
pid_t* pid,
uint32_t mode,
uint32_t maxout,
uint32_t intergral_limit,
float kp,
float ki,
float kd);
float pid_calc(pid_t* pid, float fdb, float ref);
extern pid_t pid_rol;
extern pid_t pid_pit;
extern pid_t pid_yaw;
extern pid_t pid_pit_omg;
extern pid_t pid_yaw_omg;
extern pid_t pid_spd[4];
extern pid_t pid_yaw_alfa;
extern pid_t pid_chassis_angle;
extern pid_t pid_poke;
extern pid_t pid_poke_omg;
extern pid_t pid_imu_tmp; //imu_temperature
extern pid_t pid_cali_bby; //big buff yaw
extern pid_t pid_cali_bbp;
extern pid_t pid_omg;
extern pid_t pid_pos;
#endif
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment