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

fix can rx fifo bug

This commit is contained in:
NeoZeng 2022-11-23 22:10:44 +08:00
parent 62b1783b59
commit 490b957045
11 changed files with 123 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored
View File

@ -4,7 +4,10 @@
"stdint-gcc.h": "c",
"led_task.h": "c",
"stdinf.h": "c",
"usart.h": "c"
"usart.h": "c",
"controller.h": "c",
"ins_task.h": "c",
"task.h": "c"
},
"C_Cpp.default.configurationProvider": "ms-vscode.makefile-tools",
}

31
HAL_N_Middlewares/Src/freertos.c
View File

@ -25,7 +25,8 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "ins_task.h"
#include "motor_task.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -51,6 +52,9 @@ osThreadId defaultTaskHandle;
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
void StartINSTASK(void const * argument);
void StartMOTORTASK(void const * argument);
/* USER CODE END FunctionPrototypes */
@ -106,6 +110,12 @@ void MX_FREERTOS_Init(void) {
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
osThreadDef(instask, StartINSTASK, osPriorityNormal, 0, 1024);
defaultTaskHandle = osThreadCreate(osThread(instask), NULL);
osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256);
defaultTaskHandle = osThreadCreate(osThread(motortask), NULL);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
/* USER CODE END RTOS_THREADS */
@ -134,5 +144,24 @@ void StartDefaultTask(void const * argument)
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
void StartINSTASK(void const * argument)
{
while (1)
{
INS_Task();
osDelay(1);
}
}
void StartMOTORTASK(void const * argument)
{
while (1)
{
MotorControlTask();
osDelay(1);
}
}
/* USER CODE END Application */

13
HAL_N_Middlewares/Src/main.c
View File

@ -77,8 +77,7 @@ void MX_FREERTOS_Init(void);
* @brief The application entry point.
* @retval int
*/
int main(void)
{
int main(void){
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
@ -119,12 +118,12 @@ int main(void)
RC_init(&huart3);
DWT_Init(168);
Motor_Init_Config_s config = {
.motor_type = M2006,
.motor_type = M3508,
.can_init_config = {
.can_handle = &hcan1,
.tx_id = 1},
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | CURRENT_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL},
.controller_param_init_config = {.current_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}};
.tx_id = 2},
.controller_setting_init_config = {.angle_feedback_source = MOTOR_FEED, .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .speed_feedback_source = MOTOR_FEED, .reverse_flag = MOTOR_DIRECTION_NORMAL},
.controller_param_init_config = {.angle_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}, .speed_PID = {.Improve = 0, .Kp = 1, .Ki = 0, .Kd = 0, .DeadBand = 0, .MaxOut = 4000}}};
dji_motor_instance *djimotor = DJIMotorInit(config);
RefereeInit(&huart6);
@ -143,7 +142,7 @@ int main(void)
{
/* USER CODE END WHILE */
DJIMotorSetRef(djimotor, get_remote_control_point()->rc.ch[0]);
DJIMotorSetRef(djimotor, 10);
MotorControlTask();
HAL_Delay(100);
/* USER CODE BEGIN 3 */

19
bsp/bsp_can.c
View File

@ -29,13 +29,30 @@ static void CANAddFilter(can_instance *_instance)
can_filter_conf.FilterMode = CAN_FILTERMODE_IDLIST;
can_filter_conf.FilterScale = CAN_FILTERSCALE_16BIT;
can_filter_conf.FilterFIFOAssignment = (_instance->rx_id & 1) ? CAN_RX_FIFO0 : CAN_RX_FIFO1;
can_filter_conf.FilterFIFOAssignment = CAN_RX_FIFO1;
can_filter_conf.SlaveStartFilterBank = 14;
can_filter_conf.FilterIdLow = _instance->rx_id << 5;
can_filter_conf.FilterIdHigh = _instance->rx_id << 5;
can_filter_conf.FilterMaskIdLow = _instance->rx_id << 5;
can_filter_conf.FilterMaskIdHigh = _instance->rx_id << 5;
can_filter_conf.FilterBank = _instance->can_handle == &hcan1 ? (can1_filter_idx++) : (can2_filter_idx++);
can_filter_conf.FilterActivation = CAN_FILTER_ENABLE;
HAL_CAN_ConfigFilter(_instance->can_handle, &can_filter_conf);
// CAN_FilterTypeDef can_filter_st;
// can_filter_st.FilterActivation = ENABLE;
// can_filter_st.FilterMode = CAN_FILTERMODE_IDMASK;
// can_filter_st.FilterScale = CAN_FILTERSCALE_32BIT;
// can_filter_st.FilterIdHigh = 0x0000;
// can_filter_st.FilterIdLow = 0x0000;
// can_filter_st.FilterMaskIdHigh = 0x0000;
// can_filter_st.FilterMaskIdLow = 0x0000;
// can_filter_st.FilterBank = 0;
// can_filter_st.FilterFIFOAssignment = CAN_RX_FIFO0;
// HAL_CAN_ConfigFilter(&hcan1, &can_filter_st);
// HAL_CAN_Start(&hcan1);
// HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING);
}
/**

1
modules/algorithm/QuaternionEKF.c
View File

@ -11,6 +11,7 @@
* 1
* ———————
* as + 1
*
******************************************************************************
*/
#include "QuaternionEKF.h"

42
modules/imu/BMI088driver.c
View File

