SkyLiCoder
d56254d190
- Remove RC_ctrl_t and RemoteControlInit from robot_cmd.c - Delete GIMBAL_ZERO_FORCE, gimbal_mode_e, Robot_Status_e from robot_def.h - Remove EmergencyHandler and robot_state from robot_cmd.c - Remove emergency commands from master_machine.h - Remove remote_control and can_comm from CMakeLists.txt
261 lines
8.1 KiB
C
261 lines
8.1 KiB
C
/* app */
|
|
#include "robot_def.h"
|
|
#include "robot_cmd.h"
|
|
/* module */
|
|
#include "controller.h"
|
|
#include "ins_task.h"
|
|
#include "message_center.h"
|
|
#include "general_def.h"
|
|
#include "kalman_filter.h"
|
|
#include "master_machine.h"
|
|
/* bsp */
|
|
#include "bsp_dwt.h"
|
|
#include "bsp_log.h"
|
|
|
|
/* ---- 模式 ---- */
|
|
static Vision_Mode_e current_mode = VISION_MODE_MANUAL;
|
|
|
|
/* ---- 视觉追踪 PID (外环) ---- */
|
|
static PIDInstance vision_yaw_pid;
|
|
static PIDInstance vision_pitch_pid;
|
|
|
|
/* 视觉误差卡尔曼滤波器 */
|
|
static KalmanFilter_t yaw_error_kf;
|
|
static KalmanFilter_t pitch_error_kf;
|
|
static float filtered_yaw_error;
|
|
static float filtered_pitch_error;
|
|
|
|
/* 内环角度参考累计值 */
|
|
static float gimbal_yaw_ref;
|
|
static float gimbal_pitch_ref;
|
|
|
|
/* ---- pub-sub ---- */
|
|
static Publisher_t *gimbal_cmd_pub;
|
|
static Subscriber_t *gimbal_feed_sub;
|
|
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;
|
|
static Gimbal_Upload_Data_s gimbal_fetch_data;
|
|
|
|
/* ---- 激光 ---- */
|
|
static void laser_on(void)
|
|
{
|
|
HAL_GPIO_WritePin(POWER_5V_GPIO_Port, POWER_5V_Pin, GPIO_PIN_SET);
|
|
}
|
|
|
|
static void laser_off(void)
|
|
{
|
|
HAL_GPIO_WritePin(POWER_5V_GPIO_Port, POWER_5V_Pin, GPIO_PIN_RESET);
|
|
}
|
|
|
|
/* ---- 视觉 PID 和卡尔曼滤波器重置 ---- */
|
|
static void VisionPIDReset(void)
|
|
{
|
|
vision_yaw_pid.Iout = 0;
|
|
vision_yaw_pid.ITerm = 0;
|
|
vision_yaw_pid.Last_Err = 0;
|
|
vision_yaw_pid.Err = 0;
|
|
vision_yaw_pid.Pout = 0;
|
|
vision_yaw_pid.Dout = 0;
|
|
vision_yaw_pid.Output = 0;
|
|
vision_yaw_pid.Last_Dout = 0;
|
|
|
|
vision_pitch_pid.Iout = 0;
|
|
vision_pitch_pid.ITerm = 0;
|
|
vision_pitch_pid.Last_Err = 0;
|
|
vision_pitch_pid.Err = 0;
|
|
vision_pitch_pid.Pout = 0;
|
|
vision_pitch_pid.Dout = 0;
|
|
vision_pitch_pid.Output = 0;
|
|
vision_pitch_pid.Last_Dout = 0;
|
|
|
|
memset(yaw_error_kf.xhat_data, 0, yaw_error_kf.xhatSize * sizeof(float));
|
|
memset(yaw_error_kf.P_data, 0, yaw_error_kf.xhatSize * yaw_error_kf.xhatSize * sizeof(float));
|
|
memset(pitch_error_kf.xhat_data, 0, pitch_error_kf.xhatSize * sizeof(float));
|
|
memset(pitch_error_kf.P_data, 0, pitch_error_kf.xhatSize * pitch_error_kf.xhatSize * sizeof(float));
|
|
|
|
float P_diag[4] = {5.0f, 0.0f, 0.0f, 5.0f};
|
|
memcpy(yaw_error_kf.P_data, P_diag, sizeof(P_diag));
|
|
memcpy(pitch_error_kf.P_data, P_diag, sizeof(P_diag));
|
|
|
|
filtered_yaw_error = 0;
|
|
filtered_pitch_error = 0;
|
|
}
|
|
|
|
/* ---- 模式切换 ---- */
|
|
static void SwitchToAutoMode(void)
|
|
{
|
|
if (current_mode == VISION_MODE_AUTO)
|
|
return;
|
|
|
|
gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle;
|
|
gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch;
|
|
|
|
vision_data.manual_updated = 0;
|
|
VisionPIDReset();
|
|
current_mode = VISION_MODE_AUTO;
|
|
LOGINFO("[CMD] switched to AUTO mode");
|
|
}
|
|
|
|
static void SwitchToManualMode(void)
|
|
{
|
|
if (current_mode == VISION_MODE_MANUAL)
|
|
return;
|
|
|
|
gimbal_yaw_ref = gimbal_fetch_data.gimbal_imu_data->YawTotalAngle;
|
|
gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch;
|
|
|
|
vision_data.auto_updated = 0;
|
|
current_mode = VISION_MODE_MANUAL;
|
|
LOGINFO("[CMD] switched to MANUAL mode");
|
|
}
|
|
|
|
/* ---- 命令处理 ---- */
|
|
static void ProcessCommand(void)
|
|
{
|
|
if (!vision_data.cmd_pending)
|
|
return;
|
|
|
|
vision_data.cmd_pending = 0;
|
|
|
|
switch (vision_data.cmd)
|
|
{
|
|
case VISION_CMD_SWITCH_AUTO:
|
|
SwitchToAutoMode();
|
|
break;
|
|
case VISION_CMD_SWITCH_MANUAL:
|
|
SwitchToManualMode();
|
|
break;
|
|
case VISION_CMD_LASER_ON:
|
|
laser_on();
|
|
break;
|
|
case VISION_CMD_LASER_OFF:
|
|
laser_off();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* ---- KF + PID 初始化 ---- */
|
|
static void VisionKalmanFilterInit()
|
|
{
|
|
Kalman_Filter_Init(&yaw_error_kf, 2, 0, 1);
|
|
static float P_init[4] = {5.0f, 0.0f, 0.0f, 5.0f};
|
|
memcpy(yaw_error_kf.P_data, P_init, sizeof(P_init));
|
|
static float F_init[4] = {1.0f, 0.005f, 0.0f, 1.0f};
|
|
memcpy(yaw_error_kf.F_data, F_init, sizeof(F_init));
|
|
static float Q_init[4] = {0.1f, 0.0f, 0.0f, 1.0f};
|
|
memcpy(yaw_error_kf.Q_data, Q_init, sizeof(Q_init));
|
|
static uint8_t measurement_map[1] = {1};
|
|
static float measurement_degree[1] = {1.0f};
|
|
static float mat_R_diagonal[1] = {1.0f};
|
|
static float state_min_variance[2] = {0.001f, 0.001f};
|
|
yaw_error_kf.UseAutoAdjustment = 1;
|
|
memcpy(yaw_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map));
|
|
memcpy(yaw_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
|
|
memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal));
|
|
memcpy(yaw_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance));
|
|
|
|
Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1);
|
|
memcpy(pitch_error_kf.P_data, P_init, sizeof(P_init));
|
|
memcpy(pitch_error_kf.F_data, F_init, sizeof(F_init));
|
|
memcpy(pitch_error_kf.Q_data, Q_init, sizeof(Q_init));
|
|
pitch_error_kf.UseAutoAdjustment = 1;
|
|
memcpy(pitch_error_kf.MeasurementMap, measurement_map, sizeof(measurement_map));
|
|
memcpy(pitch_error_kf.MeasurementDegree, measurement_degree, sizeof(measurement_degree));
|
|
memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal, sizeof(mat_R_diagonal));
|
|
memcpy(pitch_error_kf.StateMinVariance, state_min_variance, sizeof(state_min_variance));
|
|
}
|
|
|
|
void RobotCMDInit()
|
|
{
|
|
gimbal_cmd_pub = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
|
|
gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
|
|
|
static PID_Init_Config_s yaw_vpid_config = {
|
|
.Kp = 1.5f, .Ki = 3.0f, .Kd = 0.07f,
|
|
.IntegralLimit = 100.0f,
|
|
.CoefA = 20.0f, .CoefB = 20.0f,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
|
|
PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
|
|
.MaxOut = 200.0f,
|
|
};
|
|
static PID_Init_Config_s pitch_vpid_config = {
|
|
.Kp = 1.0f, .Ki = 2.0f, .Kd = 0.05f,
|
|
.IntegralLimit = 100.0f,
|
|
.CoefA = 10.0f, .CoefB = 30.0f,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
|
|
PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
|
|
.MaxOut = 200.0f,
|
|
};
|
|
PIDInit(&vision_yaw_pid, &yaw_vpid_config);
|
|
PIDInit(&vision_pitch_pid, &pitch_vpid_config);
|
|
VisionKalmanFilterInit();
|
|
|
|
Vision_Uart_Init(&huart10);
|
|
laser_off();
|
|
}
|
|
|
|
/* ---- 自动 / 视觉追踪模式 ---- */
|
|
static void VisionTrackingSet()
|
|
{
|
|
if (vision_data.auto_updated)
|
|
{
|
|
vision_data.auto_updated = 0;
|
|
|
|
yaw_error_kf.MeasuredVector[0] = vision_data.error_x;
|
|
Kalman_Filter_Update(&yaw_error_kf);
|
|
filtered_yaw_error = yaw_error_kf.FilteredValue[0];
|
|
|
|
pitch_error_kf.MeasuredVector[0] = vision_data.error_y;
|
|
Kalman_Filter_Update(&pitch_error_kf);
|
|
filtered_pitch_error = pitch_error_kf.FilteredValue[0];
|
|
|
|
float yaw_increment = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0);
|
|
float pitch_increment = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0);
|
|
|
|
gimbal_yaw_ref += 0.001f * yaw_increment;
|
|
gimbal_pitch_ref += 0.001f * pitch_increment;
|
|
}
|
|
|
|
if (gimbal_pitch_ref > 30.0f)
|
|
gimbal_pitch_ref = 30.0f;
|
|
else if (gimbal_pitch_ref < -30.0f)
|
|
gimbal_pitch_ref = -30.0f;
|
|
|
|
gimbal_cmd_send.yaw = gimbal_yaw_ref;
|
|
gimbal_cmd_send.pitch = gimbal_pitch_ref;
|
|
}
|
|
|
|
/* ---- 手动模式: 上位机角度参考 ---- */
|
|
static void ManualModeSet()
|
|
{
|
|
if (vision_data.manual_updated)
|
|
{
|
|
vision_data.manual_updated = 0;
|
|
gimbal_yaw_ref = vision_data.yaw_ref;
|
|
gimbal_pitch_ref = vision_data.pitch_ref;
|
|
}
|
|
|
|
if (gimbal_pitch_ref > 30.0f)
|
|
gimbal_pitch_ref = 30.0f;
|
|
else if (gimbal_pitch_ref < -30.0f)
|
|
gimbal_pitch_ref = -30.0f;
|
|
|
|
gimbal_cmd_send.yaw = gimbal_yaw_ref;
|
|
gimbal_cmd_send.pitch = gimbal_pitch_ref;
|
|
}
|
|
|
|
void RobotCMDTask()
|
|
{
|
|
SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);
|
|
|
|
ProcessCommand();
|
|
|
|
if (current_mode == VISION_MODE_AUTO)
|
|
VisionTrackingSet();
|
|
else
|
|
ManualModeSet();
|
|
|
|
PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
|
|
}
|