From 11329aaddb62aa9160987db6c6e70ec1de3b8faf Mon Sep 17 00:00:00 2001 From: NeoZng Date: Tue, 13 Dec 2022 19:40:03 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BA=86LK=E7=94=B5=E6=9C=BA?= =?UTF-8?q?=E5=92=8CHT=E7=94=B5=E6=9C=BA=E7=9A=84=E5=9F=BA=E6=9C=AC?= =?UTF-8?q?=E6=94=AF=E6=8C=81,=E5=BE=85=E7=BC=96=E5=86=99=E6=8E=A7?= =?UTF-8?q?=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 3 +- application/cmd/robot_cmd.c | 4 +- bsp/bsp_can.c | 2 +- bsp/bsp_usart.c | 4 +- modules/algorithm/QuaternionEKF.c | 14 +-- modules/algorithm/kalman_filter.c | 6 +- modules/can_comm/can_comm.c | 80 ++++++++-------- modules/daemon/daemon.c | 2 +- modules/imu/BMI088driver.c | 10 +- modules/imu/ins_task.c | 6 +- modules/led_light/led_task.c | 4 +- modules/message_center/message_center.c | 10 +- modules/motor/HT04.c | 119 +++++++++++++++++------- modules/motor/HT04.h | 86 ++++++++++++++--- modules/motor/LK9025.c | 66 +++++++------ modules/motor/LK9025.h | 2 +- modules/motor/dji_motor.c | 19 ++-- modules/motor/dji_motor.h | 2 +- modules/motor/dji_motor.md | 2 +- modules/motor/motor_def.h | 8 +- modules/remote/remote_control.c | 2 +- 21 files changed, 280 insertions(+), 171 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index f8f50e8..cf7cc80 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -41,7 +41,8 @@ "motor_def.h": "c", "quaternionekf.h": "c", "shoot.h": "c", - "lk9025.h": "c" + "lk9025.h": "c", + "ht04.h": "c" }, "C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools", } \ No newline at end of file diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 35b2a89..a50da6a 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -8,8 +8,8 @@ #include "dji_motor.h" // 私有宏,自动将编码器转换成角度值 -#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF) -#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF) +#define YAW_ALIGN_ANGLE (YAW_CHASSIS_ALIGN_ECD * ECD_ANGLE_COEF_DJI) +#define PTICH_HORIZON_ANGLE (PITCH_HORIZON_ECD * ECD_ANGLE_COEF_DJI) /* gimbal_cmd应用包含的模块实例指针和交互信息存储*/ #ifdef GIMBAL_BOARD // 对双板的兼容,条件编译 diff --git a/bsp/bsp_can.c b/bsp/bsp_can.c index 7c0785e..f018026 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 < MX_REGISTER_DEVICE_CNT; i++) + for (size_t i = 0; i < MX_REGISTER_DEVICE_CNT; ++i) { if (instance[i] != NULL) // 碰到NULL说明已经遍历完所有实例 { // 两者相等说明这是要找的实例 diff --git a/bsp/bsp_usart.c b/bsp/bsp_usart.c index 98b7a44..c77e97a 100644 --- a/bsp/bsp_usart.c +++ b/bsp/bsp_usart.c @@ -70,7 +70,7 @@ void USARTSend(USARTInstance *_instance, uint8_t *send_buf, uint16_t send_size) */ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) { - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) { if (huart == instance[i]->usart_handle) { @@ -93,7 +93,7 @@ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) */ void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) { if (huart == instance[i]->usart_handle) { diff --git a/modules/algorithm/QuaternionEKF.c b/modules/algorithm/QuaternionEKF.c index 08a33df..5513486 100644 --- a/modules/algorithm/QuaternionEKF.c +++ b/modules/algorithm/QuaternionEKF.c @@ -156,7 +156,7 @@ void IMU_QuaternionEKF_Update(float gx, float gy, float gz, float ax, float ay, // set z,单位化重力加速度向量 accelInvNorm = invSqrt(QEKF_INS.Accel[0] * QEKF_INS.Accel[0] + QEKF_INS.Accel[1] * QEKF_INS.Accel[1] + QEKF_INS.Accel[2] * QEKF_INS.Accel[2]); - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) { QEKF_INS.IMU_QuaternionEKF.MeasuredVector[i] = QEKF_INS.Accel[i] * accelInvNorm; // 用加速度向量更新量测值 } @@ -238,7 +238,7 @@ static void IMU_QuaternionEKF_F_Linearization_P_Fading(KalmanFilter_t *kf) // quaternion normalize qInvNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); - for (uint8_t i = 0; i < 4; i++) + for (uint8_t i = 0; i < 4; ++i) { kf->xhatminus_data[i] *= qInvNorm; } @@ -352,7 +352,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf) kf->temp_vector_data[2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3; // temp_vector = h(xhat'(k)) // 计算预测值和各个轴的方向余弦 - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) { QEKF_INS.OrientationCosine[i] = acosf(fabsf(kf->temp_vector_data[i])); } @@ -426,13 +426,13 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf) kf->MatStatus = Matrix_Multiply(&kf->temp_matrix, &kf->temp_matrix1, &kf->K); // implement adaptive - for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; i++) + for (uint8_t i = 0; i < kf->K.numRows * kf->K.numCols; ++i) { kf->K_data[i] *= QEKF_INS.AdaptiveGainScale; } - for (uint8_t i = 4; i < 6; i++) + for (uint8_t i = 4; i < 6; ++i) { - for (uint8_t j = 0; j < 3; j++) + for (uint8_t j = 0; j < 3; ++j) { kf->K_data[i * 3 + j] *= QEKF_INS.OrientationCosine[i - 4] / 1.5707963f; // 1 rad } @@ -445,7 +445,7 @@ static void IMU_QuaternionEKF_xhatUpdate(KalmanFilter_t *kf) // 零漂修正限幅,一般不会有过大的漂移 if (QEKF_INS.ConvergeFlag) { - for (uint8_t i = 4; i < 6; i++) + for (uint8_t i = 4; i < 6; ++i) { if (kf->temp_vector.pData[i] > 1e-2f * QEKF_INS.dt) { diff --git a/modules/algorithm/kalman_filter.c b/modules/algorithm/kalman_filter.c index adb7ddb..4f5460f 100644 --- a/modules/algorithm/kalman_filter.c +++ b/modules/algorithm/kalman_filter.c @@ -420,7 +420,7 @@ float *Kalman_Filter_Update(KalmanFilter_t *kf) // 避免滤波器过度收敛 // suppress filter excessive convergence - for (uint8_t i = 0; i < kf->xhatSize; i++) + for (uint8_t i = 0; i < kf->xhatSize; ++i) { if (kf->P_data[i * kf->xhatSize + i] < kf->StateMinVariance[i]) kf->P_data[i * kf->xhatSize + i] = kf->StateMinVariance[i]; @@ -445,7 +445,7 @@ static void H_K_R_Adjustment(KalmanFilter_t *kf) // recognize measurement validity and adjust matrices H R K memset(kf->R_data, 0, sizeof_float * kf->zSize * kf->zSize); memset(kf->H_data, 0, sizeof_float * kf->xhatSize * kf->zSize); - for (uint8_t i = 0; i < kf->zSize; i++) + for (uint8_t i = 0; i < kf->zSize; ++i) { if (kf->z_data[i] != 0) { @@ -459,7 +459,7 @@ static void H_K_R_Adjustment(KalmanFilter_t *kf) kf->MeasurementValidNum++; } } - for (uint8_t i = 0; i < kf->MeasurementValidNum; i++) + for (uint8_t i = 0; i < kf->MeasurementValidNum; ++i) { // 重构矩阵R // rebuild matrix R diff --git a/modules/can_comm/can_comm.c b/modules/can_comm/can_comm.c index 946cc1a..efe2eec 100644 --- a/modules/can_comm/can_comm.c +++ b/modules/can_comm/can_comm.c @@ -26,52 +26,49 @@ static void CANCommResetRx(CANCommInstance *ins) */ static void CANCommRxCallback(CANInstance *_instance) { - for (size_t i = 0; i < idx; i++) + static CANCommInstance *comm; + comm = (CANCommInstance *)_instance->id; + + /* 接收状态判断 */ + if (_instance->rx_buff[0] == CAN_COMM_HEADER && comm->recv_state == 0) // 尚未开始接收且新的一包里有帧头 { - if (can_comm_instance[i]->can_ins == _instance) // 遍历,找到对应的接收CAN COMM实例 + if (_instance->rx_buff[1] == comm->recv_data_len) // 接收长度等于设定接收长度 { - /* 接收状态判断 */ - if (_instance->rx_buff[0] == CAN_COMM_HEADER && can_comm_instance[i]->recv_state == 0) // 尚未开始接收且新的一包里有帧头 + comm->recv_state = 1; + } + else + return; // 直接跳过即可 + } + if (comm->recv_state) // 已经收到过帧头 + { // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误 + if (comm->cur_recv_len + _instance->rx_len > comm->recv_buf_len) + { + CANCommResetRx(comm); + return; // 重置状态然后返回 + } + + // 直接拷贝到当前的接收buffer后面 + memcpy(comm->raw_recvbuf + comm->cur_recv_len, _instance->rx_buff, _instance->rx_len); + comm->cur_recv_len += _instance->rx_len; + + // 当前已经收满 + if (comm->cur_recv_len == comm->recv_buf_len) + { // buff里本该是tail的位置不等于CAN_COMM_TAIL + if (comm->raw_recvbuf[comm->recv_buf_len - 1] != CAN_COMM_TAIL) { - if (_instance->rx_buff[1] == can_comm_instance[i]->recv_data_len) // 接收长度等于设定接收长度 - { - can_comm_instance[i]->recv_state = 1; - } - else - return; // 直接跳过即可 + CANCommResetRx(comm); + return; // 重置状态然后返回 } - if (can_comm_instance[i]->recv_state) // 已经收到过帧头 - { // 如果已经接收到的长度加上当前一包的长度大于总buf len,说明接收错误 - if (can_comm_instance[i]->cur_recv_len + _instance->rx_len > can_comm_instance[i]->recv_buf_len) - { - CANCommResetRx(can_comm_instance[i]); - return; // 重置状态然后返回 - } - - // 直接拷贝到当前的接收buffer后面 - memcpy(can_comm_instance[i]->raw_recvbuf + can_comm_instance[i]->cur_recv_len, _instance->rx_buff, _instance->rx_len); - can_comm_instance[i]->cur_recv_len += _instance->rx_len; - - // 当前已经收满 - if (can_comm_instance[i]->cur_recv_len == can_comm_instance[i]->recv_buf_len) - { // buff里本该是tail的位置不等于CAN_COMM_TAIL - if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 1] != CAN_COMM_TAIL) - { - CANCommResetRx(can_comm_instance[i]); - return; // 重置状态然后返回 - } - else // tail正确, 对数据进行crc8校验 - { - if (can_comm_instance[i]->raw_recvbuf[can_comm_instance[i]->recv_buf_len - 2] == - crc_8(can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len)) - { // 通过校验,复制数据到unpack_data中 - memcpy(can_comm_instance[i]->unpacked_recv_data, can_comm_instance[i]->raw_recvbuf + 2, can_comm_instance[i]->recv_data_len); - can_comm_instance[i]->update_flag = 1; // 数据更新flag置为1 - } - CANCommResetRx(can_comm_instance[i]); - return; // 重置状态然后返回 - } + else // tail正确, 对数据进行crc8校验 + { + if (comm->raw_recvbuf[comm->recv_buf_len - 2] == + crc_8(comm->raw_recvbuf + 2, comm->recv_data_len)) + { // 通过校验,复制数据到unpack_data中 + memcpy(comm->unpacked_recv_data, comm->raw_recvbuf + 2, comm->recv_data_len); + comm->update_flag = 1; // 数据更新flag置为1 } + CANCommResetRx(comm); + return; // 重置状态然后返回 } return; // 访问完一个can comm直接退出,一次中断只会也只可能会处理一个实例的回调 } @@ -90,6 +87,7 @@ CANCommInstance *CANCommInit(CANComm_Init_Config_s *comm_config) can_comm_instance[idx]->raw_sendbuf[1] = comm_config->send_data_len; can_comm_instance[idx]->raw_sendbuf[comm_config->send_data_len + CAN_COMM_OFFSET_BYTES - 1] = CAN_COMM_TAIL; + comm_config->can_config.id = can_comm_instance[idx]; comm_config->can_config.can_module_callback = CANCommRxCallback; can_comm_instance[idx]->can_ins = CANRegister(&comm_config->can_config); return can_comm_instance[idx++]; diff --git a/modules/daemon/daemon.c b/modules/daemon/daemon.c index 6520c8b..49d6677 100644 --- a/modules/daemon/daemon.c +++ b/modules/daemon/daemon.c @@ -31,7 +31,7 @@ uint8_t DaemonIsOnline(DaemonInstance *instance) void DaemonTask() { static DaemonInstance* pins; //提高可读性同时降低访存开销 - for (size_t i = 0; i < idx; i++) + for (size_t i = 0; i < idx; ++i) { pins=daemon_instances[i]; if(pins->temp_count>0) diff --git a/modules/imu/BMI088driver.c b/modules/imu/BMI088driver.c index 4a008d2..787a304 100644 --- a/modules/imu/BMI088driver.c +++ b/modules/imu/BMI088driver.c @@ -144,7 +144,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088) bmi088->GyroOffset[1] = 0; bmi088->GyroOffset[2] = 0; - for (uint16_t i = 0; i < CaliTimes; i++) + for (uint16_t i = 0; i < CaliTimes; ++i) { BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6); bmi088_raw_temp = (int16_t)((buf[1]) << 8) | buf[0]; @@ -176,7 +176,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088) { gNormMax = gNormTemp; gNormMin = gNormTemp; - for (uint8_t j = 0; j < 3; j++) + for (uint8_t j = 0; j < 3; ++j) { gyroMax[j] = bmi088->Gyro[j]; gyroMin[j] = bmi088->Gyro[j]; @@ -188,7 +188,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088) gNormMax = gNormTemp; if (gNormTemp < gNormMin) gNormMin = gNormTemp; - for (uint8_t j = 0; j < 3; j++) + for (uint8_t j = 0; j < 3; ++j) { if (bmi088->Gyro[j] > gyroMax[j]) gyroMax[j] = bmi088->Gyro[j]; @@ -198,7 +198,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088) } gNormDiff = gNormMax - gNormMin; - for (uint8_t j = 0; j < 3; j++) + for (uint8_t j = 0; j < 3; ++j) gyroDiff[j] = gyroMax[j] - gyroMin[j]; if (gNormDiff > 0.5f || gyroDiff[0] > 0.15f || @@ -209,7 +209,7 @@ void Calibrate_MPU_Offset(IMU_Data_t *bmi088) } bmi088->gNorm /= (float)CaliTimes; - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) bmi088->GyroOffset[i] /= (float)CaliTimes; BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2); diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index d70f575..87d81d1 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -112,7 +112,7 @@ void INS_Task(void) // 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度 float gravity_b[3]; EarthFrameToBodyFrame(gravity, gravity_b, INS.q); - for (uint8_t i = 0; i < 3; i++) // 同样过一个低通滤波 + 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); } @@ -218,7 +218,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[ param->flag = 0; } float gyro_temp[3]; - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) gyro_temp[i] = gyro[i] * param->scale[i]; gyro[X] = c_11 * gyro_temp[X] + @@ -232,7 +232,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[ c_33 * gyro_temp[Z]; float accel_temp[3]; - for (uint8_t i = 0; i < 3; i++) + for (uint8_t i = 0; i < 3; ++i) accel_temp[i] = accel[i]; accel[X] = c_11 * accel_temp[X] + diff --git a/modules/led_light/led_task.c b/modules/led_light/led_task.c index 76a9fdc..21037b7 100644 --- a/modules/led_light/led_task.c +++ b/modules/led_light/led_task.c @@ -29,7 +29,7 @@ void led_RGB_flow_task() static float alpha, red, green, blue; static uint32_t aRGB; - for (size_t i = 0; i < RGB_FLOW_COLOR_LENGHT; i++) + for (size_t i = 0; i < RGB_FLOW_COLOR_LENGHT; ++i) { alpha = (RGB_flow_color[i] & 0xFF000000) >> 24; red = ((RGB_flow_color[i] & 0x00FF0000) >> 16); @@ -45,7 +45,7 @@ void led_RGB_flow_task() delta_red /= RGB_FLOW_COLOR_CHANGE_TIME; delta_green /= RGB_FLOW_COLOR_CHANGE_TIME; delta_blue /= RGB_FLOW_COLOR_CHANGE_TIME; - for (size_t j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; j++) + for (size_t j = 0; j < RGB_FLOW_COLOR_CHANGE_TIME; ++j) { alpha += delta_alpha; red += delta_red; diff --git a/modules/message_center/message_center.c b/modules/message_center/message_center.c index 0beded9..443bff8 100644 --- a/modules/message_center/message_center.c +++ b/modules/message_center/message_center.c @@ -14,11 +14,11 @@ void MessageInit() { // pub必须唯一,即消息名称不能重复,不得有多个pub发布相同消息名称 // 对每一个subscriber,寻找相同消息名称的publisher,可能有多个sub从相同pub获取消息 - for (size_t i = 0; i < MAX_EVENT_COUNT; i++) + for (size_t i = 0; i < MAX_EVENT_COUNT; ++i) { if (s_pptr[i] != NULL) { - for (size_t j = 0; j < MAX_EVENT_COUNT; j++) // 遍历publisher + for (size_t j = 0; j < MAX_EVENT_COUNT; ++j) // 遍历publisher { if (p_ptr[j] != NULL) // 不为空 { @@ -46,7 +46,7 @@ void MessageInit() void PublisherRegister(char *name, void *data) { static uint8_t idx; - for (size_t i = 0; i < idx; i++) + for (size_t i = 0; i < idx; ++i) { if (strcmp(pname[i], name) == 0) while (1) @@ -105,7 +105,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) memset(ret, 0, sizeof(Subscriber_t)); // 对新建的Subscriber进行初始化 ret->data_len = data_len; // 设定数据长度 - for (size_t i = 0; i < QUEUE_SIZE; i++) + for (size_t i = 0; i < QUEUE_SIZE; ++i) { // 给消息队列的每一个元素分配空间,queue里保存的实际上是数据执指针,这样可以兼容不同的数据长度 ret->queue[i] = malloc(sizeof(data_len)); } @@ -135,7 +135,7 @@ Subscriber_t *SubRegister(char *name, uint8_t data_len) Subscriber_t *ret = (Subscriber_t *)malloc(sizeof(Subscriber_t)); memset(ret, 0, sizeof(Subscriber_t)); ret->data_len = data_len; - for (size_t i = 0; i < QUEUE_SIZE; i++) + for (size_t i = 0; i < QUEUE_SIZE; ++i) { // 给消息队列分配空间 ret->queue[i] = malloc(sizeof(data_len)); } diff --git a/modules/motor/HT04.c b/modules/motor/HT04.c index 4b04d15..f3a7e74 100644 --- a/modules/motor/HT04.c +++ b/modules/motor/HT04.c @@ -1,15 +1,31 @@ #include "HT04.h" #include "memory.h" +#include "general_def.h" -HKMotor_Measure_t *joint_motor_info[HT_MOTOR_CNT]; +static uint8_t idx; +HTMotorInstance *ht_motor_info[HT_MOTOR_CNT]; +/** + * @brief + * + * @param cmd + * @param motor + */ +static void HTMotorSetMode(HTMotor_Mode_t cmd, HTMotorInstance *motor) +{ + static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; + buf[7] = (uint8_t)cmd; + memcpy(motor->motor_can_instace->tx_buff, buf, sizeof(buf)); + CANTransmit(motor->motor_can_instace); +} + +/* 两个用于将uint值和float值进行映射的函数,在设定发送值和解析反馈值时使用 */ static uint16_t float_to_uint(float x, float x_min, float x_max, uint8_t bits) { float span = x_max - x_min; float offset = x_min; return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span); } - static float uint_to_float(int x_int, float x_min, float x_max, int bits) { float span = x_max - x_min; @@ -17,47 +33,84 @@ static float uint_to_float(int x_int, float x_min, float x_max, int bits) return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset; } -static void DecodeJoint(CANInstance *motor_instance) +/** + * @brief 解析电机反馈值 + * + * @param motor_can 收到 + */ +static void HTMotorDecode(CANInstance *motor_can) { - uint16_t tmp; - for (size_t i = 0; i < HT_MOTOR_CNT; i++) + static uint16_t tmp; // 用于暂存解析值,稍后转换成float数据,避免多次创建临时变量 + static HTMotor_Measure_t *measure; + static uint8_t *rxbuff; + + rxbuff = motor_can->rx_buff; + measure = &((HTMotorInstance *)motor_can->id)->motor_measure; + + measure->last_angle = measure->total_angle; + + tmp = (uint16_t)((rxbuff[1] << 8) | rxbuff[2]); + measure->total_angle = RAD_2_ANGLE * uint_to_float(tmp, P_MAX, P_MIN, 16); + + tmp = (uint16_t)((rxbuff[3] << 4) | (rxbuff[4] >> 4)); + measure->speed_aps = RAD_2_ANGLE * SPEED_SMOOTH_COEF * uint_to_float(tmp, V_MAX, V_MIN, 12) + + (1 - SPEED_SMOOTH_COEF) * measure->speed_aps; + + tmp = (uint16_t)(((rxbuff[4] & 0x0f) << 8) | rxbuff[5]); + measure->real_current = CURRENT_SMOOTH_COEF * uint_to_float(tmp, T_MAX, T_MIN, 12) + + (1 - CURRENT_SMOOTH_COEF) * measure->real_current; +} + +HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config) +{ + + ht_motor_info[idx] = (HTMotorInstance *)malloc(sizeof(HTMotorInstance)); + memset(ht_motor_info[idx], 0, sizeof(HTMotorInstance)); + + ht_motor_info[idx]->motor_settings = config->controller_setting_init_config; + PID_Init(&ht_motor_info[idx]->current_PID, &config->controller_param_init_config.current_PID); + PID_Init(&ht_motor_info[idx]->speed_PID, &config->controller_param_init_config.speed_PID); + PID_Init(&ht_motor_info[idx]->angle_PID, &config->controller_param_init_config.angle_PID); + ht_motor_info[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + ht_motor_info[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; + + config->can_init_config.can_module_callback = HTMotorDecode; + config->can_init_config.id = ht_motor_info[idx]; + ht_motor_info[idx]->motor_can_instace = CANRegister(&config->can_init_config); + + HTMotorEnable(ht_motor_info[idx]); + return ht_motor_info[idx++]; +} + +void HTMotorSetRef(HTMotorInstance *motor, float ref) +{ + motor->pid_ref = ref; +} + +void HTMotorControl() +{ + static uint16_t tmp; + for (size_t i = 0; i < idx; i++) { - if (joint_motor_info[i]->motor_can_instace == motor_instance) - { - tmp = (motor_instance->rx_buff[1] << 8) | motor_instance->rx_buff[2]; - joint_motor_info[i]->last_ecd = joint_motor_info[i]->ecd; - joint_motor_info[i]->ecd = uint_to_float(tmp, P_MAX, P_MIN, 16); - tmp = (motor_instance->rx_buff[3] << 4) | (motor_instance->rx_buff[4] >> 4); - joint_motor_info[i]->speed_rpm = uint_to_float(tmp, V_MAX, V_MIN, 12); - tmp = ((motor_instance->rx_buff[4] & 0xf) << 8) | motor_instance->rx_buff[5]; - joint_motor_info[i]->real_current = uint_to_float(tmp, T_MAX, T_MIN, 12); - break; - } + // tmp=float_to_uint() + // _instance->motor_can_instace->rx_buff[6] = tmp >> 8; + // _instance->motor_can_instace->rx_buff[7] = tmp & 0xff; + // CANTransmit(_instance->motor_can_instace); } + } -HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config) +void HTMotorStop(HTMotorInstance *motor) { - static uint8_t idx; - joint_motor_info[idx] = (HKMotor_Measure_t *)malloc(sizeof(HKMotor_Measure_t)); - joint_motor_info[idx]->motor_can_instace = CANRegister(&config); - return joint_motor_info[idx++]; + HTMotorSetMode(CMD_RESET_MODE, motor); } -void JointControl(HKMotor_Measure_t *_instance, float current) +void HTMotorEnable(HTMotorInstance *motor) { - uint16_t tmp; - LIMIT_MIN_MAX(current, T_MIN, T_MAX); - tmp = float_to_uint(current, T_MIN, T_MAX, 12); - _instance->motor_can_instace->rx_buff[6] = tmp >> 8; - _instance->motor_can_instace->rx_buff[7] = tmp & 0xff; - CANTransmit(_instance->motor_can_instace); + HTMotorSetMode(CMD_MOTOR_MODE, motor); } -void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance) +void HTMotorCalibEncoder(HTMotorInstance *motor) { - static uint8_t buf[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00}; - buf[7] = (uint8_t)cmd; - memcpy(_instance->motor_can_instace->rx_buff, buf, 8 * sizeof(uint8_t)); - CANTransmit(_instance->motor_can_instace); + HTMotorSetMode(CMD_ZERO_POSITION, motor); } diff --git a/modules/motor/HT04.h b/modules/motor/HT04.h index 1e8bce4..21317f0 100644 --- a/modules/motor/HT04.h +++ b/modules/motor/HT04.h @@ -7,36 +7,94 @@ #include "motor_def.h" #define HT_MOTOR_CNT 4 +#define CURRENT_SMOOTH_COEF 0.9f +#define SPEED_SMOOTH_COEF 0.85f #define P_MIN -95.5f // Radians #define P_MAX 95.5f #define V_MIN -45.0f // Rad/s #define V_MAX 45.0f -#define T_MIN -18.0f +#define T_MIN -18.0f // N·m #define T_MAX 18.0f typedef struct // HT04 -{ - float last_ecd; - float ecd; - float speed_rpm; +{ // 角度为多圈角度,范围是-95.5~95.5,单位为rad + float last_angle; + float total_angle; + float speed_aps; float real_current; +} HTMotor_Measure_t; + +/* HT电机类型定义*/ +typedef struct +{ + HTMotor_Measure_t motor_measure; + + Motor_Control_Setting_s motor_settings; + + PIDInstance current_PID; + PIDInstance speed_PID; + PIDInstance angle_PID; + float *other_angle_feedback_ptr; + float *other_speed_feedback_ptr; + float pid_ref; - PIDInstance pid; CANInstance *motor_can_instace; -} HKMotor_Measure_t; +} HTMotorInstance; +/* HT电机模式,初始化时自动进入CMD_MOTOR_MODE*/ typedef enum { - CMD_MOTOR_MODE = 0xfc, - CMD_RESET_MODE = 0xfd, - CMD_ZERO_POSITION = 0xfe -} joint_mode; + CMD_MOTOR_MODE = 0xfc, // 使能,会响应指令 + CMD_RESET_MODE = 0xfd, // 停止 + CMD_ZERO_POSITION = 0xfe // 将当前的位置设置为编码器零位 +} HTMotor_Mode_t; -HKMotor_Measure_t *HTMotorInit(CAN_Init_Config_s config); +/** + * @brief + * + * @param config + * @return HTMotorInstance* + */ +HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config); -void JointControl(HKMotor_Measure_t *_instance, float current); +/** + * @brief 设定电机的参考值 + * + * @param motor 要设定的电机 + * @param current 设定值 + */ +void HTMotorSetRef(HTMotorInstance *motor, float ref); -void SetJointMode(joint_mode cmd, HKMotor_Measure_t *_instance); +/** + * @brief 给所有的HT电机发送控制指令 + * + */ +void HTMotorControl(); + +/** + * @brief 停止电机,之后电机不会响应HTMotorSetRef设定的值 + * + * @param motor + */ +void HTMotorStop(HTMotorInstance *motor); + +/** + * @brief 启动电机 + * + * @param motor 要启动的电机 + */ +void HTMotorEnable(HTMotorInstance *motor); + +/** + * @brief 校准电机编码器 + * @attention 注意,校准时务必将电机和其他机构分离,电机会旋转360°! + * 注意,校准时务必将电机和其他机构分离,电机会旋转360°! + * 注意,校准时务必将电机和其他机构分离,电机会旋转360°! + * 注意,校准时务必将电机和其他机构分离,电机会旋转360°! + * + * @param motor + */ +void HTMotorCalibEncoder(HTMotorInstance *motor); #endif // !HT04_H#define HT04_H \ No newline at end of file diff --git a/modules/motor/LK9025.c b/modules/motor/LK9025.c index 82ca1a2..c265faf 100644 --- a/modules/motor/LK9025.c +++ b/modules/motor/LK9025.c @@ -2,25 +2,25 @@ #include "stdlib.h" static uint8_t idx; -static LKMotorInstance* lkmotor_instance[LK_MOTOR_MX_CNT]={NULL}; +static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL}; static void LKMotorDecode(CANInstance *_instance) { - static LKMotor_Measure_t* measure; - static uint8_t* rx_buff; - rx_buff=_instance->rx_buff; - measure=&((LKMotorInstance*)_instance)->measure; - - measure->last_ecd=measure->ecd; - measure->ecd=(uint16_t)((rx_buff[7] << 8) | rx_buff[6]); - measure->angle_single_round=ECD_ANGLE_COEF*measure->ecd; - measure->speed_aps=(1-SPEED_SMOOTH_COEF)*measure->speed_aps+ - SPEED_SMOOTH_COEF*(float)((int16_t)(rx_buff[5] << 8 | rx_buff[4])); - measure->real_current=(1-CURRENT_SMOOTH_COEF)*measure->real_current+ - CURRENT_SMOOTH_COEF*(float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])); - measure->temperate=rx_buff[1]; + static LKMotor_Measure_t *measure; + static uint8_t *rx_buff; + rx_buff = _instance->rx_buff; + measure = &((LKMotorInstance *)_instance)->measure; - //计算多圈角度 + measure->last_ecd = measure->ecd; + measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]); + measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd; + measure->speed_aps = (1 - SPEED_SMOOTH_COEF) * measure->speed_aps + + SPEED_SMOOTH_COEF * (float)((int16_t)(rx_buff[5] << 8 | rx_buff[4])); + measure->real_current = (1 - CURRENT_SMOOTH_COEF) * measure->real_current + + CURRENT_SMOOTH_COEF * (float)((int16_t)(rx_buff[3] << 8 | rx_buff[2])); + measure->temperate = rx_buff[1]; + + // 计算多圈角度 if (measure->ecd - measure->last_ecd > 32678) measure->total_round--; else if (measure->ecd - measure->last_ecd < -32678) @@ -28,48 +28,46 @@ static void LKMotorDecode(CANInstance *_instance) measure->total_angle = measure->total_round * 360 + measure->angle_single_round; } - void LKMotorControl() { - for (size_t i = 0; i < idx; i++) + for (size_t i = 0; i < idx; ++i) { - } } LKMotorInstance *LKMotroInit(Motor_Init_Config_s *config) { - lkmotor_instance[idx]=(LKMotorInstance*)malloc(sizeof(LKMotorInstance)); - memset(lkmotor_instance[idx],0,sizeof(LKMotorInstance)); + lkmotor_instance[idx] = (LKMotorInstance *)malloc(sizeof(LKMotorInstance)); + memset(lkmotor_instance[idx], 0, sizeof(LKMotorInstance)); - lkmotor_instance[idx]->motor_settings=config->controller_setting_init_config; - PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID); - PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID); - PID_Init(&lkmotor_instance[idx]->current_PID,&config->controller_param_init_config.current_PID); - lkmotor_instance[idx]->other_angle_feedback_ptr=config->controller_param_init_config.other_angle_feedback_ptr; - lkmotor_instance[idx]->other_speed_feedback_ptr=config->controller_param_init_config.other_speed_feedback_ptr; + lkmotor_instance[idx]->motor_settings = config->controller_setting_init_config; + PID_Init(&lkmotor_instance[idx]->current_PID, &config->controller_param_init_config.current_PID); + PID_Init(&lkmotor_instance[idx]->speed_PID, &config->controller_param_init_config.speed_PID); + PID_Init(&lkmotor_instance[idx]->angle_PID, &config->controller_param_init_config.angle_PID); + lkmotor_instance[idx]->other_angle_feedback_ptr = config->controller_param_init_config.other_angle_feedback_ptr; + lkmotor_instance[idx]->other_speed_feedback_ptr = config->controller_param_init_config.other_speed_feedback_ptr; - config->can_init_config.can_module_callback=LKMotorDecode; - config->can_init_config.rx_id=0x140+config->can_init_config.tx_id; - config->can_init_config.tx_id=config->can_init_config.tx_id+0x240; - lkmotor_instance[idx]->motor_can_ins=CANRegister(&config->can_init_config); + config->can_init_config.id = lkmotor_instance[idx]; + config->can_init_config.can_module_callback = LKMotorDecode; + config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id; + config->can_init_config.tx_id = config->can_init_config.tx_id + 0x240; + lkmotor_instance[idx]->motor_can_ins = CANRegister(&config->can_init_config); LKMotorEnable(lkmotor_instance[idx]); return lkmotor_instance[idx++]; } - void LKMotorStop(LKMotorInstance *motor) { - motor->stop_flag=MOTOR_STOP; + motor->stop_flag = MOTOR_STOP; } void LKMotorEnable(LKMotorInstance *motor) { - motor->stop_flag=MOTOR_ENALBED; + motor->stop_flag = MOTOR_ENALBED; } void LKMotorSetRef(LKMotorInstance *motor, float ref) { - motor->pid_ref=ref; + motor->pid_ref = ref; } diff --git a/modules/motor/LK9025.h b/modules/motor/LK9025.h index 7c9360c..592111e 100644 --- a/modules/motor/LK9025.h +++ b/modules/motor/LK9025.h @@ -13,7 +13,7 @@ #define CURRENT_SMOOTH_COEF 0.9f #define SPEED_SMOOTH_COEF 0.85f #define REDUCTION_RATIO_DRIVEN 1 -#define ECD_ANGLE_COEF (360.0f/65536.0f) +#define ECD_ANGLE_COEF_LK (360.0f/65536.0f) typedef struct // 9025 { diff --git a/modules/motor/dji_motor.c b/modules/motor/dji_motor.c index 4fce573..bda25c2 100644 --- a/modules/motor/dji_motor.c +++ b/modules/motor/dji_motor.c @@ -76,7 +76,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config) dji_motor_info[idx]->sender_group = motor_grouping; // 检查是否发生id冲突 - for (size_t i = 0; i < idx; i++) + for (size_t i = 0; i < idx; ++i) { if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id) IDcrash_Handler(i, idx); @@ -100,7 +100,7 @@ static void MotorSenderGrouping(CAN_Init_Config_s *config) dji_motor_info[idx]->message_num = motor_send_num; dji_motor_info[idx]->sender_group = motor_grouping; - for (size_t i = 0; i < idx; i++) + for (size_t i = 0; i < idx; ++i) { if (dji_motor_info[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_info[i]->motor_can_instance->rx_id == config->rx_id) IDcrash_Handler(i, idx); @@ -124,15 +124,14 @@ static void DecodeDJIMotor(CANInstance *_instance) static uint8_t *rxbuff; static DJI_Motor_Measure_s *measure; rxbuff = _instance->rx_buff; - measure = &((DJIMotorInstance*)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销 + measure = &((DJIMotorInstance *)_instance->id)->motor_measure; // measure要多次使用,保存指针减小访存开销 - uint8_t nice; // resolve data and apply filter to current and speed measure->last_ecd = measure->ecd; measure->ecd = ((uint16_t)rxbuff[0]) << 8 | rxbuff[1]; - measure->angle_single_round = ECD_ANGLE_COEF * (float)measure->ecd; - measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + RPM_2_ANGLE_PER_SEC * - SPEED_SMOOTH_COEF *(float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])) ; + measure->angle_single_round = ECD_ANGLE_COEF_DJI * (float)measure->ecd; + measure->speed_aps = (1.0f - SPEED_SMOOTH_COEF) * measure->speed_aps + + RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)((int16_t)(rxbuff[2] << 8 | rxbuff[3])); measure->real_current = (1.0f - CURRENT_SMOOTH_COEF) * measure->real_current + CURRENT_SMOOTH_COEF * (float)((int16_t)(rxbuff[4] << 8 | rxbuff[5])); measure->temperate = rxbuff[6]; @@ -167,7 +166,7 @@ DJIMotorInstance *DJIMotorInit(Motor_Init_Config_s *config) // register motor to CAN bus config->can_init_config.can_module_callback = DecodeDJIMotor; // set callback - config->can_init_config.id=dji_motor_info[idx]; //set id,eq to address(it is identity) + config->can_init_config.id = dji_motor_info[idx]; // set id,eq to address(it is identity) dji_motor_info[idx]->motor_can_instance = CANRegister(&config->can_init_config); DJIMotorEnable(dji_motor_info[idx]); @@ -220,7 +219,7 @@ void DJIMotorControl() static DJI_Motor_Measure_s *motor_measure; static float pid_measure, pid_ref; // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 - for (size_t i = 0; i < idx; i++) + for (size_t i = 0; i < idx; ++i) { if (dji_motor_info[i]) { @@ -279,7 +278,7 @@ void DJIMotorControl() } // 遍历flag,检查是否要发送这一帧报文 - for (size_t i = 0; i < 6; i++) + for (size_t i = 0; i < 6; ++i) { if (sender_enable_flag[i]) { diff --git a/modules/motor/dji_motor.h b/modules/motor/dji_motor.h index 5dde915..10ef101 100644 --- a/modules/motor/dji_motor.h +++ b/modules/motor/dji_motor.h @@ -24,7 +24,7 @@ /* 滤波系数设置为1的时候即关闭滤波 */ #define SPEED_SMOOTH_COEF 0.85f // better to be greater than 0.85 #define CURRENT_SMOOTH_COEF 0.9f // this coef *must* be greater than 0.9 -#define ECD_ANGLE_COEF 0.043945f // 360/8192,将编码器值转化为角度制 +#define ECD_ANGLE_COEF_DJI (360.0f/8192.0f) // ,将编码器值转化为角度制 /* DJI电机CAN反馈信息*/ typedef struct diff --git a/modules/motor/dji_motor.md b/modules/motor/dji_motor.md index 3f048d8..b383020 100644 --- a/modules/motor/dji_motor.md +++ b/modules/motor/dji_motor.md @@ -387,7 +387,7 @@ static dji_motor_instance *dji_motor_info[DJI_MOTOR_CNT] = {NULL}; ```c #define PI2 (3.141592f * 2) -#define ECD_ANGLE_COEF 3.835e-4 // ecd/8192*pi +#define ECD_ANGLE_COEF_DJI 3.835e-4 // ecd/8192*pi ``` 这两个宏用于在电机反馈信息中的多圈角度计算，将编码器的0~8192转化为角度表示。 diff --git a/modules/motor/motor_def.h b/modules/motor/motor_def.h index 2cf2cce..46c9842 100644 --- a/modules/motor/motor_def.h +++ b/modules/motor/motor_def.h @@ -99,13 +99,15 @@ typedef enum */ typedef struct { - float *other_angle_feedback_ptr; - float *other_speed_feedback_ptr; + float *other_angle_feedback_ptr; // 角度反馈数据指针,注意电机使用total_angle + float *other_speed_feedback_ptr; // 速度反馈数据指针,单位为angle per sec + + float *speed_feedforward_ptr; // 速度前馈数据指针 + float *current_feedforward_ptr; // 电流前馈数据指针 PID_Init_config_s current_PID; PID_Init_config_s speed_PID; PID_Init_config_s angle_PID; - } Motor_Controller_Init_s; /* 用于初始化CAN电机的结构体,各类电机通用 */ diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 9960294..a017199 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -15,7 +15,7 @@ static USARTInstance *rc_usart_instance; */ static void RectifyRCjoystick() { - for (uint8_t i = 0; i < 5; i++) + for (uint8_t i = 0; i < 5; ++i) { if (*(&rc_ctrl[TEMP].rc.rocker_l_+i) > 660 || *(&rc_ctrl[TEMP].rc.rocker_l_+i) < -660) *(&rc_ctrl[TEMP].rc.rocker_l_+i) = 0;