@ -1,15 +1,3 @@
/**
******************************************************************************
* @file BMI088driver.c
* @author
* @version V1.2.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "BMI088driver.h"
#include "BMI088reg.h"
#include "BMI088Middleware.h"
@ -82,30 +70,30 @@ static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len);
#endif
static uint8_t write_BMI088_accel_reg_data_error[BMI088_WRITE_ACCEL_REG_NUM][3] =
{
{BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
{BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
{BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
{BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
{BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
{BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}
{
{BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
{BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
{BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
{BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
{BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW, BMI088_INT1_IO_CTRL_ERROR},
{BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}
};
static uint8_t write_BMI088_gyro_reg_data_error[BMI088_WRITE_GYRO_REG_NUM][3] =
{
{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
{BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
{BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
{BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
{BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
{BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}
{
{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
{BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
{BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
{BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
{BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW, BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
{BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}
};
static void Calibrate_MPU_Offset(IMU_Data_t *bmi088);
uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate)
uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate)
{
BMI088_SPI = bmi088_SPI;
error = BMI088_NO_ERROR;

50
modules/imu/BMI088driver.h
View File

@ -1,16 +1,3 @@
/**
******************************************************************************
* @file BMI088driver.h
* @author
* @version V1.1.2
* @version V1.2.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#ifndef BMI088DRIVER_H
#define BMI088DRIVER_H
@ -44,7 +31,7 @@
#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
// ÐèÊÖ¶¯ÐÞ¸Ä
// <EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD><EFBFBD>޸<EFBFBD>
#if INFANTRY_ID == 0
#define GxOFFSET 0.00247530174f
#define GyOFFSET 0.000393082853f
@ -72,6 +59,7 @@
#define gNORM 9.876785f
#endif
/* IMU数据结构体 */
typedef struct
{
float Accel[3];
@ -87,6 +75,7 @@ typedef struct
float gNorm;
} IMU_Data_t;
/* BMI088错误码枚举 */
enum
{
BMI088_NO_ERROR = 0x00,
@ -109,13 +98,36 @@ enum
BMI088_NO_SENSOR = 0xFF,
};
void BMI088_Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
extern uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
extern uint8_t bmi088_accel_init(void);
extern uint8_t bmi088_gyro_init(void);
extern IMU_Data_t BMI088;
/**
* @brief 初始化BMI088,传入连接的SPI总线handle,以及是否进行在线标定
*
* @param bmi088_SPI handle
* @param calibrate 1为进行在线标定,0使用离线数据
* @return uint8_t 成功则返回BMI088_NO_ERROR
*/
extern uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
/**
* @brief 加速计初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_accel_init(void);
/**
* @brief 陀螺仪初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_gyro_init(void);
/**
* @brief 读取一次BMI088的数据,包括gyro和accel
*
* @param bmi088 传入BMI088实例(结构体)
*/
extern void BMI088_Read(IMU_Data_t *bmi088);
#endif

15
modules/imu/ins_task.c
View File

@ -26,13 +26,14 @@ const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔
uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
uint8_t ins_debug_mode = 0;
float RefTemp = 40;
float RefTemp = 40; // 恒温设定温度
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
/* 调用此函数获得姿态数据指针 */
attitude_t *GetINSptr()
{
return (attitude_t *)&INS.Roll;
@ -45,13 +46,12 @@ attitude_t *GetINSptr()
static void IMU_Temperature_Ctrl(void)
{
PID_Calculate(&TempCtrl, BMI088.Temperature, RefTemp);
imu_pwm_set(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
void INS_Init(void)
{
// while (BMI088_init(&hspi1, 1) != BMI088_NO_ERROR);
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR);
IMU_Param.scale[X] = 1;
IMU_Param.scale[Y] = 1;
IMU_Param.scale[Z] = 1;
@ -75,6 +75,7 @@ void INS_Init(void)
INS.AccelLPF = 0.0085;
}
/* 注意以1kHz的频率运行此任务 */
void INS_Task(void)
{
static uint32_t count = 0;
@ -134,12 +135,10 @@ void INS_Task(void)
IMU_Temperature_Ctrl();
}
if ((count % 1000) == 0)
if ((count++ % 1000) == 0)
{
// 200hz
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
count++;
}
/**

11
modules/imu/ins_task.h
View File

@ -2,11 +2,13 @@
******************************************************************************
* @file ins_task.h
* @author Wang Hongxi
* @author annotation and modification by NeoZeng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
* @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
* 应用层调用初始化函数.
*
******************************************************************************
*/
@ -29,7 +31,7 @@ typedef struct
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t;
} attitude_t; //最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct
{
@ -57,10 +59,7 @@ typedef struct
float YawTotalAngle;
} INS_t;
/**
* @brief 用于修正安装误差的参数
*
*/
/* 用于修正安装误差的参数 */
typedef struct
{
uint8_t flag;

3
modules/imu/ins_task.md
View File

@ -5,3 +5,6 @@
## 硬触发流程
![image-20221113212706633](assets\image-20221113212706633.png)
## 算法解析
介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643)

2
modules/motor/dji_motor.c
View File

@ -242,7 +242,7 @@ void DJIMotorControl()
group = motor->sender_group;
num = motor->message_num;
sender_assignment[group].tx_buff[num] = 0xff & set >> 8;
sender_assignment[group].tx_buff[num + 1] = 0xff & set;
sender_assignment[group].tx_buff[2*num + 1] = 0xff & set;
}
else // 遇到空指针说明所有遍历结束,退出循环
break;