fix can rx fifo bug
This commit is contained in:
NeoZeng
parent
62b1783b59
commit
490b957045
|
|
@ -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",
|
||||
}
|
||||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@
|
|||
* 1
|
||||
* ———————
|
||||
* as + 1
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
#include "QuaternionEKF.h"
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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++;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -2,12 +2,14 @@
|
|||
******************************************************************************
|
||||
* @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拥有实例,随后在
|
||||
* 应用层调用初始化函数.
|
||||
*
|
||||
******************************************************************************
|
||||
*/
|
||||
#ifndef __INS_TASK_H
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -4,4 +4,7 @@
|
|||
|
||||
## 硬触发流程
|
||||
|
||||
|
||||
|
||||
|
||||
## 算法解析
|
||||
介绍EKF四元数姿态解算的教程在:[四元数EKF姿态更新算法](https://zhuanlan.zhihu.com/p/454155643)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue