From 9f090022359654d38e0182ae22e6bcdc1e189959 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 8 Dec 2022 23:08:28 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E5=A4=8DINS=5FTask=E5=8F=8C=E9=87=8D?= =?UTF-8?q?=E5=BE=AA=E7=8E=AFNAN=E7=9A=84=E9=97=AE=E9=A2=98=EF=BC=8C?= =?UTF-8?q?=E6=9A=82=E6=97=B6=E5=85=B3=E9=97=AD=E4=BA=86=E6=80=A5=E5=81=9C?= =?UTF-8?q?=E6=A8=A1=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .gitignore | 2 +- HAL_N_Middlewares/Src/freertos.c | 2 +- application/chassis/chassis.c | 8 ++- application/gimbal/gimbal.c | 18 +++-- application/shoot/shoot.c | 38 ++++++----- bsp/bsp_can.c | 2 +- modules/imu/ins_task.c | 111 +++++++++++++++---------------- modules/motor/dji_motor.c | 14 ++-- 8 files changed, 101 insertions(+), 94 deletions(-) diff --git a/.gitignore b/.gitignore index 8c744c2..6f37ac8 100644 --- a/.gitignore +++ b/.gitignore @@ -51,4 +51,4 @@ build ./idea .vscode/.cortex-debug.peripherals.state.json .vscode/.cortex-debug.registers.state.json -.jdebug +*.jdebug diff --git a/HAL_N_Middlewares/Src/freertos.c b/HAL_N_Middlewares/Src/freertos.c index 30d57c7..04d0536 100644 --- a/HAL_N_Middlewares/Src/freertos.c +++ b/HAL_N_Middlewares/Src/freertos.c @@ -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 */ diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index ba2e8f8..311c62d 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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 = { diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index ab20752..d5e4411 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -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); diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index dd08c44..1da35f6 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -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 = { diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index dea4a56..7c0785e 100644 --- a/bsp/bsp_can.c +++ b/bsp/bsp_can.c @@ -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说明已经遍历完所有实例 { // 两者相等说明这是要找的实例 diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 6090acb..a75c6ff 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -75,69 +75,66 @@ 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; + + // ins update + if ((count % 1) == 0) { - dt = DWT_GetDeltaT(&INS_DWT_Count); - t += dt; + BMI088_Read(&BMI088); - // ins update - if ((count % 1) == 0) + INS.Accel[X] = BMI088.Accel[X]; + INS.Accel[Y] = BMI088.Accel[Y]; + INS.Accel[Z] = BMI088.Accel[Z]; + INS.Gyro[X] = BMI088.Gyro[X]; + INS.Gyro[Y] = BMI088.Gyro[Y]; + INS.Gyro[Z] = BMI088.Gyro[Z]; + + // demo function,用于修正安装误差,可以不管,本demo暂时没用 + IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel); + + // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用 + INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI; + INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI; + + // 核心函数,EKF更新四元数 + IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt); + + memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q)); + + // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系 + BodyFrameToEarthFrame(xb, INS.xn, INS.q); + BodyFrameToEarthFrame(yb, INS.yn, INS.q); + BodyFrameToEarthFrame(zb, INS.zn, INS.q); + + // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度 + float gravity_b[3]; + EarthFrameToBodyFrame(gravity, gravity_b, INS.q); + for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波 { - BMI088_Read(&BMI088); - - INS.Accel[X] = BMI088.Accel[X]; - INS.Accel[Y] = BMI088.Accel[Y]; - INS.Accel[Z] = BMI088.Accel[Z]; - INS.Gyro[X] = BMI088.Gyro[X]; - INS.Gyro[Y] = BMI088.Gyro[Y]; - INS.Gyro[Z] = BMI088.Gyro[Z]; - - // demo function,用于修正安装误差,可以不管,本demo暂时没用 - IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel); - - // 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用 - INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI; - INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI; - - // 核心函数,EKF更新四元数 - IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt); - - memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q)); - - // 机体系基向量转换到导航坐标系，本例选取惯性系为导航系 - BodyFrameToEarthFrame(xb, INS.xn, INS.q); - BodyFrameToEarthFrame(yb, INS.yn, INS.q); - BodyFrameToEarthFrame(zb, INS.zn, INS.q); - - // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度 - float gravity_b[3]; - EarthFrameToBodyFrame(gravity, gravity_b, INS.q); - for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波 - { - INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); - } - BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n - - // 获取最终数据 - INS.Yaw = QEKF_INS.Yaw; - INS.Pitch = QEKF_INS.Pitch; - INS.Roll = QEKF_INS.Roll; - INS.YawTotalAngle = QEKF_INS.YawTotalAngle; + INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt); } + BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n - // temperature control - if ((count % 2) == 0) - { - // 500hz - IMU_Temperature_Ctrl(); - } - - if ((count++ % 1000) == 0) - { - // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线 - } + // 获取最终数据 + INS.Yaw = QEKF_INS.Yaw; + INS.Pitch = QEKF_INS.Pitch; + INS.Roll = QEKF_INS.Roll; + INS.YawTotalAngle = QEKF_INS.YawTotalAngle; + } + + // temperature control + if ((count % 2) == 0) + { + // 500hz + IMU_Temperature_Ctrl(); + } + + if ((count++ % 1000) == 0) + { + // 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线 } - osDelay(1); } /** diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index a9415d3..f0d96c3 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -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,13 +275,11 @@ 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,检查是否要发送这一帧报文