HFUT_Firmament_EC
/
sentry_chassis_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

修复INS_Task双重循环NAN的问题，暂时关闭了急停模式

This commit is contained in:
NeoZng 2022-12-08 23:08:28 +08:00
parent fe85ae5a6e
commit 9f09002235
8 changed files with 101 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -51,4 +51,4 @@ build
./idea
.vscode/.cortex-debug.peripherals.state.json
.vscode/.cortex-debug.registers.state.json
.jdebug
*.jdebug

2
HAL_N_Middlewares/Src/freertos.c
View File

@ -128,7 +128,7 @@ void MX_FREERTOS_Init(void) {
osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 512);
defaultTaskHandle = osThreadCreate(osThread(daemontask), NULL);
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 2048);
osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024);
defaultTaskHandle = osThreadCreate(osThread(robottask), NULL);
/* USER CODE BEGIN RTOS_THREADS */

8
application/chassis/chassis.c
View File

@ -62,10 +62,14 @@ void ChassisInit()
.can_init_config.can_handle=&hcan2,
.controller_param_init_config = {
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {

18
application/gimbal/gimbal.c
View File

@ -26,12 +26,14 @@ void GimbalInit()
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
.Kd = 1,
.Ki = 0,
.Kd = 0,
},
.speed_PID = {
.Kd = 1,
.Ki = 0,
.Kd = 0,
},
.other_angle_feedback_ptr = &Gimbal_IMU_data->YawTotalAngle,
// 还需要增加角速度额外反馈指针
@ -58,7 +60,9 @@ void GimbalInit()
.Kd = 2,
},
.speed_PID = {
.Kd = 1,
.Ki = 0,
.Kd = 0,
},
.other_angle_feedback_ptr = &Gimbal_IMU_data->Pitch,
// 还需要增加角速度额外反馈指针
@ -71,8 +75,8 @@ void GimbalInit()
.close_loop_type = ANGLE_LOOP | SPEED_LOOP,
.reverse_flag = MOTOR_DIRECTION_REVERSE,
},
.motor_type = GM6020};
.motor_type = GM6020,
};
// 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动
yaw_motor = DJIMotorInit(&yaw_config);
pitch_motor = DJIMotorInit(&pitch_config);

38
application/shoot/shoot.c
View File

@ -28,16 +28,15 @@ void ShootInit()
.tx_id = 1,
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {
@ -56,16 +55,15 @@ void ShootInit()
.tx_id = 2,
},
.controller_param_init_config = {
.angle_PID = {
.Kd = 10,
.Ki = 1,
.Kd = 2,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {
@ -90,13 +88,19 @@ void ShootInit()
.Kd = 2,
},
.angle_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.speed_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
.current_PID = {
.Kp=1,
.Ki=0,
.Kd=0,
},
},
.controller_setting_init_config = {

2
bsp/bsp_can.c
View File

@ -115,7 +115,7 @@ static void CANFIFOxCallback(CAN_HandleTypeDef *_hcan, uint32_t fifox)
uint8_t can_rx_buff[8];
CAN_RxHeaderTypeDef rxconf;
HAL_CAN_GetRxMessage(_hcan, fifox, &rxconf, can_rx_buff);
for (size_t i = 0; i < DEVICE_CAN_CNT; i++)
for (size_t i = 0; i < MX_REGISTER_DEVICE_CNT; i++)
{
if (instance[i] != NULL) // 碰到NULL说明已经遍历完所有实例
{ // 两者相等说明这是要找的实例

5
modules/imu/ins_task.c
View File

@ -75,8 +75,7 @@ void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
while (1)
{
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
@ -137,8 +136,6 @@ void INS_Task(void)
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
}
osDelay(1);
}
/**
* @brief Transform 3dvector from BodyFrame to EarthFrame

14
modules/motor/dji_motor.c
View File

@ -124,7 +124,7 @@ static void DecodeDJIMotor(CANInstance *_instance)
static uint8_t *rxbuff;
static DJI_Motor_Measure_s *measure;
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i]->motor_can_instance == _instance)
{
@ -225,7 +225,7 @@ void DJIMotorControl()
static DJI_Motor_Measure_s *motor_measure;
static float pid_measure;
// 遍历所有电机实例,进行串级PID的计算并设置发送报文的值
for (size_t i = 0; i < DJI_MOTOR_CNT; i++)
for (size_t i = 0; i < idx; i++)
{
if (dji_motor_info[i])
{
@ -275,14 +275,12 @@ void DJIMotorControl()
sender_assignment[group].tx_buff[2 * num + 1] = 0xff & set;
// 电机是否停止运行
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将buff置零
memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
// if (motor->stop_flag == MOTOR_STOP)
// { // 若该电机处于停止状态,直接将buff置零
// memset(sender_assignment[group].tx_buff + 2 * num, 0, 16u);
// }
}
}
else // 遇到空指针说明所有遍历结束,退出循环
break;
}
// 遍历flag,检查是否要发送这一帧报文
for (size_t i = 0; i < 6; i++)