570 lines
18 KiB
C
570 lines
18 KiB
C
/*
|
||
* 云台模组
|
||
模式自行选择：遥控器改变或自行初始化
|
||
delta获取从遥控器自行获取
|
||
example:
|
||
Gimbal_t gimbal;
|
||
Gimbal_IMU_t gimbal_imu;
|
||
Gimbal_CMD_t gimbal_cmd;
|
||
|
||
Gimbal_Init(&gimbal,&Config_GetRobotParam()->gimbal_param,GIMBAL_CTRL_FREQ);
|
||
|
||
while(1){
|
||
//这里写遥控器获取gimbal_cmd数据
|
||
if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0))
|
||
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
|
||
|
||
Gimbal_UpdateFeedback(&gimbal);
|
||
Gimbal_Control(&gimbal,&gimbal_cmd);
|
||
Gimbal_Output(&gimbal);
|
||
}
|
||
*/
|
||
|
||
/* Includes ----------------------------------------------------------------- */
|
||
#include "module\gimbal.h"
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private define ----------------------------------------------------------- */
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private variables -------------------------------------------------------- */
|
||
/* Private function -------------------------------------------------------- */
|
||
/**
|
||
* \brief 计算角度偏差
|
||
*
|
||
* \param
|
||
* \param
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
static float motor_imu_offset(float* motor, float* imu){
|
||
|
||
float motor_imu_offset = motor - imu;
|
||
/* 处理跨越±π的情况 */
|
||
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
|
||
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
|
||
|
||
return motor_imu_offset;
|
||
}
|
||
|
||
/**
|
||
* \brief 设置云台模式
|
||
*
|
||
* \param c 包含云台数据的结构体
|
||
* \param mode 要设置的模式
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
||
if (g == NULL)
|
||
return -1;
|
||
if (mode == g->mode)
|
||
return GIMBAL_OK;
|
||
|
||
PID_Reset(&g->pid.major_yaw_angle);
|
||
PID_Reset(&g->pid.major_yaw_omega);
|
||
|
||
PID_Reset(&g->pid.yaw_angle);
|
||
PID_Reset(&g->pid.yaw_omega);
|
||
PID_Reset(&g->pid.pit_angle);
|
||
PID_Reset(&g->pid.pit_omega);
|
||
LowPassFilter2p_Reset(&g->filter_out.major_yaw, 0.0f);
|
||
|
||
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
|
||
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
|
||
|
||
|
||
|
||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
||
|
||
g->setpoint.eulr.pit = g->feedback.imu.eulr.pit;
|
||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
||
|
||
|
||
g->mode = mode;
|
||
return 0;
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
|
||
/**
|
||
* \brief 初始化云台
|
||
*
|
||
* \param g 包含云台数据的结构体
|
||
* \param param 包含云台参数的结构体指针
|
||
* \param target_freq 任务预期的运行频率
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
|
||
float target_freq) {
|
||
if (g == NULL)
|
||
return -1;
|
||
|
||
g->param = param;
|
||
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
|
||
|
||
/* 初始化云台电机控制PID和LPF */
|
||
PID_Init(&(g->pid.major_yaw_angle), KPID_MODE_CALC_D, target_freq,
|
||
&(g->param->pid.major_yaw_angle));
|
||
PID_Init(&(g->pid.major_yaw_omega), KPID_MODE_CALC_D, target_freq,
|
||
&(g->param->pid.major_yaw_omega));
|
||
|
||
PID_Init(&(g->pid.yaw_angle), KPID_MODE_CALC_D, target_freq,
|
||
&(g->param->pid.yaw_angle));
|
||
PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
|
||
&(g->param->pid.yaw_omega));
|
||
PID_Init(&(g->pid.pit_angle), KPID_MODE_CALC_D, target_freq,
|
||
&(g->param->pid.pit_angle));
|
||
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
|
||
&(g->param->pid.pit_omega));
|
||
|
||
|
||
|
||
g->limit.yaw.max = g->param->Limit_t.zero.yaw_encoder+g->param->Limit_t.zero.travel.yaw;
|
||
g->limit.yaw.min = g->param->Limit_t.zero.yaw_encoder;
|
||
|
||
LowPassFilter2p_Init(&g->filter_out.major_yaw, target_freq,
|
||
g->param->low_pass_cutoff_freq.out);
|
||
|
||
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
|
||
g->param->low_pass_cutoff_freq.out);
|
||
LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
|
||
g->param->low_pass_cutoff_freq.out);
|
||
|
||
BSP_CAN_Init();
|
||
/* 大yaw电机注册 */
|
||
if(g->param->motor.major_yaw==RM)
|
||
MOTOR_RM_Register(&(g->param->motor.major_yaw_rm_motor));
|
||
|
||
if(g->param->motor.major_yaw==DM){
|
||
MOTOR_DM_Register(&(g->param->motor.major_yaw_dm_motor));
|
||
MOTOR_DM_Enable(&(g->param->motor.major_yaw_dm_motor));
|
||
}
|
||
|
||
/*大疆电机注册*/
|
||
if(g->param->motor.yaw==RM)
|
||
MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor));
|
||
if(g->param->motor.pit==RM)
|
||
MOTOR_RM_Register(&(g->param->motor.pit_rm_motor));
|
||
|
||
/*达妙电机注册*/
|
||
if(g->param->motor.yaw==DM){
|
||
MOTOR_DM_Register(&(g->param->motor.yaw_dm_motor));
|
||
MOTOR_DM_Enable(&(g->param->motor.yaw_dm_motor));
|
||
}
|
||
|
||
if(g->param->motor.pit==DM){
|
||
MOTOR_DM_Register(&(g->param->motor.pit_dm_motor));
|
||
MOTOR_DM_Enable(&(g->param->motor.pit_dm_motor));
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
static int8_t Gimbal_Direction(Gimbal_t *g){
|
||
|
||
if (g == NULL) {
|
||
return -1;
|
||
}
|
||
/* 欧拉角方向选择 */
|
||
switch(g->param->Direction.Eulr.pit){
|
||
case Pit:
|
||
g->direction.Eulr.pit=g->feedback.imu.eulr.pit;
|
||
break;
|
||
case Yaw:
|
||
g->direction.Eulr.pit=g->feedback.imu.eulr.yaw;
|
||
break;
|
||
case Rol:
|
||
g->direction.Eulr.pit=-g->feedback.imu.eulr.rol;
|
||
break;
|
||
}
|
||
|
||
switch(g->param->Direction.Eulr.yaw){
|
||
case Pit:
|
||
g->direction.Eulr.yaw=g->feedback.imu.eulr.pit;
|
||
break;
|
||
case Yaw:
|
||
g->direction.Eulr.yaw=g->feedback.imu.eulr.yaw;
|
||
break;
|
||
case Rol:
|
||
g->direction.Eulr.yaw=g->feedback.imu.eulr.rol;
|
||
break;
|
||
}
|
||
/* 角速度方向选择 */
|
||
switch(g->param->Direction.Gyro.pit){
|
||
case Gyro_x:
|
||
g->direction.Gyro.pit=g->feedback.imu.gyro.x;
|
||
break;
|
||
case Gyro_y:
|
||
g->direction.Gyro.pit=g->feedback.imu.gyro.y;
|
||
break;
|
||
case Gyro_z:
|
||
g->direction.Gyro.pit=g->feedback.imu.gyro.z;
|
||
break;
|
||
}
|
||
|
||
switch(g->param->Direction.Gyro.yaw){
|
||
case Gyro_x:
|
||
g->direction.Gyro.yaw=g->feedback.imu.gyro.x;
|
||
break;
|
||
case Gyro_y:
|
||
g->direction.Gyro.yaw=g->feedback.imu.gyro.y;
|
||
break;
|
||
case Gyro_z:
|
||
g->direction.Gyro.yaw=g->feedback.imu.gyro.z;
|
||
break;
|
||
}
|
||
g->direction.Quaternion_t.q0=g->feedback.imu.quat.q0;
|
||
g->direction.Quaternion_t.q1=g->feedback.imu.quat.q1;
|
||
g->direction.Quaternion_t.q2=g->feedback.imu.quat.q2;
|
||
g->direction.Quaternion_t.q3=g->feedback.imu.quat.q3;
|
||
|
||
return 0;
|
||
}
|
||
/**
|
||
* \brief 通过CAN设备更新云台反馈信息
|
||
*
|
||
* \param gimbal 云台
|
||
* \param can CAN设备
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
|
||
if (gimbal == NULL)
|
||
return -1;
|
||
|
||
|
||
/* 更新大yaw电机反馈数据（RM和DM） */
|
||
if(gimbal->param->motor.major_yaw==RM){
|
||
MOTOR_RM_Update(&(gimbal->param->motor.major_yaw_rm_motor));
|
||
MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.major_yaw_rm_motor));
|
||
if(rm_motor_yaw != NULL)
|
||
gimbal->feedback.motor.major_yaw = rm_motor_yaw->feedback;
|
||
}
|
||
|
||
if(gimbal->param->motor.major_yaw==DM){
|
||
MOTOR_DM_Update(&(gimbal->param->motor.major_yaw_dm_motor));
|
||
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.major_yaw_dm_motor));
|
||
if (dm_motor != NULL) {
|
||
gimbal->feedback.motor.major_yaw = dm_motor->motor.feedback;
|
||
}
|
||
}
|
||
|
||
/* 更新RM电机反馈数据 */
|
||
if(gimbal->param->motor.yaw==RM){
|
||
MOTOR_RM_Update(&(gimbal->param->motor.yaw_rm_motor));
|
||
MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.yaw_rm_motor));
|
||
if(rm_motor_yaw != NULL)
|
||
gimbal->feedback.motor.yaw = rm_motor_yaw->feedback;
|
||
}
|
||
|
||
|
||
if(gimbal->param->motor.pit==RM){
|
||
MOTOR_RM_Update(&(gimbal->param->motor.pit_rm_motor));
|
||
MOTOR_RM_t *rm_motor_pit = MOTOR_RM_GetMotor(&(gimbal->param->motor.pit_rm_motor));
|
||
if (rm_motor_pit != NULL)
|
||
gimbal->feedback.motor.pit = rm_motor_pit->feedback;
|
||
}
|
||
|
||
/* 更新DM电机反馈数据 */
|
||
if(gimbal->param->motor.yaw==DM){
|
||
MOTOR_DM_Update(&(gimbal->param->motor.yaw_dm_motor));
|
||
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.yaw_dm_motor));
|
||
if (dm_motor != NULL) {
|
||
gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
|
||
}
|
||
}
|
||
if(gimbal->param->motor.pit==DM){
|
||
MOTOR_DM_Update(&(gimbal->param->motor.pit_dm_motor));
|
||
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.pit_dm_motor));
|
||
if (dm_motor != NULL) {
|
||
gimbal->feedback.motor.pit = dm_motor->motor.feedback;
|
||
}
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
||
|
||
if (gimbal == NULL) {
|
||
return -1;
|
||
}
|
||
gimbal->feedback.imu.gyro = imu->gyro;
|
||
gimbal->feedback.imu.eulr = imu->eulr;
|
||
gimbal->feedback.imu.accl = imu->accl;
|
||
gimbal->feedback.imu.quat = imu->quat;
|
||
Gimbal_Direction(gimbal);
|
||
}
|
||
/**
|
||
* \brief 运行大yaw控制逻辑
|
||
*
|
||
* \param g 包含云台数据的结构体
|
||
* \param
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t major_yaw_Control(Gimbal_t *g){
|
||
|
||
/*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
|
||
/*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
|
||
跟随小YAW向相同方向运动，将遥控数据进行分配*/
|
||
|
||
/*小YAW中点*/
|
||
float small_yaw_center_offset = g->param->Limit_t.zero.yaw_encoder +
|
||
g->param->Limit_t.zero.travel.yaw* 0.5f;
|
||
g->setpoint.major_yaw=small_yaw_center_offset;
|
||
|
||
// /* 小yaw是否开启 */
|
||
// float err = g->feedback.motor.yaw.rotor_abs_angle - small_yaw_center_offset;
|
||
|
||
// float threshold = (g->param->Limit_t.zero.travel.yaw * 0.35f);
|
||
// /* 超出阈值范围，让大yaw跟随 */
|
||
// if (fabsf(err) > threshold) {
|
||
// /* 此时让大yaw动起来，跟随err的符号 */
|
||
//// g->setpoint.major_yaw = g->feedback.motor.major_yaw.rotor_abs_angle - err;
|
||
// g->setpoint.major_yaw = small_yaw_center_offset;;
|
||
// } else {
|
||
// /* 阈值以内，大yaw不动*/
|
||
// g->setpoint.major_yaw = g->feedback.motor.major_yaw.rotor_abs_angle; //当前值，不动
|
||
// }
|
||
return 0;
|
||
}
|
||
|
||
|
||
int8_t Gimbal_Control_mode(Gimbal_t *g,Gimbal_CMD_t *g_cmd){
|
||
if (g == NULL || g_cmd == NULL) {
|
||
return -1;
|
||
}
|
||
switch(g_cmd->ctrl_mode){
|
||
case GIMBAL_MODE_REMOTE:
|
||
g->setpoint.eulr.yaw+=(g_cmd->delta_yaw*g->dt);
|
||
g->setpoint.eulr.pit+=(g_cmd->delta_pit*g->dt);
|
||
break;
|
||
case GIMBAL_MODE_AI:
|
||
g->setpoint.eulr.yaw=g_cmd->set_yaw;
|
||
g->setpoint.eulr.pit=g_cmd->set_pit;
|
||
break;
|
||
case GIMBAL_MODE_NUC:
|
||
/* 可能用不上，还在想 */
|
||
break;
|
||
}
|
||
return GIMBAL_OK;
|
||
};
|
||
|
||
|
||
/**
|
||
* \brief 运行云台控制逻辑
|
||
*
|
||
* \param g 包含云台数据的结构体
|
||
* \param g_cmd 云台控制指令
|
||
*
|
||
* \return 函数运行结果
|
||
*/
|
||
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
||
if (g == NULL || g_cmd == NULL) {
|
||
return -1;
|
||
}
|
||
|
||
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
|
||
g->lask_wakeup = BSP_TIME_Get_us();
|
||
Gimbal_SetMode(g, g_cmd->mode);
|
||
/* 双yaw的大yaw,setpoint从这设立 */
|
||
major_yaw_Control(g);
|
||
/* 欧拉角控制相关逻辑 */
|
||
float yaw_omega_set_point, pit_omega_set_point,major_yaw_omega_set_point;
|
||
|
||
float delta_yaw = g_cmd->delta_yaw*g->dt;
|
||
float delta_pit = g_cmd->delta_pit*g->dt;
|
||
|
||
float yaw_motor_imu_offset=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,
|
||
&g->feedback.imu.eulr.yaw);
|
||
|
||
float delta_max = CircleError(g->limit.yaw.max,
|
||
(g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);
|
||
float delta_min = CircleError(g->limit.yaw.min,
|
||
(g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);
|
||
|
||
switch (g->mode) {
|
||
case GIMBAL_MODE_RELAX:/*放松模式*/
|
||
|
||
g->out.major_yaw = 0.0f;
|
||
g->out.yaw = 0.0f;
|
||
g->out.pit = 0.0f;
|
||
break;
|
||
case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/
|
||
/* 手动和自瞄 */
|
||
Gimbal_Control_mode(g, g_cmd);
|
||
if (g->param->motor.limit_yaw==true)
|
||
Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);
|
||
else{
|
||
/*限制在-3.14~3.14*/
|
||
if (g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
|
||
if (g->setpoint.eulr.yaw < -M_PI) g->setpoint.eulr.yaw += M_2PI;
|
||
}
|
||
|
||
|
||
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
|
||
|
||
/* 限制pit控制命令 */
|
||
if (g->param->motor.limit_pit == true)
|
||
Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);
|
||
/*4310大YAW控制
|
||
这里是单环控制的，有需要加双环
|
||
*/
|
||
|
||
|
||
major_yaw_omega_set_point =PID_Calc(&g->pid.major_yaw_angle,
|
||
g->setpoint.major_yaw,
|
||
g->feedback.motor.yaw.rotor_abs_angle,
|
||
g->feedback.motor.yaw.rotor_speed,g->dt);
|
||
// g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
||
// g->feedback.imu.gyro.z,0.0f,g->dt);
|
||
g->out.major_yaw = major_yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
||
|
||
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
|
||
g->direction.Eulr.yaw, 0.0f, g->dt);
|
||
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
|
||
g->direction.Gyro.yaw, 0.f, g->dt);
|
||
|
||
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
|
||
g->direction.Eulr.pit, 0.0f, g->dt);
|
||
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
||
g->direction.Gyro.pit, 0.f, g->dt);
|
||
|
||
/*前馈添加*/
|
||
if(g->param->feedforward.imu.yaw==true)
|
||
g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
|
||
if(g->param->feedforward.imu.pit==true)
|
||
g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
|
||
|
||
break;
|
||
case GIMBAL_MODE_RELATIVE:
|
||
/*计算零点*/
|
||
CircleAdd(&(g->relative_angle.yaw), delta_yaw, M_2PI);
|
||
|
||
/* -PI~PI 归一化 */
|
||
if(g->relative_angle.yaw > M_PI) g->relative_angle.yaw -= M_2PI;
|
||
if(g->relative_angle.yaw < -M_PI) g->relative_angle.yaw += M_2PI;
|
||
|
||
/* 计算底盘当前在世界坐标系的方向 (坐标系转换) */
|
||
/* 底盘Yaw(世界) = 云台Yaw(世界) - 云台电机Yaw(机械) */
|
||
|
||
float current_chassis_yaw_world=-motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw);
|
||
/* 归一化底盘角度 */
|
||
if(current_chassis_yaw_world > M_PI) current_chassis_yaw_world -= M_2PI;
|
||
if(current_chassis_yaw_world < -M_PI) current_chassis_yaw_world += M_2PI;
|
||
|
||
|
||
/* 最终的欧拉角控制目标 (Setpoint) */
|
||
/* 目标(世界) = 底盘(世界) + 期望相对角(相对) */
|
||
g->setpoint.eulr.yaw = current_chassis_yaw_world + g->relative_angle.yaw;
|
||
|
||
/* 再次归一化 Setpoint，防止数值溢出 */
|
||
if(g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
|
||
if(g->setpoint.eulr.yaw < -M_PI) g->setpoint.eulr.yaw += M_2PI;
|
||
|
||
/* Pitch 轴处理 ( Pitch 保持绝对水平，不随底盘颠簸) */
|
||
g->setpoint.eulr.pit += delta_pit;
|
||
/* Pitch 限位 */
|
||
if (g->param->motor.limit_pit == true)
|
||
Clip(&(g->setpoint.eulr.pit), g->param->Limit_t.pit_min, g->param->Limit_t.pit_max);
|
||
|
||
/*4310大YAW控制
|
||
这里是单环控制的，有需要加双环
|
||
*/
|
||
major_yaw_omega_set_point =PID_Calc(&g->pid.major_yaw_angle,
|
||
g->setpoint.major_yaw,
|
||
g->feedback.motor.major_yaw.rotor_abs_angle,
|
||
g->feedback.motor.major_yaw.rotor_speed,g->dt);
|
||
// g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
|
||
// g->feedback.imu.gyro.z,0.0f,g->dt);
|
||
g->out.major_yaw = major_yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
|
||
|
||
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
|
||
g->direction.Eulr.yaw, 0.0f, g->dt);
|
||
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
|
||
g->direction.Gyro.yaw, 0.f, g->dt);
|
||
|
||
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
|
||
g->direction.Eulr.pit, 0.0f, g->dt);
|
||
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
||
g->direction.Gyro.pit, 0.f, g->dt);
|
||
/*前馈添加*/
|
||
if(g->param->feedforward.imu.yaw==true)
|
||
g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
|
||
if(g->param->feedforward.imu.pit==true)
|
||
g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
|
||
break;
|
||
}
|
||
/* 输出滤波 */
|
||
g->out.major_yaw = LowPassFilter2p_Apply(&g->filter_out.major_yaw, g->out.major_yaw);
|
||
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
|
||
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
|
||
return 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/**
|
||
* \brief 云台输出
|
||
*
|
||
* \param s 包含云台数据的结构体
|
||
* \param out CAN设备云台输出结构体
|
||
*/
|
||
void Gimbal_Output(Gimbal_t *g){
|
||
|
||
/*大yaw电机输出*/
|
||
if(g->param->motor.major_yaw==RM){
|
||
MOTOR_RM_Ctrl(&g->param->motor.major_yaw_rm_motor);
|
||
MOTOR_RM_SetOutput(&g->param->motor.major_yaw_rm_motor, g->out.major_yaw);
|
||
}
|
||
|
||
if(g->param->motor.major_yaw==DM){
|
||
|
||
g->out.major_yaw_dm.angle=g->param->dm_Params_t.major_yaw_dm.angle;
|
||
g->out.major_yaw_dm.kd=g->param->dm_Params_t.major_yaw_dm.kd;
|
||
g->out.major_yaw_dm.kp=g->param->dm_Params_t.major_yaw_dm.kp;
|
||
g->out.major_yaw_dm.velocity=g->param->dm_Params_t.major_yaw_dm.velocity;
|
||
|
||
g->out.major_yaw_dm.torque= g->out.major_yaw * g->param->dm_Params_t.major_yaw_dm_Reduction_ratio; // 乘以减速比
|
||
|
||
MOTOR_DM_MITCtrl(&g->param->motor.major_yaw_dm_motor,&(g->out.major_yaw_dm));
|
||
}
|
||
|
||
/*大疆电机输出*/
|
||
if(g->param->motor.yaw==RM){
|
||
MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor);
|
||
MOTOR_RM_SetOutput(&g->param->motor.yaw_rm_motor, g->out.yaw);
|
||
}
|
||
if(g->param->motor.pit==RM){
|
||
MOTOR_RM_Ctrl(&g->param->motor.pit_rm_motor);
|
||
MOTOR_RM_SetOutput(&g->param->motor.pit_rm_motor, g->out.pit);//
|
||
}
|
||
|
||
/*达妙电机输出*/
|
||
if(g->param->motor.pit==DM){
|
||
|
||
g->out.pit_dm.angle=g->param->dm_Params_t.pit_dm.angle;
|
||
g->out.pit_dm.kd=g->param->dm_Params_t.pit_dm.kd;
|
||
g->out.pit_dm.kp=g->param->dm_Params_t.pit_dm.kp;
|
||
g->out.pit_dm.velocity=g->param->dm_Params_t.pit_dm.velocity;
|
||
|
||
g->out.pit_dm.torque= g->out.pit * g->param->dm_Params_t.pit_dm_Reduction_ratio; // 乘以减速比
|
||
|
||
MOTOR_DM_MITCtrl(&g->param->motor.pit_dm_motor,&(g->out.pit_dm));
|
||
}
|
||
if(g->param->motor.yaw==DM){
|
||
g->out.yaw_dm.angle=g->param->dm_Params_t.yaw_dm.angle;
|
||
g->out.yaw_dm.kd=g->param->dm_Params_t.yaw_dm.kd;
|
||
g->out.yaw_dm.kp=g->param->dm_Params_t.yaw_dm.kp;
|
||
g->out.yaw_dm.velocity=g->param->dm_Params_t.yaw_dm.velocity;
|
||
|
||
g->out.yaw_dm.torque= g->out.yaw * g->param->dm_Params_t.yaw_dm_Reduction_ratio; // 乘以减速比
|
||
|
||
MOTOR_DM_MITCtrl(&g->param->motor.yaw_dm_motor,&(g->out.yaw_dm));
|
||
}
|
||
|
||
}
|
||
|