Compare commits
No commits in common. "51fe29f605879981902b7dca483f6cdcff58c902" and "cedc8bbab1c49793d583d72bc0fb02982add9c8e" have entirely different histories.
51fe29f605
...
cedc8bbab1
@ -67,8 +67,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
User/component/vmc.c
|
User/component/vmc.c
|
||||||
|
|
||||||
# User/device sources
|
# User/device sources
|
||||||
User/device/bmi088.c
|
|
||||||
User/device/buzzer.c
|
User/device/buzzer.c
|
||||||
|
User/device/dm_imu.c
|
||||||
User/device/dr16.c
|
User/device/dr16.c
|
||||||
User/device/motor.c
|
User/device/motor.c
|
||||||
User/device/motor_dm.c
|
User/device/motor_dm.c
|
||||||
@ -80,18 +80,13 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
|
|||||||
# User/module sources
|
# User/module sources
|
||||||
User/module/balance_chassis.c
|
User/module/balance_chassis.c
|
||||||
User/module/config.c
|
User/module/config.c
|
||||||
User/module/gimbal.c
|
|
||||||
User/module/shoot.c
|
|
||||||
|
|
||||||
# User/task sources
|
# User/task sources
|
||||||
User/task/ai.c
|
|
||||||
User/task/atti_esti.c
|
User/task/atti_esti.c
|
||||||
User/task/blink.c
|
User/task/blink.c
|
||||||
User/task/ctrl_chassis.c
|
User/task/ctrl_chassis.c
|
||||||
User/task/ctrl_gimbal.c
|
User/task/ctrl_gimbal.c
|
||||||
User/task/ctrl_shoot.c
|
|
||||||
User/task/init.c
|
User/task/init.c
|
||||||
User/task/monitor.c
|
|
||||||
User/task/rc.c
|
User/task/rc.c
|
||||||
User/task/user_task.c
|
User/task/user_task.c
|
||||||
)
|
)
|
||||||
|
|||||||
@ -24,9 +24,7 @@ extern "C" {
|
|||||||
#ifndef M_PI
|
#ifndef M_PI
|
||||||
#define M_PI 3.14159265358979323846f
|
#define M_PI 3.14159265358979323846f
|
||||||
#endif
|
#endif
|
||||||
#ifndef M_PI_2
|
|
||||||
#define M_PI_2 1.57079632679489661923f
|
|
||||||
#endif
|
|
||||||
#ifndef M_2PI
|
#ifndef M_2PI
|
||||||
#define M_2PI 6.28318530717958647692f
|
#define M_2PI 6.28318530717958647692f
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -1,381 +0,0 @@
|
|||||||
/*
|
|
||||||
BMI088 陀螺仪+加速度计传感器。
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "bmi088.h"
|
|
||||||
|
|
||||||
#include <cmsis_os2.h>
|
|
||||||
#include <gpio.h>
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <string.h>
|
|
||||||
|
|
||||||
#include "bsp/time.h"
|
|
||||||
#include "bsp/gpio.h"
|
|
||||||
#include "bsp/spi.h"
|
|
||||||
#include "component/user_math.h"
|
|
||||||
|
|
||||||
/* USER INCLUDE BEGIN */
|
|
||||||
|
|
||||||
/* USER INCLUDE END */
|
|
||||||
|
|
||||||
/* Private define ----------------------------------------------------------- */
|
|
||||||
/* Private define ----------------------------------------------------------- */
|
|
||||||
#define BMI088_REG_ACCL_CHIP_ID (0x00)
|
|
||||||
#define BMI088_REG_ACCL_ERR (0x02)
|
|
||||||
#define BMI088_REG_ACCL_STATUS (0x03)
|
|
||||||
#define BMI088_REG_ACCL_X_LSB (0x12)
|
|
||||||
#define BMI088_REG_ACCL_X_MSB (0x13)
|
|
||||||
#define BMI088_REG_ACCL_Y_LSB (0x14)
|
|
||||||
#define BMI088_REG_ACCL_Y_MSB (0x15)
|
|
||||||
#define BMI088_REG_ACCL_Z_LSB (0x16)
|
|
||||||
#define BMI088_REG_ACCL_Z_MSB (0x17)
|
|
||||||
#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
|
|
||||||
#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
|
|
||||||
#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
|
|
||||||
#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
|
|
||||||
#define BMI088_REG_ACCL_TEMP_MSB (0x22)
|
|
||||||
#define BMI088_REG_ACCL_TEMP_LSB (0x23)
|
|
||||||
#define BMI088_REG_ACCL_CONF (0x40)
|
|
||||||
#define BMI088_REG_ACCL_RANGE (0x41)
|
|
||||||
#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
|
|
||||||
#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
|
|
||||||
#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
|
|
||||||
#define BMI088_REG_ACCL_SELF_TEST (0x6D)
|
|
||||||
#define BMI088_REG_ACCL_PWR_CONF (0x7C)
|
|
||||||
#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
|
|
||||||
#define BMI088_REG_ACCL_SOFTRESET (0x7E)
|
|
||||||
|
|
||||||
#define BMI088_REG_GYRO_CHIP_ID (0x00)
|
|
||||||
#define BMI088_REG_GYRO_X_LSB (0x02)
|
|
||||||
#define BMI088_REG_GYRO_X_MSB (0x03)
|
|
||||||
#define BMI088_REG_GYRO_Y_LSB (0x04)
|
|
||||||
#define BMI088_REG_GYRO_Y_MSB (0x05)
|
|
||||||
#define BMI088_REG_GYRO_Z_LSB (0x06)
|
|
||||||
#define BMI088_REG_GYRO_Z_MSB (0x07)
|
|
||||||
#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
|
|
||||||
#define BMI088_REG_GYRO_RANGE (0x0F)
|
|
||||||
#define BMI088_REG_GYRO_BANDWIDTH (0x10)
|
|
||||||
#define BMI088_REG_GYRO_LPM1 (0x11)
|
|
||||||
#define BMI088_REG_GYRO_SOFTRESET (0x14)
|
|
||||||
#define BMI088_REG_GYRO_INT_CTRL (0x15)
|
|
||||||
#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
|
|
||||||
#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
|
|
||||||
#define BMI088_REG_GYRO_SELF_TEST (0x3C)
|
|
||||||
|
|
||||||
#define BMI088_CHIP_ID_ACCL (0x1E)
|
|
||||||
#define BMI088_CHIP_ID_GYRO (0x0F)
|
|
||||||
|
|
||||||
#define BMI088_LEN_RX_BUFF (19)
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
|
||||||
#define BMI088_ACCL_NSS_SET() \
|
|
||||||
BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET)
|
|
||||||
#define BMI088_ACCL_NSS_RESET() \
|
|
||||||
BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET)
|
|
||||||
|
|
||||||
#define BMI088_GYRO_NSS_SET() \
|
|
||||||
BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET)
|
|
||||||
#define BMI088_GYRO_NSS_RESET() \
|
|
||||||
BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET)
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
|
||||||
typedef enum {
|
|
||||||
BMI_ACCL,
|
|
||||||
BMI_GYRO,
|
|
||||||
} BMI_Device_t;
|
|
||||||
|
|
||||||
/* USER STRUCT BEGIN */
|
|
||||||
|
|
||||||
/* USER STRUCT END */
|
|
||||||
|
|
||||||
/* Private variables -------------------------------------------------------- */
|
|
||||||
static uint8_t buffer[2];
|
|
||||||
static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];
|
|
||||||
|
|
||||||
static osThreadId_t thread_alert;
|
|
||||||
static bool inited = false;
|
|
||||||
|
|
||||||
/* Private function -------------------------------------------------------- */
|
|
||||||
/* USER FUNCTION BEGIN */
|
|
||||||
|
|
||||||
/* USER FUNCTION END */
|
|
||||||
|
|
||||||
static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
|
|
||||||
buffer[0] = (reg & 0x7f);
|
|
||||||
buffer[1] = data;
|
|
||||||
|
|
||||||
BSP_TIME_Delay(1);
|
|
||||||
switch (dv) {
|
|
||||||
case BMI_ACCL:
|
|
||||||
BMI088_ACCL_NSS_RESET();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case BMI_GYRO:
|
|
||||||
BMI088_GYRO_NSS_RESET();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false);
|
|
||||||
|
|
||||||
switch (dv) {
|
|
||||||
case BMI_ACCL:
|
|
||||||
BMI088_ACCL_NSS_SET();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case BMI_GYRO:
|
|
||||||
BMI088_GYRO_NSS_SET();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
|
|
||||||
BSP_TIME_Delay(1);
|
|
||||||
switch (dv) {
|
|
||||||
case BMI_ACCL:
|
|
||||||
BMI088_ACCL_NSS_RESET();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case BMI_GYRO:
|
|
||||||
BMI088_GYRO_NSS_RESET();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
buffer[0] = (uint8_t)(reg | 0x80);
|
|
||||||
BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
|
|
||||||
BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false);
|
|
||||||
|
|
||||||
switch (dv) {
|
|
||||||
case BMI_ACCL:
|
|
||||||
BMI088_ACCL_NSS_SET();
|
|
||||||
return buffer[1];
|
|
||||||
|
|
||||||
case BMI_GYRO:
|
|
||||||
BMI088_GYRO_NSS_SET();
|
|
||||||
return buffer[0];
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
|
|
||||||
if (data == NULL) return;
|
|
||||||
|
|
||||||
switch (dv) {
|
|
||||||
case BMI_ACCL:
|
|
||||||
BMI088_ACCL_NSS_RESET();
|
|
||||||
break;
|
|
||||||
|
|
||||||
case BMI_GYRO:
|
|
||||||
BMI088_GYRO_NSS_RESET();
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
buffer[0] = (uint8_t)(reg | 0x80);
|
|
||||||
BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
|
|
||||||
BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void BMI088_RxCpltCallback(void) {
|
|
||||||
if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) {
|
|
||||||
BMI088_ACCL_NSS_SET();
|
|
||||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
|
|
||||||
}
|
|
||||||
if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) {
|
|
||||||
BMI088_GYRO_NSS_SET();
|
|
||||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
static void BMI088_AcclIntCallback(void) {
|
|
||||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
|
|
||||||
}
|
|
||||||
|
|
||||||
static void BMI088_GyroIntCallback(void) {
|
|
||||||
osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
|
||||||
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
|
|
||||||
if (bmi088 == NULL) return DEVICE_ERR_NULL;
|
|
||||||
if (cali == NULL) return DEVICE_ERR_NULL;
|
|
||||||
if (inited) return DEVICE_ERR_INITED;
|
|
||||||
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
bmi088->cali = cali;
|
|
||||||
|
|
||||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
|
|
||||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
|
|
||||||
BSP_TIME_Delay(30);
|
|
||||||
|
|
||||||
/* Switch accl to SPI mode. */
|
|
||||||
BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);
|
|
||||||
|
|
||||||
if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
|
|
||||||
return DEVICE_ERR_NO_DEV;
|
|
||||||
|
|
||||||
if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
|
|
||||||
return DEVICE_ERR_NO_DEV;
|
|
||||||
|
|
||||||
BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT);
|
|
||||||
BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT);
|
|
||||||
|
|
||||||
BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB,
|
|
||||||
BMI088_RxCpltCallback);
|
|
||||||
BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback);
|
|
||||||
BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback);
|
|
||||||
|
|
||||||
/* Accl init. */
|
|
||||||
/* Filter setting: Normal. */
|
|
||||||
/* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
|
|
||||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);
|
|
||||||
|
|
||||||
/* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
|
|
||||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);
|
|
||||||
|
|
||||||
/* INT1 as output. Push-pull. Active low. Output. */
|
|
||||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);
|
|
||||||
|
|
||||||
/* Map data ready interrupt to INT1. */
|
|
||||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);
|
|
||||||
|
|
||||||
/* Turn on accl. Now we can read data. */
|
|
||||||
BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
|
|
||||||
BSP_TIME_Delay(50);
|
|
||||||
|
|
||||||
/* Gyro init. */
|
|
||||||
/* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
|
|
||||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);
|
|
||||||
|
|
||||||
/* Filter bw: 47Hz. */
|
|
||||||
/* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
|
|
||||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);
|
|
||||||
|
|
||||||
/* INT3 and INT4 as output. Push-pull. Active low. */
|
|
||||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);
|
|
||||||
|
|
||||||
/* Map data ready interrupt to INT3. */
|
|
||||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);
|
|
||||||
|
|
||||||
/* Enable new data interrupt. */
|
|
||||||
BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);
|
|
||||||
|
|
||||||
BSP_TIME_Delay(10);
|
|
||||||
|
|
||||||
inited = true;
|
|
||||||
|
|
||||||
BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT);
|
|
||||||
BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT);
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
|
|
||||||
return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t BMI088_WaitNew() {
|
|
||||||
return osThreadFlagsWait(
|
|
||||||
SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
|
|
||||||
osWaitForever);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t BMI088_AcclStartDmaRecv() {
|
|
||||||
BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t BMI088_AcclWaitDmaCplt() {
|
|
||||||
return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
|
|
||||||
osWaitForever);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t BMI088_GyroStartDmaRecv() {
|
|
||||||
BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t BMI088_GyroWaitDmaCplt() {
|
|
||||||
return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
|
|
||||||
osWaitForever);
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
|
|
||||||
if (bmi088 == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
#if 1
|
|
||||||
int16_t raw_x, raw_y, raw_z;
|
|
||||||
memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
|
|
||||||
memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
|
|
||||||
memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));
|
|
||||||
|
|
||||||
bmi088->accl.x = (float)raw_x;
|
|
||||||
bmi088->accl.y = (float)raw_y;
|
|
||||||
bmi088->accl.z = (float)raw_z;
|
|
||||||
|
|
||||||
#else
|
|
||||||
const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
|
|
||||||
const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
|
|
||||||
const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);
|
|
||||||
|
|
||||||
bmi088->accl.x = (float)*praw_x;
|
|
||||||
bmi088->accl.y = (float)*praw_y;
|
|
||||||
bmi088->accl.z = (float)*praw_z;
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
|
|
||||||
bmi088->accl.x /= 5460.0f;
|
|
||||||
bmi088->accl.y /= 5460.0f;
|
|
||||||
bmi088->accl.z /= 5460.0f;
|
|
||||||
|
|
||||||
int16_t raw_temp =
|
|
||||||
(uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));
|
|
||||||
|
|
||||||
if (raw_temp > 1023) raw_temp -= 2048;
|
|
||||||
|
|
||||||
bmi088->temp = (float)raw_temp * 0.125f + 23.0f;
|
|
||||||
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
|
|
||||||
if (bmi088 == NULL) return DEVICE_ERR_NULL;
|
|
||||||
|
|
||||||
#if 1
|
|
||||||
/* Gyroscope imu_raw -> degrees/sec -> radians/sec */
|
|
||||||
int16_t raw_x, raw_y, raw_z;
|
|
||||||
memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
|
|
||||||
memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
|
|
||||||
memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));
|
|
||||||
|
|
||||||
bmi088->gyro.x = (float)raw_x;
|
|
||||||
bmi088->gyro.y = (float)raw_y;
|
|
||||||
bmi088->gyro.z = (float)raw_z;
|
|
||||||
|
|
||||||
#else
|
|
||||||
/* Gyroscope imu_raw -> degrees/sec -> radians/sec */
|
|
||||||
const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
|
|
||||||
const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
|
|
||||||
const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);
|
|
||||||
|
|
||||||
bmi088->gyro.x = (float)*raw_x;
|
|
||||||
bmi088->gyro.y = (float)*raw_y;
|
|
||||||
bmi088->gyro.z = (float)*raw_z;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
|
|
||||||
* FS2000: 16.384.*/
|
|
||||||
bmi088->gyro.x /= 32.768f;
|
|
||||||
bmi088->gyro.y /= 32.768f;
|
|
||||||
bmi088->gyro.z /= 32.768f;
|
|
||||||
|
|
||||||
bmi088->gyro.x *= M_DEG2RAD_MULT;
|
|
||||||
bmi088->gyro.y *= M_DEG2RAD_MULT;
|
|
||||||
bmi088->gyro.z *= M_DEG2RAD_MULT;
|
|
||||||
|
|
||||||
bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
|
|
||||||
bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
|
|
||||||
bmi088->gyro.z -= bmi088->cali->gyro_offset.z;
|
|
||||||
|
|
||||||
return DEVICE_ERR_NULL;
|
|
||||||
}
|
|
||||||
|
|
||||||
float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
|
|
||||||
(void)bmi088;
|
|
||||||
return 400.0f;
|
|
||||||
}
|
|
||||||
@ -1,81 +0,0 @@
|
|||||||
#pragma once
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
extern "C" {
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include <stdint.h>
|
|
||||||
|
|
||||||
#include "component/ahrs.h"
|
|
||||||
#include "device/device.h"
|
|
||||||
|
|
||||||
/* USER INCLUDE BEGIN */
|
|
||||||
|
|
||||||
/* USER INCLUDE END */
|
|
||||||
|
|
||||||
/* USER DEFINE BEGIN */
|
|
||||||
|
|
||||||
/* USER DEFINE END */
|
|
||||||
|
|
||||||
/* Exported constants ------------------------------------------------------- */
|
|
||||||
/* Exported macro ----------------------------------------------------------- */
|
|
||||||
/* Exported types ----------------------------------------------------------- */
|
|
||||||
typedef struct {
|
|
||||||
struct {
|
|
||||||
float x;
|
|
||||||
float y;
|
|
||||||
float z;
|
|
||||||
} gyro_offset; /* 陀螺仪偏置 */
|
|
||||||
} BMI088_Cali_t; /* BMI088校准数据 */
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
DEVICE_Header_t header;
|
|
||||||
AHRS_Accl_t accl;
|
|
||||||
AHRS_Gyro_t gyro;
|
|
||||||
|
|
||||||
float temp; /* 温度 */
|
|
||||||
|
|
||||||
const BMI088_Cali_t *cali;
|
|
||||||
} BMI088_t;
|
|
||||||
|
|
||||||
/* USER STRUCT BEGIN */
|
|
||||||
|
|
||||||
/* USER STRUCT END */
|
|
||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
|
||||||
int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali);
|
|
||||||
int8_t BMI088_Restart(void);
|
|
||||||
|
|
||||||
bool BMI088_GyroStable(AHRS_Gyro_t *gyro);
|
|
||||||
|
|
||||||
/* Sensor use right-handed coordinate system. */
|
|
||||||
/*
|
|
||||||
x < R(logo)
|
|
||||||
y
|
|
||||||
UP is z
|
|
||||||
All implementation should follow this rule.
|
|
||||||
*/
|
|
||||||
uint32_t BMI088_WaitNew();
|
|
||||||
|
|
||||||
/*
|
|
||||||
BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。
|
|
||||||
即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能
|
|
||||||
出现 BMI088_GyroStartDmaRecv()。
|
|
||||||
*/
|
|
||||||
int8_t BMI088_AcclStartDmaRecv();
|
|
||||||
uint32_t BMI088_AcclWaitDmaCplt();
|
|
||||||
int8_t BMI088_GyroStartDmaRecv();
|
|
||||||
uint32_t BMI088_GyroWaitDmaCplt();
|
|
||||||
int8_t BMI088_ParseAccl(BMI088_t *bmi088);
|
|
||||||
int8_t BMI088_ParseGyro(BMI088_t *bmi088);
|
|
||||||
float BMI088_GetUpdateFreq(BMI088_t *bmi088);
|
|
||||||
|
|
||||||
/* USER FUNCTION BEGIN */
|
|
||||||
|
|
||||||
/* USER FUNCTION END */
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
@ -23,10 +23,6 @@ extern "C" {
|
|||||||
|
|
||||||
/* AUTO GENERATED SIGNALS BEGIN */
|
/* AUTO GENERATED SIGNALS BEGIN */
|
||||||
#define SIGNAL_DR16_RAW_REDY (1u << 0)
|
#define SIGNAL_DR16_RAW_REDY (1u << 0)
|
||||||
#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 1)
|
|
||||||
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
|
|
||||||
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3)
|
|
||||||
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4)
|
|
||||||
/* AUTO GENERATED SIGNALS END */
|
/* AUTO GENERATED SIGNALS END */
|
||||||
|
|
||||||
/* USER SIGNALS BEGIN */
|
/* USER SIGNALS BEGIN */
|
||||||
|
|||||||
@ -1,11 +1,3 @@
|
|||||||
bmi088:
|
|
||||||
bsp_config:
|
|
||||||
BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS
|
|
||||||
BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT
|
|
||||||
BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS
|
|
||||||
BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT
|
|
||||||
BSP_SPI_BMI088: BSP_SPI_BMI088
|
|
||||||
enabled: true
|
|
||||||
buzzer:
|
buzzer:
|
||||||
bsp_config:
|
bsp_config:
|
||||||
BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
|
BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
|
||||||
|
|||||||
270
User/device/dm_imu.c
Normal file
270
User/device/dm_imu.c
Normal file
@ -0,0 +1,270 @@
|
|||||||
|
/*
|
||||||
|
DM_IMU数据获取（CAN）
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "dm_imu.h"
|
||||||
|
|
||||||
|
#include "bsp/can.h"
|
||||||
|
#include "bsp/time.h"
|
||||||
|
#include "component/user_math.h"
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
/* Private define ----------------------------------------------------------- */
|
||||||
|
#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms
|
||||||
|
|
||||||
|
#define ACCEL_CAN_MAX (58.8f)
|
||||||
|
#define ACCEL_CAN_MIN (-58.8f)
|
||||||
|
#define GYRO_CAN_MAX (34.88f)
|
||||||
|
#define GYRO_CAN_MIN (-34.88f)
|
||||||
|
#define PITCH_CAN_MAX (90.0f)
|
||||||
|
#define PITCH_CAN_MIN (-90.0f)
|
||||||
|
#define ROLL_CAN_MAX (180.0f)
|
||||||
|
#define ROLL_CAN_MIN (-180.0f)
|
||||||
|
#define YAW_CAN_MAX (180.0f)
|
||||||
|
#define YAW_CAN_MIN (-180.0f)
|
||||||
|
#define TEMP_MIN (0.0f)
|
||||||
|
#define TEMP_MAX (60.0f)
|
||||||
|
#define Quaternion_MIN (-1.0f)
|
||||||
|
#define Quaternion_MAX (1.0f)
|
||||||
|
|
||||||
|
|
||||||
|
/* Private macro ------------------------------------------------------------ */
|
||||||
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
|
/* Private variables -------------------------------------------------------- */
|
||||||
|
/* Private function --------------------------------------------------------- */
|
||||||
|
|
||||||
|
static uint8_t count = 0; // 计数器，用于判断设备是否离线
|
||||||
|
/**
|
||||||
|
* @brief: 无符号整数转换为浮点数函数
|
||||||
|
*/
|
||||||
|
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
|
||||||
|
{
|
||||||
|
float span = x_max - x_min;
|
||||||
|
float offset = x_min;
|
||||||
|
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 解析加速度计数据
|
||||||
|
*/
|
||||||
|
static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
|
||||||
|
if (imu == NULL || data == NULL || len < 8) {
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
int8_t temp = data[1];
|
||||||
|
uint16_t acc_x_raw = (data[3] << 8) | data[2];
|
||||||
|
uint16_t acc_y_raw = (data[5] << 8) | data[4];
|
||||||
|
uint16_t acc_z_raw = (data[7] << 8) | data[6];
|
||||||
|
imu->data.temp = (float)temp;
|
||||||
|
imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||||||
|
imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||||||
|
imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief 解析陀螺仪数据
|
||||||
|
*/
|
||||||
|
static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
|
||||||
|
if (imu == NULL || data == NULL || len < 8) {
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
uint16_t gyro_x_raw = (data[3] << 8) | data[2];
|
||||||
|
uint16_t gyro_y_raw = (data[5] << 8) | data[4];
|
||||||
|
uint16_t gyro_z_raw = (data[7] << 8) | data[6];
|
||||||
|
imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||||||
|
imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||||||
|
imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
/**
|
||||||
|
* @brief 解析欧拉角数据
|
||||||
|
*/
|
||||||
|
static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
|
||||||
|
if (imu == NULL || data == NULL || len < 8) {
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
uint16_t pit_raw = (data[3] << 8) | data[2];
|
||||||
|
uint16_t yaw_raw = (data[5] << 8) | data[4];
|
||||||
|
uint16_t rol_raw = (data[7] << 8) | data[6];
|
||||||
|
imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
|
||||||
|
imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
|
||||||
|
imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 解析四元数数据
|
||||||
|
*/
|
||||||
|
static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
|
||||||
|
if (imu == NULL || data == NULL || len < 8) {
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
|
||||||
|
int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
|
||||||
|
int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
|
||||||
|
int z = ((data[6] & 0x3F) << 8) | data[7];
|
||||||
|
imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* Exported functions ------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 初始化DM IMU设备
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
|
||||||
|
if (imu == NULL || param == NULL) {
|
||||||
|
return DEVICE_ERR_NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 初始化设备头部
|
||||||
|
imu->header.online = false;
|
||||||
|
imu->header.last_online_time = 0;
|
||||||
|
|
||||||
|
// 配置参数
|
||||||
|
imu->param.can = param->can;
|
||||||
|
imu->param.can_id = param->can_id;
|
||||||
|
imu->param.device_id = param->device_id;
|
||||||
|
imu->param.master_id = param->master_id;
|
||||||
|
|
||||||
|
// 清零数据
|
||||||
|
memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
|
||||||
|
|
||||||
|
// 注册CAN接收队列，用于接收回复报文
|
||||||
|
int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
|
||||||
|
if (result != BSP_OK) {
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 请求IMU数据
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
|
||||||
|
if (imu == NULL) {
|
||||||
|
return DEVICE_ERR_NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
|
||||||
|
uint8_t tx_data[4] = {
|
||||||
|
imu->param.device_id & 0xFF, // id_L
|
||||||
|
(imu->param.device_id >> 8) & 0xFF, // id_H
|
||||||
|
(uint8_t)rid, // RID
|
||||||
|
0xCC // 固定值
|
||||||
|
};
|
||||||
|
|
||||||
|
// 发送标准数据帧
|
||||||
|
BSP_CAN_StdDataFrame_t frame = {
|
||||||
|
.id = imu->param.can_id,
|
||||||
|
.dlc = 4,
|
||||||
|
};
|
||||||
|
memcpy(frame.data, tx_data, 4);
|
||||||
|
int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
|
||||||
|
return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_Update(DM_IMU_t *imu) {
|
||||||
|
if (imu == NULL) {
|
||||||
|
return DEVICE_ERR_NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
BSP_CAN_Message_t msg;
|
||||||
|
int8_t result;
|
||||||
|
bool data_received = false;
|
||||||
|
|
||||||
|
// 持续接收所有可用消息
|
||||||
|
while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
|
||||||
|
// 验证回复数据格式（至少检查数据长度）
|
||||||
|
if (msg.dlc < 3) {
|
||||||
|
continue; // 跳过无效消息
|
||||||
|
}
|
||||||
|
|
||||||
|
// 根据数据位的第0位确定反馈报文类型
|
||||||
|
uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
|
||||||
|
|
||||||
|
// 根据RID类型解析数据
|
||||||
|
int8_t parse_result = DEVICE_ERR;
|
||||||
|
switch (rid) {
|
||||||
|
case 0x01: // RID_ACCL
|
||||||
|
parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
|
||||||
|
break;
|
||||||
|
case 0x02: // RID_GYRO
|
||||||
|
parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
|
||||||
|
break;
|
||||||
|
case 0x03: // RID_EULER
|
||||||
|
parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
|
||||||
|
break;
|
||||||
|
case 0x04: // RID_QUATERNION
|
||||||
|
parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
continue; // 跳过未知类型的消息
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果解析成功，标记为收到数据
|
||||||
|
if (parse_result == DEVICE_OK) {
|
||||||
|
data_received = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 如果收到任何有效数据，更新设备状态
|
||||||
|
if (data_received) {
|
||||||
|
imu->header.online = true;
|
||||||
|
imu->header.last_online_time = BSP_TIME_Get_ms();
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
return DEVICE_ERR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
|
||||||
|
if (imu == NULL) {
|
||||||
|
return DEVICE_ERR_NULL;
|
||||||
|
}
|
||||||
|
switch (count) {
|
||||||
|
case 0:
|
||||||
|
DM_IMU_Request(imu, RID_ACCL);
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
DM_IMU_Request(imu, RID_GYRO);
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
DM_IMU_Request(imu, RID_EULER);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
DM_IMU_Request(imu, RID_QUATERNION);
|
||||||
|
DM_IMU_Update(imu); // 更新所有数据
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
count++;
|
||||||
|
if (count >= 4) {
|
||||||
|
count = 0; // 重置计数器
|
||||||
|
}
|
||||||
|
return DEVICE_OK;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 检查设备是否在线
|
||||||
|
*/
|
||||||
|
bool DM_IMU_IsOnline(DM_IMU_t *imu) {
|
||||||
|
if (imu == NULL) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t current_time = BSP_TIME_Get_ms();
|
||||||
|
return imu->header.online &&
|
||||||
|
(current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
|
||||||
|
}
|
||||||
90
User/device/dm_imu.h
Normal file
90
User/device/dm_imu.h
Normal file
@ -0,0 +1,90 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
extern "C" {
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "device/device.h"
|
||||||
|
#include "component/ahrs.h"
|
||||||
|
#include "bsp/can.h"
|
||||||
|
/* Exported constants ------------------------------------------------------- */
|
||||||
|
|
||||||
|
#define DM_IMU_CAN_ID_DEFAULT 0x6FF
|
||||||
|
#define DM_IMU_ID_DEFAULT 0x42
|
||||||
|
#define DM_IMU_MST_ID_DEFAULT 0x43
|
||||||
|
|
||||||
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
|
/* Exported types ----------------------------------------------------------- */
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
BSP_CAN_t can; // CAN总线句柄
|
||||||
|
uint16_t can_id; // CAN通信ID
|
||||||
|
uint8_t device_id; // 设备ID
|
||||||
|
uint8_t master_id; // 主机ID
|
||||||
|
} DM_IMU_Param_t;
|
||||||
|
typedef enum {
|
||||||
|
RID_ACCL = 0x01, // 加速度计
|
||||||
|
RID_GYRO = 0x02, // 陀螺仪
|
||||||
|
RID_EULER = 0x03, // 欧拉角
|
||||||
|
RID_QUATERNION = 0x04, // 四元数
|
||||||
|
} DM_IMU_RID_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
AHRS_Accl_t accl; // 加速度计
|
||||||
|
AHRS_Gyro_t gyro; // 陀螺仪
|
||||||
|
AHRS_Eulr_t euler; // 欧拉角
|
||||||
|
AHRS_Quaternion_t quat; // 四元数
|
||||||
|
float temp; // 温度
|
||||||
|
} DM_IMU_Data_t;
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
DEVICE_Header_t header;
|
||||||
|
DM_IMU_Param_t param; // IMU参数配置
|
||||||
|
DM_IMU_Data_t data; // IMU数据
|
||||||
|
} DM_IMU_t;
|
||||||
|
|
||||||
|
/* Exported functions prototypes -------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 初始化DM IMU设备
|
||||||
|
* @param imu DM IMU设备结构体指针
|
||||||
|
* @param param IMU参数配置指针
|
||||||
|
* @return DEVICE_OK 成功，其他值失败
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 请求IMU数据
|
||||||
|
* @param imu DM IMU设备结构体指针
|
||||||
|
* @param rid 请求的数据类型
|
||||||
|
* @return DEVICE_OK 成功，其他值失败
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
|
||||||
|
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 更新IMU数据（从CAN中获取所有数据并解析）
|
||||||
|
* @param imu DM IMU设备结构体指针
|
||||||
|
* @return DEVICE_OK 成功，其他值失败
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_Update(DM_IMU_t *imu);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
|
||||||
|
* @param imu DM IMU设备结构体指针
|
||||||
|
* @return DEVICE_OK 成功，其他值失败
|
||||||
|
*/
|
||||||
|
int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief 检查设备是否在线
|
||||||
|
* @param imu DM IMU设备结构体指针
|
||||||
|
* @return true 在线，false 离线
|
||||||
|
*/
|
||||||
|
bool DM_IMU_IsOnline(DM_IMU_t *imu);
|
||||||
|
|
||||||
|
|
||||||
|
#ifdef __cplusplus
|
||||||
|
}
|
||||||
|
#endif
|
||||||
@ -243,7 +243,6 @@ int8_t MOTOR_LZ_Init(void) {
|
|||||||
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
|
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
|
||||||
if (param == NULL) return DEVICE_ERR_NULL;
|
if (param == NULL) return DEVICE_ERR_NULL;
|
||||||
|
|
||||||
|
|||||||
@ -139,13 +139,13 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
|
|||||||
motor->feedback.rotor_speed = rotor_speed;
|
motor->feedback.rotor_speed = rotor_speed;
|
||||||
motor->feedback.torque_current = torque_current;
|
motor->feedback.torque_current = torque_current;
|
||||||
}
|
}
|
||||||
|
if (motor->motor.reverse) {
|
||||||
while (motor->feedback.rotor_abs_angle < 0) {
|
while (motor->feedback.rotor_abs_angle < 0) {
|
||||||
motor->feedback.rotor_abs_angle += M_2PI;
|
motor->feedback.rotor_abs_angle += M_2PI;
|
||||||
}
|
}
|
||||||
while (motor->feedback.rotor_abs_angle >= M_2PI) {
|
while (motor->feedback.rotor_abs_angle >= M_2PI) {
|
||||||
motor->feedback.rotor_abs_angle -= M_2PI;
|
motor->feedback.rotor_abs_angle -= M_2PI;
|
||||||
}
|
}
|
||||||
if (motor->motor.reverse) {
|
|
||||||
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
|
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
|
||||||
motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
|
motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
|
||||||
motor->feedback.torque_current = -motor->feedback.torque_current;
|
motor->feedback.torque_current = -motor->feedback.torque_current;
|
||||||
|
|||||||
@ -114,24 +114,6 @@ static int8_t Virtual_Chassis_DecodeWheelMotor(Virtual_Chassis_t *chassis, uint1
|
|||||||
return DEVICE_OK;
|
return DEVICE_OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* 数据范围定义 - 与发送端一致 */
|
|
||||||
#define ACCEL_CAN_MAX (58.8f)
|
|
||||||
#define ACCEL_CAN_MIN (-58.8f)
|
|
||||||
#define GYRO_CAN_MAX (34.88f)
|
|
||||||
#define GYRO_CAN_MIN (-34.88f)
|
|
||||||
#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */
|
|
||||||
#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */
|
|
||||||
#define ROLL_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
|
|
||||||
#define ROLL_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
|
|
||||||
#define YAW_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */
|
|
||||||
#define YAW_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */
|
|
||||||
#define QUATERNION_MIN (-1.0f)
|
|
||||||
#define QUATERNION_MAX (1.0f)
|
|
||||||
|
|
||||||
/* 反向映射宏：将16位整数还原为浮点值 */
|
|
||||||
#define UNMAP_FROM_INT16(int_val, min, max) \
|
|
||||||
(((float)(int_val) + 32768.0f) / 65535.0f * ((max) - (min)) + (min))
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 解析IMU数据
|
* @brief 解析IMU数据
|
||||||
* @param chassis 虚拟底盘结构体
|
* @param chassis 虚拟底盘结构体
|
||||||
@ -145,58 +127,103 @@ static int8_t Virtual_Chassis_DecodeIMU(Virtual_Chassis_t *chassis, uint16_t id,
|
|||||||
}
|
}
|
||||||
|
|
||||||
if (id == chassis->param.imu.accl_id) {
|
if (id == chassis->param.imu.accl_id) {
|
||||||
// 加速度计数据 - 每轴使用16位编码 (6字节总共)
|
// 加速度计数据 - x/y轴24位，z轴16位
|
||||||
int16_t ax_int, ay_int, az_int;
|
// x: 24位有符号整数 (字节0-2) - 精度1/1000000
|
||||||
|
int32_t x_int = 0;
|
||||||
|
x_int |= ((int32_t)data[0]) << 16;
|
||||||
|
x_int |= ((int32_t)data[1]) << 8;
|
||||||
|
x_int |= ((int32_t)data[2]);
|
||||||
|
// 符号扩展（24位转32位）
|
||||||
|
if (x_int & 0x800000) {
|
||||||
|
x_int |= 0xFF000000;
|
||||||
|
}
|
||||||
|
chassis->data.imu.accl.x = (float)x_int / 1000000.0f;
|
||||||
|
|
||||||
memcpy(&ax_int, &data[0], sizeof(int16_t));
|
// y: 24位有符号整数 (字节3-5) - 精度1/1000000
|
||||||
memcpy(&ay_int, &data[2], sizeof(int16_t));
|
int32_t y_int = 0;
|
||||||
memcpy(&az_int, &data[4], sizeof(int16_t));
|
y_int |= ((int32_t)data[3]) << 16;
|
||||||
|
y_int |= ((int32_t)data[4]) << 8;
|
||||||
|
y_int |= ((int32_t)data[5]);
|
||||||
|
// 符号扩展（24位转32位）
|
||||||
|
if (y_int & 0x800000) {
|
||||||
|
y_int |= 0xFF000000;
|
||||||
|
}
|
||||||
|
chassis->data.imu.accl.y = (float)y_int / 1000000.0f;
|
||||||
|
|
||||||
// 反向映射到原始浮点值
|
// z: 16位有符号整数 (字节6-7) - 精度1/10000
|
||||||
chassis->data.imu.accl.x = UNMAP_FROM_INT16(ax_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
int16_t z_int;
|
||||||
chassis->data.imu.accl.y = UNMAP_FROM_INT16(ay_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
memcpy(&z_int, &data[6], sizeof(int16_t));
|
||||||
chassis->data.imu.accl.z = UNMAP_FROM_INT16(az_int, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
|
chassis->data.imu.accl.z = (float)z_int / 10000.0f;
|
||||||
}
|
}
|
||||||
else if (id == chassis->param.imu.gyro_id) {
|
else if (id == chassis->param.imu.gyro_id) {
|
||||||
// 陀螺仪数据 - 每轴使用16位编码 (6字节总共)
|
// 陀螺仪数据 - x/y轴24位，z轴16位
|
||||||
int16_t gx_int, gy_int, gz_int;
|
// x: 24位有符号整数 (字节0-2) - 精度0.001 rad/s (不再转换单位)
|
||||||
|
int32_t x_int = 0;
|
||||||
|
x_int |= ((int32_t)data[0]) << 16;
|
||||||
|
x_int |= ((int32_t)data[1]) << 8;
|
||||||
|
x_int |= ((int32_t)data[2]);
|
||||||
|
// 符号扩展（24位转32位）
|
||||||
|
if (x_int & 0x800000) {
|
||||||
|
x_int |= 0xFF000000;
|
||||||
|
}
|
||||||
|
chassis->data.imu.gyro.x = (float)x_int / 1000.0f; // 直接除以1000，保持rad/s
|
||||||
|
|
||||||
memcpy(&gx_int, &data[0], sizeof(int16_t));
|
// y: 24位有符号整数 (字节3-5) - 精度0.001 rad/s (不再转换单位)
|
||||||
memcpy(&gy_int, &data[2], sizeof(int16_t));
|
int32_t y_int = 0;
|
||||||
memcpy(&gz_int, &data[4], sizeof(int16_t));
|
y_int |= ((int32_t)data[3]) << 16;
|
||||||
|
y_int |= ((int32_t)data[4]) << 8;
|
||||||
|
y_int |= ((int32_t)data[5]);
|
||||||
|
// 符号扩展（24位转32位）
|
||||||
|
if (y_int & 0x800000) {
|
||||||
|
y_int |= 0xFF000000;
|
||||||
|
}
|
||||||
|
chassis->data.imu.gyro.y = (float)y_int / 1000.0f; // 直接除以1000，保持rad/s
|
||||||
|
|
||||||
// 反向映射到原始浮点值
|
// z: 16位有符号整数 (字节6-7) - 精度0.01 rad/s (不再转换单位)
|
||||||
chassis->data.imu.gyro.x = UNMAP_FROM_INT16(gx_int, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
int16_t z_int;
|
||||||
chassis->data.imu.gyro.y = UNMAP_FROM_INT16(gy_int, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
memcpy(&z_int, &data[6], sizeof(int16_t));
|
||||||
chassis->data.imu.gyro.z = UNMAP_FROM_INT16(gz_int, GYRO_CAN_MIN, GYRO_CAN_MAX);
|
chassis->data.imu.gyro.z = (float)z_int / 100.0f; // 直接除以100，保持rad/s
|
||||||
}
|
}
|
||||||
else if (id == chassis->param.imu.euler_id) {
|
else if (id == chassis->param.imu.euler_id) {
|
||||||
// 欧拉角数据 - 每角使用16位编码 (6字节总共)
|
// 欧拉角数据 - yaw/pitch轴24位，roll轴16位
|
||||||
int16_t yaw_int, pitch_int, roll_int;
|
// yaw: 24位有符号整数 (字节0-2) - 精度0.0001 rad (不再转换单位)
|
||||||
|
int32_t yaw_int = 0;
|
||||||
|
yaw_int |= ((int32_t)data[0]) << 16;
|
||||||
|
yaw_int |= ((int32_t)data[1]) << 8;
|
||||||
|
yaw_int |= ((int32_t)data[2]);
|
||||||
|
// 符号扩展（24位转32位）
|
||||||
|
if (yaw_int & 0x800000) {
|
||||||
|
yaw_int |= 0xFF000000;
|
||||||
|
}
|
||||||
|
chassis->data.imu.euler.yaw = (float)yaw_int / 10000.0f; // 直接除以10000，保持rad
|
||||||
|
|
||||||
memcpy(&yaw_int, &data[0], sizeof(int16_t));
|
// pitch: 24位有符号整数 (字节3-5) - 精度0.0001 rad (不再转换单位)
|
||||||
memcpy(&pitch_int, &data[2], sizeof(int16_t));
|
int32_t pit_int = 0;
|
||||||
memcpy(&roll_int, &data[4], sizeof(int16_t));
|
pit_int |= ((int32_t)data[3]) << 16;
|
||||||
|
pit_int |= ((int32_t)data[4]) << 8;
|
||||||
|
pit_int |= ((int32_t)data[5]);
|
||||||
|
// 符号扩展（24位转32位）
|
||||||
|
if (pit_int & 0x800000) {
|
||||||
|
pit_int |= 0xFF000000;
|
||||||
|
}
|
||||||
|
chassis->data.imu.euler.pit = (float)pit_int / 10000.0f; // 直接除以10000，保持rad
|
||||||
|
|
||||||
// 反向映射到原始浮点值
|
// roll: 16位有符号整数 (字节6-7) - 精度0.001 rad (不再转换单位)
|
||||||
chassis->data.imu.euler.yaw = UNMAP_FROM_INT16(yaw_int, YAW_CAN_MIN, YAW_CAN_MAX);
|
int16_t rol_int;
|
||||||
chassis->data.imu.euler.pit = UNMAP_FROM_INT16(pitch_int, PITCH_CAN_MIN, PITCH_CAN_MAX);
|
memcpy(&rol_int, &data[6], sizeof(int16_t));
|
||||||
chassis->data.imu.euler.rol = UNMAP_FROM_INT16(roll_int, ROLL_CAN_MIN, ROLL_CAN_MAX);
|
chassis->data.imu.euler.rol = (float)rol_int / 1000.0f; // 直接除以1000，保持rad
|
||||||
}
|
}
|
||||||
else if (id == chassis->param.imu.quat_id) {
|
else if (id == chassis->param.imu.quat_id) {
|
||||||
// 四元数数据 - 每个分量使用16位编码 (8字节总共)
|
// 四元数数据 - q0/q1/q2/q3 各2字节，精度1/32767
|
||||||
int16_t q0_int, q1_int, q2_int, q3_int;
|
int16_t q0, q1, q2, q3;
|
||||||
|
memcpy(&q0, &data[0], 2);
|
||||||
memcpy(&q0_int, &data[0], sizeof(int16_t));
|
memcpy(&q1, &data[2], 2);
|
||||||
memcpy(&q1_int, &data[2], sizeof(int16_t));
|
memcpy(&q2, &data[4], 2);
|
||||||
memcpy(&q2_int, &data[4], sizeof(int16_t));
|
memcpy(&q3, &data[6], 2);
|
||||||
memcpy(&q3_int, &data[6], sizeof(int16_t));
|
chassis->data.imu.quat.q0 = q0 / 32767.0f;
|
||||||
|
chassis->data.imu.quat.q1 = q1 / 32767.0f;
|
||||||
// 反向映射到原始浮点值
|
chassis->data.imu.quat.q2 = q2 / 32767.0f;
|
||||||
chassis->data.imu.quat.q0 = UNMAP_FROM_INT16(q0_int, QUATERNION_MIN, QUATERNION_MAX);
|
chassis->data.imu.quat.q3 = q3 / 32767.0f;
|
||||||
chassis->data.imu.quat.q1 = UNMAP_FROM_INT16(q1_int, QUATERNION_MIN, QUATERNION_MAX);
|
|
||||||
chassis->data.imu.quat.q2 = UNMAP_FROM_INT16(q2_int, QUATERNION_MIN, QUATERNION_MAX);
|
|
||||||
chassis->data.imu.quat.q3 = UNMAP_FROM_INT16(q3_int, QUATERNION_MIN, QUATERNION_MAX);
|
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
return DEVICE_ERR;
|
return DEVICE_ERR;
|
||||||
@ -381,8 +408,6 @@ int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_o
|
|||||||
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
|
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
|
||||||
return DEVICE_ERR;
|
return DEVICE_ERR;
|
||||||
}
|
}
|
||||||
|
|
||||||
return DEVICE_OK;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -1,15 +1,16 @@
|
|||||||
#include "module/balance_chassis.h"
|
#include "device/motor_lz.h"
|
||||||
#include "bsp/can.h"
|
#include "bsp/can.h"
|
||||||
#include "bsp/time.h"
|
#include "bsp/time.h"
|
||||||
#include "component/kinematics.h"
|
#include "component/kinematics.h"
|
||||||
#include "component/limiter.h"
|
#include "component/limiter.h"
|
||||||
#include "component/user_math.h"
|
#include "component/user_math.h"
|
||||||
#include "device/motor_lz.h"
|
|
||||||
#include "device/virtual_chassis.h"
|
#include "device/virtual_chassis.h"
|
||||||
|
#include "module/balance_chassis.h"
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
|
|
||||||
|
|
||||||
static Virtual_Chassis_t virtual_chassis;
|
static Virtual_Chassis_t virtual_chassis;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
@ -164,6 +165,13 @@ int8_t Chassis_UpdateFeedback(Chassis_t *c) {
|
|||||||
c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
|
c->feedback.imu.gyro = virtual_chassis.data.imu.gyro;
|
||||||
c->feedback.imu.euler = virtual_chassis.data.imu.euler;
|
c->feedback.imu.euler = virtual_chassis.data.imu.euler;
|
||||||
|
|
||||||
|
/* 更新云台电机反馈数据（yaw轴） */
|
||||||
|
MOTOR_DM_Update(&(c->param->yaw_motor));
|
||||||
|
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(c->param->yaw_motor));
|
||||||
|
if (dm_motor != NULL) {
|
||||||
|
c->feedback.yaw = dm_motor->motor.feedback;
|
||||||
|
}
|
||||||
|
|
||||||
// 更新机体状态估计
|
// 更新机体状态估计
|
||||||
Chassis_UpdateChassisState(c);
|
Chassis_UpdateChassisState(c);
|
||||||
|
|
||||||
@ -209,24 +217,23 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
case CHASSIS_MODE_RECOVER: {
|
case CHASSIS_MODE_RECOVER: {
|
||||||
float fn, tp;
|
float fn;
|
||||||
fn = -20.0f;
|
fn = -25.0f;
|
||||||
tp = 0.0f;
|
|
||||||
|
|
||||||
Chassis_LQRControl(c, c_cmd);
|
VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
|
||||||
|
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0],
|
||||||
VMC_InverseSolve(&c->vmc_[0], fn, tp);
|
&c->output.joint[1]);
|
||||||
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0], &c->output.joint[1]);
|
VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
|
||||||
VMC_InverseSolve(&c->vmc_[1], fn, tp);
|
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3],
|
||||||
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3], &c->output.joint[2]);
|
&c->output.joint[2]);
|
||||||
// Chassis_MotorEnable(c);
|
// Chassis_MotorEnable(c);
|
||||||
c->output.wheel[0] = c_cmd->move_vec.vx * 0.2f;
|
c->output.wheel[0] = 0.0f;
|
||||||
|
c->output.wheel[1] = 0.0f;
|
||||||
c->output.wheel[1] = c_cmd->move_vec.vx * 0.2f;
|
|
||||||
Chassis_Output(c); // 统一输出
|
Chassis_Output(c); // 统一输出
|
||||||
|
|
||||||
} break;
|
} break;
|
||||||
|
|
||||||
|
|
||||||
case CHASSIS_MODE_WHELL_LEG_BALANCE:
|
case CHASSIS_MODE_WHELL_LEG_BALANCE:
|
||||||
// 执行LQR控制（包含PID腿长控制）
|
// 执行LQR控制（包含PID腿长控制）
|
||||||
Chassis_LQRControl(c, c_cmd);
|
Chassis_LQRControl(c, c_cmd);
|
||||||
@ -254,16 +261,14 @@ void Chassis_Output(Chassis_t *c) {
|
|||||||
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
|
// MOTOR_LK_SetOutput(&c->param->wheel_motors[i], c->output.wheel[i]);
|
||||||
// // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
|
// // MOTOR_LK_SetOutput(&c->param->wheel_motors[i], 0.0f);
|
||||||
// }
|
// }
|
||||||
float out[4] = {c->output.joint[0], c->output.joint[1], c->output.joint[2],
|
float out[4] = {
|
||||||
c->output.joint[3]};
|
c->output.joint[0], c->output.joint[1], c->output.joint[2], c->output.joint[3]};
|
||||||
// out[0] = 0.0f;
|
// out[0] = 0.0f;
|
||||||
// out[1] = 0.0f;
|
// out[1] = 0.0f;
|
||||||
// out[2] = 0.0f;
|
// out[2] = 0.0f;
|
||||||
// out[3] = 0.0f;
|
// out[3] = 0.0f;
|
||||||
Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
|
Virtual_Chassis_SendJointTorque(&virtual_chassis, out);
|
||||||
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0],
|
Virtual_Chassis_SendWheelCommand(&virtual_chassis, c->output.wheel[0], c->output.wheel[1]);
|
||||||
c->output.wheel[1]);
|
|
||||||
// Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.0f, 0.0f);
|
|
||||||
// for (int i = 0; i < 2; i++) {
|
// for (int i = 0; i < 2; i++) {
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -272,6 +277,7 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
return CHASSIS_ERR_NULL;
|
return CHASSIS_ERR_NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
/* 运动参数（参考C++版本的状态估计） */
|
/* 运动参数（参考C++版本的状态估计） */
|
||||||
static float xhat = 0.0f, x_dot_hat = 0.0f;
|
static float xhat = 0.0f, x_dot_hat = 0.0f;
|
||||||
float target_dot_x = 0.0f;
|
float target_dot_x = 0.0f;
|
||||||
@ -322,19 +328,18 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
LQR_Control(&c->lqr[0]);
|
LQR_Control(&c->lqr[0]);
|
||||||
LQR_Control(&c->lqr[1]);
|
LQR_Control(&c->lqr[1]);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle;
|
||||||
c->yaw_control.target_yaw = c->param->mech_zero_yaw;
|
c->yaw_control.target_yaw = c->param->mech_zero_yaw;
|
||||||
c->yaw_control.yaw_force =
|
c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
|
||||||
PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw,
|
|
||||||
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
|
c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt);
|
||||||
|
|
||||||
/* 轮毂力矩输出（参考C++版本的减速比） */
|
/* 轮毂力矩输出（参考C++版本的减速比） */
|
||||||
c->output.wheel[0] =
|
// c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
|
||||||
c->lqr[0].control_output.T / 4.5f + c->yaw_control.yaw_force;
|
// c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
|
||||||
c->output.wheel[1] =
|
c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f;
|
||||||
c->lqr[1].control_output.T / 4.5f - c->yaw_control.yaw_force;
|
c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f;
|
||||||
// c->output.wheel[0] = c->lqr[0].control_output.T / 4.5f;
|
|
||||||
// c->output.wheel[1] = c->lqr[1].control_output.T / 4.5f;
|
|
||||||
/* 腿长控制和VMC逆解算（使用PID控制） */
|
/* 腿长控制和VMC逆解算（使用PID控制） */
|
||||||
float virtual_force[2];
|
float virtual_force[2];
|
||||||
float target_L0[2];
|
float target_L0[2];
|
||||||
@ -342,15 +347,14 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
|
|
||||||
/* 横滚角PID补偿计算 */
|
/* 横滚角PID补偿计算 */
|
||||||
// 使用PID控制器计算横滚角补偿力，目标横滚角为0
|
// 使用PID控制器计算横滚角补偿力，目标横滚角为0
|
||||||
float roll_compensation_force =
|
float roll_compensation_force = PID_Calc(&c->pid.roll, 0.0f,
|
||||||
PID_Calc(&c->pid.roll, 0.0f, c->feedback.imu.euler.rol,
|
c->feedback.imu.euler.rol,
|
||||||
c->feedback.imu.gyro.x, c->dt);
|
c->feedback.imu.gyro.x,
|
||||||
|
c->dt);
|
||||||
|
|
||||||
// 限制补偿力范围，防止过度补偿
|
// 限制补偿力范围，防止过度补偿
|
||||||
if (roll_compensation_force > 20.0f)
|
if (roll_compensation_force > 20.0f) roll_compensation_force = 20.0f;
|
||||||
roll_compensation_force = 20.0f;
|
if (roll_compensation_force < -20.0f) roll_compensation_force = -20.0f;
|
||||||
if (roll_compensation_force < -20.0f)
|
|
||||||
roll_compensation_force = -20.0f;
|
|
||||||
|
|
||||||
// 目标腿长设定（移除横滚角补偿）
|
// 目标腿长设定（移除横滚角补偿）
|
||||||
target_L0[0] = 0.12f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
|
target_L0[0] = 0.12f + c_cmd->height * 0.2f; // 左腿：基础腿长 + 高度调节
|
||||||
@ -365,22 +369,20 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
// 使用tp_pid进行左右腿摆角同步控制
|
// 使用tp_pid进行左右腿摆角同步控制
|
||||||
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
|
// 左腿补偿力矩：使左腿theta向右腿theta靠拢
|
||||||
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
|
Delta_Tp[0] = c->vmc_[0].leg.L0 * 10.0f *
|
||||||
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta,
|
PID_Calc(&c->pid.tp[0], c->vmc_[1].leg.theta, c->vmc_[0].leg.theta,
|
||||||
c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
|
c->vmc_[0].leg.d_theta, c->dt);
|
||||||
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
|
// 右腿补偿力矩：使右腿theta向左腿theta靠拢（符号相反）
|
||||||
Delta_Tp[1] = -Delta_Tp[0];
|
Delta_Tp[1] = -Delta_Tp[0];
|
||||||
|
|
||||||
|
|
||||||
// 左腿控制
|
// 左腿控制
|
||||||
{
|
{
|
||||||
// 使用PID进行腿长控制
|
// 使用PID进行腿长控制
|
||||||
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
|
float pid_output = PID_Calc(&c->pid.leg_length[0], target_L0[0],
|
||||||
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
|
c->vmc_[0].leg.L0, leg_d_length[0], c->dt);
|
||||||
|
|
||||||
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 -
|
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 - 横滚角补偿力（左腿减少支撑力）
|
||||||
// 横滚角补偿力（左腿减少支撑力）
|
virtual_force[0] = (c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) + pid_output + 20.0f ;
|
||||||
virtual_force[0] =
|
|
||||||
(c->lqr[0].control_output.Tp) * sinf(c->vmc_[0].leg.theta) +
|
|
||||||
pid_output + 40.0f;
|
|
||||||
|
|
||||||
// VMC逆解算（包含摆角补偿）
|
// VMC逆解算（包含摆角补偿）
|
||||||
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
|
if (VMC_InverseSolve(&c->vmc_[0], virtual_force[0],
|
||||||
@ -389,8 +391,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
&c->output.joint[1]);
|
&c->output.joint[1]);
|
||||||
} else {
|
} else {
|
||||||
// VMC失败，设为0力矩
|
// VMC失败，设为0力矩
|
||||||
c->output.joint[0] = 0.0f;
|
c->output.joint[0]= 0.0f;
|
||||||
c->output.joint[1] = 0.0f;
|
c->output.joint[1]= 0.0f;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -400,11 +402,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
|
float pid_output = PID_Calc(&c->pid.leg_length[1], target_L0[1],
|
||||||
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
|
c->vmc_[1].leg.L0, leg_d_length[1], c->dt);
|
||||||
|
|
||||||
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 +
|
// 腿长控制力 = LQR摆杆力矩的径向分量 + PID腿长控制输出 + 基础支撑力 + 横滚角补偿力（右腿增加支撑力）
|
||||||
// 横滚角补偿力（右腿增加支撑力）
|
virtual_force[1] = (c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) + pid_output + 20.0f ;
|
||||||
virtual_force[1] =
|
|
||||||
(c->lqr[1].control_output.Tp) * sinf(c->vmc_[1].leg.theta) +
|
|
||||||
pid_output + 40.0f;
|
|
||||||
|
|
||||||
// VMC逆解算（包含摆角补偿）
|
// VMC逆解算（包含摆角补偿）
|
||||||
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
|
if (VMC_InverseSolve(&c->vmc_[1], virtual_force[1],
|
||||||
@ -427,7 +426,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
|
|||||||
|
|
||||||
for (int i = 0; i < 4; i++) {
|
for (int i = 0; i < 4; i++) {
|
||||||
if (fabsf(c->output.joint[i]) > 20.0f) {
|
if (fabsf(c->output.joint[i]) > 20.0f) {
|
||||||
c->output.joint[i] = (c->output.joint[i] > 0) ? 20.0f : -20.0f;
|
c->output.joint[i] =
|
||||||
|
(c->output.joint[i] > 0) ? 20.0f : -20.0f;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -17,153 +17,12 @@
|
|||||||
|
|
||||||
// 机器人参数配置
|
// 机器人参数配置
|
||||||
Config_RobotParam_t robot_config = {
|
Config_RobotParam_t robot_config = {
|
||||||
.shoot_param = {
|
|
||||||
.trig_step_angle=M_2PI/8,
|
.imu_param = {
|
||||||
.shot_delay_time=0.05f,
|
|
||||||
.shot_burst_num=1,
|
|
||||||
.fric_motor_param[0] = {
|
|
||||||
.can = BSP_CAN_2,
|
.can = BSP_CAN_2,
|
||||||
.id = 0x201,
|
.can_id = 0x6FF,
|
||||||
.module = MOTOR_M3508,
|
.device_id = 0x42,
|
||||||
.reverse = true,
|
.master_id = 0x43,
|
||||||
.gear=false,
|
|
||||||
},
|
|
||||||
.fric_motor_param[1] = {
|
|
||||||
.can = BSP_CAN_2,
|
|
||||||
.id = 0x202,
|
|
||||||
.module = MOTOR_M3508,
|
|
||||||
.reverse = false,
|
|
||||||
.gear=false,
|
|
||||||
},
|
|
||||||
.trig_motor_param = {
|
|
||||||
.can = BSP_CAN_1,
|
|
||||||
.id = 0x201,
|
|
||||||
.module = MOTOR_M2006,
|
|
||||||
.reverse = true,
|
|
||||||
.gear=true,
|
|
||||||
},
|
|
||||||
.fric_follow = {
|
|
||||||
.k=1.0f,
|
|
||||||
.p=1.8f,
|
|
||||||
.i=0.0f,
|
|
||||||
.d=0.0f,
|
|
||||||
.i_limit=0.0f,
|
|
||||||
.out_limit=0.9f,
|
|
||||||
.d_cutoff_freq=30.0f,
|
|
||||||
.range=-1.0f,
|
|
||||||
},
|
|
||||||
|
|
||||||
.fric_err = {
|
|
||||||
.k=1.0f,
|
|
||||||
.p=4.0f,
|
|
||||||
.i=0.4f,
|
|
||||||
.d=0.04f,
|
|
||||||
.i_limit=0.25f,
|
|
||||||
.out_limit=0.25f,
|
|
||||||
.d_cutoff_freq=40.0f,
|
|
||||||
.range=-1.0f,
|
|
||||||
},
|
|
||||||
.trig_omg = {
|
|
||||||
.k=1.0f,
|
|
||||||
.p=1.5f,
|
|
||||||
.i=0.3f,
|
|
||||||
.d=0.5f,
|
|
||||||
.i_limit=0.2f,
|
|
||||||
.out_limit=0.9f,
|
|
||||||
.d_cutoff_freq=-1.0f,
|
|
||||||
.range=-1.0f,
|
|
||||||
},
|
|
||||||
.trig = {
|
|
||||||
.k=1.0f,
|
|
||||||
.p=1.0f,
|
|
||||||
.i=0.1f,
|
|
||||||
.d=0.05f,
|
|
||||||
.i_limit=0.8f,
|
|
||||||
.out_limit=0.5f,
|
|
||||||
.d_cutoff_freq=-1.0f,
|
|
||||||
.range=M_2PI,
|
|
||||||
},
|
|
||||||
.filter.fric = {
|
|
||||||
.in = 30.0f,
|
|
||||||
.out = 30.0f,
|
|
||||||
},
|
|
||||||
.filter.trig = {
|
|
||||||
.in = 30.0f,
|
|
||||||
.out = 30.0f,
|
|
||||||
},
|
|
||||||
|
|
||||||
|
|
||||||
},
|
|
||||||
|
|
||||||
.gimbal_param = {
|
|
||||||
.pid = {
|
|
||||||
.yaw_omega = {
|
|
||||||
.k = 1.0f,
|
|
||||||
.p = 1.0f,
|
|
||||||
.i = 0.5f,
|
|
||||||
.d = 0.0f,
|
|
||||||
.i_limit = 1.0f,
|
|
||||||
.out_limit = 1.0f,
|
|
||||||
.d_cutoff_freq = -1.0f,
|
|
||||||
.range = -1.0f,
|
|
||||||
},
|
|
||||||
.yaw_angle = {
|
|
||||||
.k = 10.0f,
|
|
||||||
.p = 3.0f,
|
|
||||||
.i = 0.0f,
|
|
||||||
.d = 0.0f,
|
|
||||||
.i_limit = 0.0f,
|
|
||||||
.out_limit = 10.0f,
|
|
||||||
.d_cutoff_freq = -1.0f,
|
|
||||||
.range = M_2PI,
|
|
||||||
},
|
|
||||||
.pit_omega = {
|
|
||||||
.k = 0.25f,
|
|
||||||
.p = 1.0f,
|
|
||||||
.i = 0.0f,
|
|
||||||
.d = 0.0f,
|
|
||||||
.i_limit = 1.0f,
|
|
||||||
.out_limit = 1.0f,
|
|
||||||
.d_cutoff_freq = -1.0f,
|
|
||||||
.range = -1.0f,
|
|
||||||
},
|
|
||||||
.pit_angle = {
|
|
||||||
.k = 5.0f,
|
|
||||||
.p = 5.0f,
|
|
||||||
.i = 2.5f,
|
|
||||||
.d = 0.0f,
|
|
||||||
.i_limit = 0.0f,
|
|
||||||
.out_limit = 10.0f,
|
|
||||||
.d_cutoff_freq = -1.0f,
|
|
||||||
.range = M_2PI,
|
|
||||||
},
|
|
||||||
},
|
|
||||||
.mech_zero = {
|
|
||||||
.yaw = 0.0f,
|
|
||||||
.pit = 1.77f,
|
|
||||||
},
|
|
||||||
.travel = {
|
|
||||||
.yaw = -1.0f,
|
|
||||||
.pit = 0.8f,
|
|
||||||
},
|
|
||||||
.low_pass_cutoff_freq = {
|
|
||||||
.out = -1.0f,
|
|
||||||
.gyro = 1000.0f,
|
|
||||||
},
|
|
||||||
.pit_motor ={
|
|
||||||
.can = BSP_CAN_2,
|
|
||||||
.id = 0x206,
|
|
||||||
.gear = false,
|
|
||||||
.module = MOTOR_GM6020,
|
|
||||||
.reverse = true,
|
|
||||||
},
|
|
||||||
.yaw_motor = {
|
|
||||||
.can = BSP_CAN_1,
|
|
||||||
.can_id = 0x1,
|
|
||||||
.master_id = 0x11,
|
|
||||||
.module = MOTOR_DM_J4310,
|
|
||||||
.reverse = false,
|
|
||||||
}
|
|
||||||
},
|
},
|
||||||
|
|
||||||
.chassis_param = {
|
.chassis_param = {
|
||||||
@ -254,20 +113,19 @@ Config_RobotParam_t robot_config = {
|
|||||||
}
|
}
|
||||||
},
|
},
|
||||||
.lqr_gains = {
|
.lqr_gains = {
|
||||||
.k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
|
.k11_coeff = { -1.971875171647865e+02f, 2.333085109680034e+02f, -1.246153590631013e+02f, -1.174312882115276e+00f }, // theta
|
||||||
.k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f }, // d_theta
|
.k12_coeff = { 1.013831636839851e+01f, -9.401172931240684e+00f, -7.093019361028483e+00f, -1.965070661986029e-01f }, // d_theta
|
||||||
.k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f }, // x
|
.k13_coeff = { -8.071626496047382e+01f, 8.488840381053994e+01f, -3.185529183755884e+01f, 8.124359207371032e-01f }, // x
|
||||||
.k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f }, // d_x
|
.k14_coeff = { -9.146471594077464e+01f, 9.719345618895781e+01f, -3.842805305561099e+01f, 7.255654659220748e-01f }, // d_x
|
||||||
.k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
|
.k15_coeff = { 6.971426904628133e+00f, 1.184154915473761e+00f, -5.491870448330107e+00f, 1.440694962464955e+00f }, // phi
|
||||||
.k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f }, // d_phi
|
.k16_coeff = { -1.435148366342453e+00f, 3.514683607111185e+00f, -2.799267599341074e+00f, 7.265018312372623e-01f }, // d_phi
|
||||||
.k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
|
.k21_coeff = { 2.190542850560747e+02f, -1.777611537012371e+02f, 2.829510877414540e+01f, 1.179295840423852e+01f }, // theta
|
||||||
.k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f }, // d_theta
|
.k22_coeff = { 1.499600560463937e+01f, -1.645293844666804e+01f, 6.159388841822305e+00f, 1.123989134168646e+00f }, // d_theta
|
||||||
.k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f }, // x
|
.k23_coeff = { -2.101291454047308e+00f, 2.194459164295155e+01f, -2.170715123911771e+01f, 7.721879713384771e+00f }, // x
|
||||||
.k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f }, // d_x
|
.k24_coeff = { -8.999340091522836e+00f, 3.152322448910924e+01f, -2.733558805289098e+01f, 9.654200348790059e+00f }, // d_x
|
||||||
.k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
|
.k25_coeff = { 2.294462014541718e+01f, -2.777789794229430e+01f, 1.160794075992378e+01f, 2.826138680231716e+00f }, // phi
|
||||||
.k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f }, // d_phi
|
.k26_coeff = { 1.624214174273862e+01f, -1.723094946447953e+01f, 6.402650924792812e+00f, 8.755827436652930e-02f }, // d_phi
|
||||||
},
|
},
|
||||||
|
|
||||||
.virtual_chassis_param = {
|
.virtual_chassis_param = {
|
||||||
.motors = {
|
.motors = {
|
||||||
.can = BSP_CAN_1,
|
.can = BSP_CAN_1,
|
||||||
|
|||||||
@ -9,17 +9,15 @@ extern "C" {
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
#include "device/dm_imu.h"
|
||||||
#include "device/motor_lz.h"
|
#include "device/motor_lz.h"
|
||||||
#include "device/motor_lk.h"
|
#include "device/motor_lk.h"
|
||||||
#include "module/balance_chassis.h"
|
#include "module/balance_chassis.h"
|
||||||
#include "module/gimbal.h"
|
|
||||||
#include "module/shoot.h"
|
|
||||||
#include "device/virtual_chassis.h"
|
#include "device/virtual_chassis.h"
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
Shoot_Params_t shoot_param;
|
DM_IMU_Param_t imu_param;
|
||||||
Chassis_Params_t chassis_param;
|
Chassis_Params_t chassis_param;
|
||||||
Gimbal_Params_t gimbal_param;
|
|
||||||
Virtual_Chassis_Param_t virtual_chassis_param; // 虚拟底盘参数
|
Virtual_Chassis_Param_t virtual_chassis_param; // 虚拟底盘参数
|
||||||
} Config_RobotParam_t;
|
} Config_RobotParam_t;
|
||||||
|
|
||||||
|
|||||||
@ -1,246 +0,0 @@
|
|||||||
/*
|
|
||||||
* 云台模组
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "gimbal.h"
|
|
||||||
#include "bsp/can.h"
|
|
||||||
#include "bsp/time.h"
|
|
||||||
#include <math.h>
|
|
||||||
#include "component/filter.h"
|
|
||||||
#include "component/pid.h"
|
|
||||||
#include "device/motor_dm.h"
|
|
||||||
#include "device/motor_rm.h"
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
|
||||||
/* Private define ----------------------------------------------------------- */
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
|
||||||
/* Private variables -------------------------------------------------------- */
|
|
||||||
/* Private function -------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 设置云台模式
|
|
||||||
*
|
|
||||||
* \param c 包含云台数据的结构体
|
|
||||||
* \param mode 要设置的模式
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
|
|
||||||
if (g == NULL)
|
|
||||||
return -1;
|
|
||||||
if (mode == g->mode)
|
|
||||||
return GIMBAL_OK;
|
|
||||||
|
|
||||||
PID_Reset(&g->pid.yaw_angle);
|
|
||||||
PID_Reset(&g->pid.yaw_omega);
|
|
||||||
PID_Reset(&g->pid.pit_angle);
|
|
||||||
PID_Reset(&g->pid.pit_omega);
|
|
||||||
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
|
|
||||||
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
|
|
||||||
|
|
||||||
MOTOR_DM_Enable(&(g->param->yaw_motor));
|
|
||||||
|
|
||||||
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
|
|
||||||
// if (g->mode == GIMBAL_MODE_RELAX) {
|
|
||||||
// if (mode == GIMBAL_MODE_ABSOLUTE) {
|
|
||||||
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
|
||||||
// } else if (mode == GIMBAL_MODE_RELATIVE) {
|
|
||||||
// g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
|
||||||
// }
|
|
||||||
// }
|
|
||||||
g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
|
|
||||||
g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
|
|
||||||
|
|
||||||
g->mode = mode;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 初始化云台
|
|
||||||
*
|
|
||||||
* \param g 包含云台数据的结构体
|
|
||||||
* \param param 包含云台参数的结构体指针
|
|
||||||
* \param target_freq 任务预期的运行频率
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
|
|
||||||
float target_freq) {
|
|
||||||
if (g == NULL)
|
|
||||||
return -1;
|
|
||||||
|
|
||||||
g->param = param;
|
|
||||||
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
|
|
||||||
|
|
||||||
/* 初始化云台电机控制PID和LPF */
|
|
||||||
PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
|
|
||||||
&(g->param->pid.yaw_angle));
|
|
||||||
PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
|
|
||||||
&(g->param->pid.yaw_omega));
|
|
||||||
PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq,
|
|
||||||
&(g->param->pid.pit_angle));
|
|
||||||
PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
|
|
||||||
&(g->param->pid.pit_omega));
|
|
||||||
|
|
||||||
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
|
|
||||||
g->param->low_pass_cutoff_freq.out);
|
|
||||||
LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
|
|
||||||
g->param->low_pass_cutoff_freq.out);
|
|
||||||
g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
|
|
||||||
g->limit.yaw.min = g->param->mech_zero.yaw;
|
|
||||||
g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
|
|
||||||
g->limit.pit.min = g->param->mech_zero.pit;
|
|
||||||
BSP_CAN_Init();
|
|
||||||
|
|
||||||
MOTOR_RM_Register(&(g->param->pit_motor));
|
|
||||||
MOTOR_DM_Register(&(g->param->yaw_motor));
|
|
||||||
|
|
||||||
MOTOR_DM_Enable(&(g->param->yaw_motor));
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 通过CAN设备更新云台反馈信息
|
|
||||||
*
|
|
||||||
* \param gimbal 云台
|
|
||||||
* \param can CAN设备
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
|
|
||||||
if (gimbal == NULL)
|
|
||||||
return -1;
|
|
||||||
|
|
||||||
/* 更新RM电机反馈数据（pitch轴） */
|
|
||||||
MOTOR_RM_Update(&(gimbal->param->pit_motor));
|
|
||||||
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
|
|
||||||
if (rm_motor != NULL) {
|
|
||||||
gimbal->feedback.motor.pit = rm_motor->feedback;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* 更新DM电机反馈数据（yaw轴） */
|
|
||||||
MOTOR_DM_Update(&(gimbal->param->yaw_motor));
|
|
||||||
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
|
|
||||||
if (dm_motor != NULL) {
|
|
||||||
gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
|
|
||||||
}
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
|
|
||||||
|
|
||||||
if (gimbal == NULL) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
gimbal->feedback.imu.gyro = imu->gyro;
|
|
||||||
gimbal->feedback.imu.eulr = imu->eulr;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 运行云台控制逻辑
|
|
||||||
*
|
|
||||||
* \param g 包含云台数据的结构体
|
|
||||||
* \param g_cmd 云台控制指令
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
|
|
||||||
if (g == NULL || g_cmd == NULL) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
|
|
||||||
g->lask_wakeup = BSP_TIME_Get_us();
|
|
||||||
|
|
||||||
Gimbal_SetMode(g, g_cmd->mode);
|
|
||||||
|
|
||||||
/* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
|
|
||||||
float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
|
|
||||||
if (g->param->travel.yaw > 0) {
|
|
||||||
/* 计算当前电机角度与IMU角度的偏差 */
|
|
||||||
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
|
|
||||||
/* 处理跨越±π的情况 */
|
|
||||||
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
|
|
||||||
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
|
|
||||||
|
|
||||||
/* 计算到限位边界的距离 */
|
|
||||||
const float delta_max = CircleError(g->limit.yaw.max,
|
|
||||||
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
|
|
||||||
const float delta_min = CircleError(g->limit.yaw.min,
|
|
||||||
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
|
|
||||||
|
|
||||||
/* 限制控制命令 */
|
|
||||||
if (delta_yaw > delta_max) delta_yaw = delta_max;
|
|
||||||
if (delta_yaw < delta_min) delta_yaw = delta_min;
|
|
||||||
}
|
|
||||||
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
|
|
||||||
|
|
||||||
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
|
|
||||||
float delta_pit = g_cmd->delta_pit * g->dt;
|
|
||||||
if (g->param->travel.pit > 0) {
|
|
||||||
/* 计算当前电机角度与IMU角度的偏差 */
|
|
||||||
float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
|
|
||||||
/* 处理跨越±π的情况 */
|
|
||||||
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
|
|
||||||
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
|
|
||||||
|
|
||||||
/* 计算到限位边界的距离 */
|
|
||||||
const float delta_max = CircleError(g->limit.pit.max,
|
|
||||||
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
|
|
||||||
const float delta_min = CircleError(g->limit.pit.min,
|
|
||||||
(g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
|
|
||||||
|
|
||||||
/* 限制控制命令 */
|
|
||||||
if (delta_pit > delta_max) delta_pit = delta_max;
|
|
||||||
if (delta_pit < delta_min) delta_pit = delta_min;
|
|
||||||
}
|
|
||||||
CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
|
|
||||||
|
|
||||||
/* 控制相关逻辑 */
|
|
||||||
float yaw_omega_set_point, pit_omega_set_point;
|
|
||||||
switch (g->mode) {
|
|
||||||
case GIMBAL_MODE_RELAX:
|
|
||||||
g->out.yaw = 0.0f;
|
|
||||||
g->out.pit = 0.0f;
|
|
||||||
break;
|
|
||||||
|
|
||||||
case GIMBAL_MODE_ABSOLUTE:
|
|
||||||
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
|
|
||||||
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
|
|
||||||
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
|
|
||||||
g->feedback.imu.gyro.z, 0.f, g->dt);
|
|
||||||
|
|
||||||
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
|
|
||||||
g->feedback.imu.eulr.rol, 0.0f, g->dt);
|
|
||||||
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
|
|
||||||
g->feedback.imu.gyro.y, 0.f, g->dt);
|
|
||||||
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
/* 输出滤波 */
|
|
||||||
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
|
|
||||||
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 云台输出
|
|
||||||
*
|
|
||||||
* \param s 包含云台数据的结构体
|
|
||||||
* \param out CAN设备云台输出结构体
|
|
||||||
*/
|
|
||||||
void Gimbal_Output(Gimbal_t *g){
|
|
||||||
MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
|
|
||||||
MOTOR_MIT_Output_t output = {0};
|
|
||||||
output.torque = g->out.yaw * 5.0f; // 乘以减速比
|
|
||||||
output.kd = 0.5f;
|
|
||||||
MOTOR_RM_Ctrl(&g->param->pit_motor);
|
|
||||||
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
|
|
||||||
}
|
|
||||||
@ -1,180 +0,0 @@
|
|||||||
/*
|
|
||||||
* 云台模组
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
extern "C" {
|
|
||||||
#endif
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "component/ahrs.h"
|
|
||||||
#include "component/filter.h"
|
|
||||||
#include "component/pid.h"
|
|
||||||
#include "device/motor.h"
|
|
||||||
#include "device/motor_dm.h"
|
|
||||||
#include "device/motor_rm.h"
|
|
||||||
|
|
||||||
/* Exported constants ------------------------------------------------------- */
|
|
||||||
#define GIMBAL_OK (0) /* 运行正常 */
|
|
||||||
#define GIMBAL_ERR (-1) /* 运行时发现了其他错误 */
|
|
||||||
#define GIMBAL_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
|
||||||
#define GIMBAL_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */
|
|
||||||
|
|
||||||
/* Exported macro ----------------------------------------------------------- */
|
|
||||||
/* Exported types ----------------------------------------------------------- */
|
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
|
|
||||||
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
|
|
||||||
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
|
|
||||||
} Gimbal_Mode_t;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
Gimbal_Mode_t mode;
|
|
||||||
float delta_yaw;
|
|
||||||
float delta_pit;
|
|
||||||
} Gimbal_CMD_t;
|
|
||||||
|
|
||||||
/* 软件限位 */
|
|
||||||
typedef struct {
|
|
||||||
float max;
|
|
||||||
float min;
|
|
||||||
} Gimbal_Limit_t;
|
|
||||||
|
|
||||||
/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
|
|
||||||
typedef struct {
|
|
||||||
MOTOR_RM_Param_t pit_motor; /* pitch轴电机参数 */
|
|
||||||
MOTOR_DM_Param_t yaw_motor; /* yaw轴电机参数 */
|
|
||||||
struct {
|
|
||||||
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
|
|
||||||
KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
|
|
||||||
KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
|
|
||||||
KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
|
|
||||||
} pid;
|
|
||||||
|
|
||||||
/* 低通滤波器截止频率 */
|
|
||||||
struct {
|
|
||||||
float out; /* 电机输出 */
|
|
||||||
float gyro; /* 陀螺仪数据 */
|
|
||||||
} low_pass_cutoff_freq;
|
|
||||||
|
|
||||||
struct {
|
|
||||||
float yaw; /* yaw轴机械限位 */
|
|
||||||
float pit; /* pitch轴机械限位 */
|
|
||||||
} mech_zero;
|
|
||||||
|
|
||||||
struct {
|
|
||||||
float yaw; /* yaw轴机械限位行程 -1表示无限位 */
|
|
||||||
float pit; /* pitch轴机械限位行程 -1表示无限位*/
|
|
||||||
} travel;
|
|
||||||
|
|
||||||
} Gimbal_Params_t;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
AHRS_Gyro_t gyro;
|
|
||||||
AHRS_Eulr_t eulr;
|
|
||||||
} Gimbal_IMU_t;
|
|
||||||
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
|
|
||||||
typedef struct {
|
|
||||||
Gimbal_IMU_t imu;
|
|
||||||
struct {
|
|
||||||
MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
|
|
||||||
MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
|
|
||||||
} motor;
|
|
||||||
} Gimbal_Feedback_t;
|
|
||||||
|
|
||||||
/* 云台输出数据的结构体*/
|
|
||||||
typedef struct {
|
|
||||||
float yaw; /* yaw轴电机输出 */
|
|
||||||
float pit; /* pitch轴电机输出 */
|
|
||||||
} Gimbal_Output_t;
|
|
||||||
/*
|
|
||||||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体。
|
|
||||||
* 包含了初始化参数，中间变量，输出变量。
|
|
||||||
*/
|
|
||||||
typedef struct {
|
|
||||||
uint64_t lask_wakeup;
|
|
||||||
float dt;
|
|
||||||
|
|
||||||
Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
|
|
||||||
|
|
||||||
/* 模块通用 */
|
|
||||||
Gimbal_Mode_t mode; /* 云台模式 */
|
|
||||||
|
|
||||||
/* PID计算的目标值 */
|
|
||||||
struct {
|
|
||||||
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
|
|
||||||
} setpoint;
|
|
||||||
|
|
||||||
struct {
|
|
||||||
KPID_t yaw_angle; /* yaw轴角位置环PID */
|
|
||||||
KPID_t yaw_omega; /* yaw轴角速度环PID */
|
|
||||||
KPID_t pit_angle; /* pitch轴角位置环PID */
|
|
||||||
KPID_t pit_omega; /* pitch轴角速度环PID */
|
|
||||||
} pid;
|
|
||||||
|
|
||||||
struct {
|
|
||||||
Gimbal_Limit_t yaw;
|
|
||||||
Gimbal_Limit_t pit;
|
|
||||||
} limit;
|
|
||||||
|
|
||||||
struct {
|
|
||||||
LowPassFilter2p_t yaw;
|
|
||||||
LowPassFilter2p_t pit;
|
|
||||||
} filter_out;
|
|
||||||
|
|
||||||
Gimbal_Output_t out; /* 云台输出 */
|
|
||||||
Gimbal_Feedback_t feedback; /* 反馈 */
|
|
||||||
|
|
||||||
} Gimbal_t;
|
|
||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 初始化云台
|
|
||||||
*
|
|
||||||
* \param g 包含云台数据的结构体
|
|
||||||
* \param param 包含云台参数的结构体指针
|
|
||||||
* \param target_freq 任务预期的运行频率
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
|
|
||||||
float target_freq);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 通过CAN设备更新云台反馈信息
|
|
||||||
*
|
|
||||||
* \param gimbal 云台
|
|
||||||
* \param can CAN设备
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal);
|
|
||||||
|
|
||||||
int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu);
|
|
||||||
/**
|
|
||||||
* \brief 运行云台控制逻辑
|
|
||||||
*
|
|
||||||
* \param g 包含云台数据的结构体
|
|
||||||
* \param fb 云台反馈信息
|
|
||||||
* \param g_cmd 云台控制指令
|
|
||||||
* \param dt_sec 两次调用的时间间隔
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 云台输出
|
|
||||||
*
|
|
||||||
* \param s 包含云台数据的结构体
|
|
||||||
* \param out CAN设备云台输出结构体
|
|
||||||
*/
|
|
||||||
void Gimbal_Output(Gimbal_t *g);
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
@ -1,305 +0,0 @@
|
|||||||
|
|
||||||
#include "shoot.h"
|
|
||||||
#include <string.h>
|
|
||||||
#include "can.h"
|
|
||||||
#include "component/filter.h"
|
|
||||||
#include "component/user_math.h"
|
|
||||||
#include <math.h>
|
|
||||||
#include "bsp/time.h"
|
|
||||||
|
|
||||||
static bool last_firecmd;
|
|
||||||
|
|
||||||
static inline void ScaleSumTo1(float *a, float *b) {
|
|
||||||
float sum = *a + *b;
|
|
||||||
if (sum > 1.0f) {
|
|
||||||
float scale = 1.0f / sum;
|
|
||||||
*a *= scale;
|
|
||||||
*b *= scale;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
|
|
||||||
{
|
|
||||||
if (s == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
s->mode=mode;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_ResetIntegral(Shoot_t *s)
|
|
||||||
{
|
|
||||||
if (s == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{
|
|
||||||
PID_ResetIntegral(&s->pid.fric_follow[i]);
|
|
||||||
PID_ResetIntegral(&s->pid.fric_err[i]);
|
|
||||||
}
|
|
||||||
PID_ResetIntegral(&s->pid.trig);
|
|
||||||
PID_ResetIntegral(&s->pid.trig_omg);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_ResetCalu(Shoot_t *s)
|
|
||||||
{
|
|
||||||
if (s == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{
|
|
||||||
PID_Reset(&s->pid.fric_follow[i]);
|
|
||||||
PID_Reset(&s->pid.fric_err[i]);
|
|
||||||
LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
|
|
||||||
LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
|
|
||||||
}
|
|
||||||
PID_Reset(&s->pid.trig);
|
|
||||||
PID_Reset(&s->pid.trig_omg);
|
|
||||||
LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
|
|
||||||
LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_ResetOutput(Shoot_t *s)
|
|
||||||
{
|
|
||||||
if (s == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{
|
|
||||||
s->output.out_follow[i]=0.0f;
|
|
||||||
s->output.out_err[i]=0.0f;
|
|
||||||
s->output.out_fric[i]=0.0f;
|
|
||||||
s->output.lpfout_fric[i]=0.0f;
|
|
||||||
}
|
|
||||||
s->output.outagl_trig=0.0f;
|
|
||||||
s->output.outomg_trig=0.0f;
|
|
||||||
s->output.outlpf_trig=0.0f;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
|
|
||||||
{
|
|
||||||
if (s == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
s->target_variable.target_rpm=4000.0f/MAX_FRIC_RPM;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
/**
|
|
||||||
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
|
|
||||||
*
|
|
||||||
* \param s 包含发射数据的结构体
|
|
||||||
* \param num 需要发射的弹丸数量
|
|
||||||
*/
|
|
||||||
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
|
|
||||||
{
|
|
||||||
if (s == NULL || s->shoot_Anglecalu.num_to_shoot == 0) {
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
if(s->now - s->shoot_Anglecalu.time_last_shoot >= s->param->shot_delay_time && cmd->firecmd)
|
|
||||||
{
|
|
||||||
s->shoot_Anglecalu.time_last_shoot=s->now;
|
|
||||||
s->target_variable.target_angle += s->param->trig_step_angle;
|
|
||||||
if(s->target_variable.target_angle>M_PI)s->target_variable.target_angle-=M_2PI;
|
|
||||||
else if((s->target_variable.target_angle<-M_PI))s->target_variable.target_angle+=M_2PI;
|
|
||||||
s->shoot_Anglecalu.num_to_shoot--;
|
|
||||||
}
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
|
|
||||||
{
|
|
||||||
if (s == NULL || param == NULL || target_freq <= 0.0f) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
s->param=param;
|
|
||||||
|
|
||||||
BSP_CAN_Init();
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++){
|
|
||||||
MOTOR_RM_Register(¶m->fric_motor_param[i]);
|
|
||||||
PID_Init(&s->pid.fric_follow[i], KPID_MODE_CALC_D, target_freq,¶m->fric_follow);
|
|
||||||
LowPassFilter2p_Init(&s->filter.fric.in[i], target_freq, s->param->filter.fric.in);
|
|
||||||
LowPassFilter2p_Init(&s->filter.fric.out[i], target_freq, s->param->filter.fric.out);
|
|
||||||
}
|
|
||||||
MOTOR_RM_Register(¶m->trig_motor_param);
|
|
||||||
PID_Init(&s->pid.trig, KPID_MODE_CALC_D, target_freq,¶m->trig);
|
|
||||||
PID_Init(&s->pid.trig_omg, KPID_MODE_CALC_D, target_freq,¶m->trig_omg);
|
|
||||||
|
|
||||||
LowPassFilter2p_Init(&s->filter.trig.in, target_freq, s->param->filter.trig.in);
|
|
||||||
LowPassFilter2p_Init(&s->filter.trig.out, target_freq, s->param->filter.trig.out);
|
|
||||||
|
|
||||||
memset(&s->shoot_Anglecalu,0,sizeof(s->shoot_Anglecalu));
|
|
||||||
memset(&s->output,0,sizeof(s->output));
|
|
||||||
s->target_variable.target_rpm=4000.0f;
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_UpdateFeedback(Shoot_t *s)
|
|
||||||
{
|
|
||||||
if (s == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
float rpm_sum=0.0f;
|
|
||||||
for(int i = 0; i < SHOOT_FRIC_NUM; i++) {
|
|
||||||
/* 更新摩擦电机反馈 */
|
|
||||||
MOTOR_RM_Update(&s->param->fric_motor_param[i]);
|
|
||||||
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->fric_motor_param[i]);
|
|
||||||
if(motor_fed!=NULL)
|
|
||||||
{
|
|
||||||
s->feedback.fric[i]=motor_fed->motor.feedback;
|
|
||||||
}
|
|
||||||
/* 滤波反馈rpm */
|
|
||||||
s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
|
|
||||||
/* 归一化rpm */
|
|
||||||
s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
|
|
||||||
if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f;
|
|
||||||
if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f;
|
|
||||||
/* 计算平均rpm */
|
|
||||||
rpm_sum+=s->feedback.fric_rpm[i];
|
|
||||||
}
|
|
||||||
s->feedback.fric_avgrpm=rpm_sum/SHOOT_FRIC_NUM;
|
|
||||||
/* 更新拨弹电机反馈 */
|
|
||||||
MOTOR_RM_Update(&s->param->trig_motor_param);
|
|
||||||
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->trig_motor_param);
|
|
||||||
if(motor_fed!=NULL)
|
|
||||||
{
|
|
||||||
s->feedback.trig=motor_fed->feedback;
|
|
||||||
}
|
|
||||||
/* 将多圈角度归化到单圈 (-M_PI, M_PI) */
|
|
||||||
s->feedback.trig_angle_cicle = s->feedback.trig.rotor_abs_angle;
|
|
||||||
s->feedback.trig_angle_cicle = fmod(s->feedback.trig_angle_cicle, M_2PI); // 将角度限制在 [-2π, 2π]
|
|
||||||
if (s->feedback.trig_angle_cicle > M_PI) {
|
|
||||||
s->feedback.trig_angle_cicle -= M_2PI; // 调整到 [-π, π]
|
|
||||||
}else if (s->feedback.trig_angle_cicle < -M_PI) {
|
|
||||||
s->feedback.trig_angle_cicle += M_2PI; // 调整到 [-π, π]
|
|
||||||
}
|
|
||||||
|
|
||||||
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.rotor_speed);
|
|
||||||
s->feedback.trig_rpm = s->feedback.trig.rotor_speed / MAX_TRIG_RPM;
|
|
||||||
if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f; //如果单环效果好就删
|
|
||||||
if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
|
|
||||||
|
|
||||||
s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
|
|
||||||
{
|
|
||||||
if (s == NULL || cmd == NULL) {
|
|
||||||
return -1; // 参数错误
|
|
||||||
}
|
|
||||||
s->now = BSP_TIME_Get_us() / 1000000.0f;
|
|
||||||
s->dt = (BSP_TIME_Get_us() - s->lask_wakeup) / 1000000.0f;
|
|
||||||
s->lask_wakeup = BSP_TIME_Get_us();
|
|
||||||
s->online = cmd->online;
|
|
||||||
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{
|
|
||||||
MOTOR_RM_Relax(&s->param->fric_motor_param[i]);
|
|
||||||
}
|
|
||||||
MOTOR_RM_Relax(&s->param->trig_motor_param);
|
|
||||||
}
|
|
||||||
else{
|
|
||||||
switch(s->running_state)
|
|
||||||
{
|
|
||||||
case SHOOT_STATE_IDLE:/*熄火等待*/
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{
|
|
||||||
PID_ResetIntegral(&s->pid.fric_follow[i]);
|
|
||||||
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->dt);
|
|
||||||
s->output.out_fric[i]=s->output.out_follow[i];
|
|
||||||
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
|
||||||
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
|
|
||||||
}
|
|
||||||
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
|
|
||||||
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
|
|
||||||
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
|
||||||
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
|
|
||||||
if(cmd->ready)
|
|
||||||
{
|
|
||||||
Shoot_ResetCalu(s);
|
|
||||||
Shoot_ResetIntegral(s);
|
|
||||||
Shoot_ResetOutput(s);
|
|
||||||
s->running_state=SHOOT_STATE_READY;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case SHOOT_STATE_READY:/*准备射击*/
|
|
||||||
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{ /* 计算跟随输出、计算修正输出 */
|
|
||||||
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
|
|
||||||
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
|
|
||||||
/* 按比例缩放并加和输出 */
|
|
||||||
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
|
|
||||||
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
|
|
||||||
/* 滤波 */
|
|
||||||
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
|
||||||
/* 输出 */
|
|
||||||
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
|
|
||||||
}
|
|
||||||
/* 拨弹电机输出 */
|
|
||||||
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
|
|
||||||
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
|
|
||||||
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
|
||||||
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
|
|
||||||
|
|
||||||
/* 检查状态机 */
|
|
||||||
if(!cmd->ready)
|
|
||||||
{
|
|
||||||
Shoot_ResetCalu(s);
|
|
||||||
Shoot_ResetOutput(s);
|
|
||||||
s->running_state=SHOOT_STATE_IDLE;
|
|
||||||
}
|
|
||||||
else if(last_firecmd==false&&cmd->firecmd==true)
|
|
||||||
{
|
|
||||||
Shoot_ResetCalu(s);
|
|
||||||
Shoot_ResetOutput(s);
|
|
||||||
s->running_state=SHOOT_STATE_FIRE;
|
|
||||||
s->shoot_Anglecalu.num_to_shoot+=s->param->shot_burst_num;
|
|
||||||
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
case SHOOT_STATE_FIRE:
|
|
||||||
Shoot_CaluTargetAngle(s, cmd);
|
|
||||||
for(int i=0;i<SHOOT_FRIC_NUM;i++)
|
|
||||||
{
|
|
||||||
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],s->target_variable.target_rpm/MAX_FRIC_RPM,s->feedback.fric_rpm[i],0,s->dt);
|
|
||||||
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],s->feedback.fric_avgrpm,s->feedback.fric_rpm[i],0,s->dt);
|
|
||||||
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
|
|
||||||
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
|
|
||||||
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
|
|
||||||
MOTOR_RM_SetOutput(&s->param->fric_motor_param[i], s->output.lpfout_fric[i]);
|
|
||||||
}
|
|
||||||
s->output.outagl_trig =PID_Calc(&s->pid.trig,s->target_variable.target_angle,s->feedback.trig_angle_cicle,0,s->dt);
|
|
||||||
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->dt);
|
|
||||||
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
|
|
||||||
MOTOR_RM_SetOutput(&s->param->trig_motor_param, s->output.outlpf_trig);
|
|
||||||
if(!cmd->firecmd)
|
|
||||||
{
|
|
||||||
Shoot_ResetCalu(s);
|
|
||||||
Shoot_ResetOutput(s);
|
|
||||||
s->running_state=SHOOT_STATE_READY;
|
|
||||||
}
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
s->running_state=SHOOT_STATE_IDLE;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
MOTOR_RM_Ctrl(&s->param->fric_motor_param[0]);
|
|
||||||
MOTOR_RM_Ctrl(&s->param->trig_motor_param);
|
|
||||||
last_firecmd = cmd->firecmd;
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,200 +0,0 @@
|
|||||||
/*
|
|
||||||
* far蛇模组
|
|
||||||
*/
|
|
||||||
|
|
||||||
#pragma once
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
extern "C" {
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#include "main.h"
|
|
||||||
#include <stdbool.h>
|
|
||||||
#include "component/pid.h"
|
|
||||||
#include "device/motor_rm.h"
|
|
||||||
|
|
||||||
|
|
||||||
/* Exported constants ------------------------------------------------------- */
|
|
||||||
#define SHOOT_OK (0) /* 运行正常 */
|
|
||||||
#define SHOOT_ERR (-1) /* 运行时发现了其他错误 */
|
|
||||||
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
|
|
||||||
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
|
|
||||||
#define SHOOT_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
|
|
||||||
|
|
||||||
#define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
|
|
||||||
#define MAX_FRIC_RPM 7000.0f
|
|
||||||
#define MAX_TRIG_RPM 5000.0f
|
|
||||||
/* Exported macro ----------------------------------------------------------- */
|
|
||||||
/* Exported types ----------------------------------------------------------- */
|
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
SHOOT_STATE_IDLE = 0, // 熄火
|
|
||||||
SHOOT_STATE_READY, // 准备射击
|
|
||||||
SHOOT_STATE_FIRE // 射击
|
|
||||||
} Shoot_State_t;
|
|
||||||
|
|
||||||
typedef enum {
|
|
||||||
SHOOT_MODE_SAFE = 0, // 安全模式
|
|
||||||
SHOOT_MODE_SINGLE, // 单发模式
|
|
||||||
SHOOT_MODE_BURST, // 多发模式
|
|
||||||
SHOOT_MODE_CONTINUE // 连发模式
|
|
||||||
} Shoot_Mode_t;
|
|
||||||
typedef struct {
|
|
||||||
bool online;
|
|
||||||
|
|
||||||
bool ready; /* 准备射击 */
|
|
||||||
bool firecmd; /* 射击指令 */
|
|
||||||
|
|
||||||
} Shoot_CMD_t;
|
|
||||||
typedef struct {
|
|
||||||
MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
|
|
||||||
MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
|
|
||||||
|
|
||||||
float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
|
|
||||||
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
|
|
||||||
|
|
||||||
float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */
|
|
||||||
|
|
||||||
float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
|
|
||||||
float fric_avgrpm; /* 归一化摩擦轮平均转速*/
|
|
||||||
float trig_rpm; /* 归一化拨弹电机转速*/
|
|
||||||
|
|
||||||
}Shoot_Feedback_t;
|
|
||||||
|
|
||||||
typedef struct{
|
|
||||||
float time_last_shoot;
|
|
||||||
uint8_t num_to_shoot;
|
|
||||||
uint8_t num_shooted;
|
|
||||||
}Shoot_AngleCalu_t;
|
|
||||||
|
|
||||||
typedef struct {
|
|
||||||
float out_follow[SHOOT_FRIC_NUM];
|
|
||||||
float out_err[SHOOT_FRIC_NUM];
|
|
||||||
float out_fric[SHOOT_FRIC_NUM];
|
|
||||||
float lpfout_fric[SHOOT_FRIC_NUM];
|
|
||||||
|
|
||||||
|
|
||||||
float outagl_trig;
|
|
||||||
float outomg_trig;
|
|
||||||
float outlpf_trig;
|
|
||||||
}Shoot_Output_t;
|
|
||||||
|
|
||||||
|
|
||||||
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
|
|
||||||
typedef struct {
|
|
||||||
float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
|
|
||||||
float shot_delay_time; /* 射击间隔时间，单位秒 */
|
|
||||||
uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
|
|
||||||
|
|
||||||
MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM];
|
|
||||||
MOTOR_RM_Param_t trig_motor_param;
|
|
||||||
|
|
||||||
|
|
||||||
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
|
|
||||||
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
|
|
||||||
KPID_Params_t trig; /* 拨弹电机PID控制参数 */
|
|
||||||
KPID_Params_t trig_omg; /* 拨弹电机PID控制参数 */
|
|
||||||
|
|
||||||
/* 低通滤波器截止频率 */
|
|
||||||
struct {
|
|
||||||
struct{
|
|
||||||
float in; /* 反馈值滤波器 */
|
|
||||||
float out; /* 输出值滤波器 */
|
|
||||||
}fric;
|
|
||||||
struct{
|
|
||||||
float in; /* 反馈值滤波器 */
|
|
||||||
float out; /* 输出值滤波器 */
|
|
||||||
}trig;
|
|
||||||
} filter;
|
|
||||||
} Shoot_Params_t;
|
|
||||||
|
|
||||||
/*
|
|
||||||
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
|
|
||||||
* 包含了初始化参数，中间变量，输出变量
|
|
||||||
*/
|
|
||||||
typedef struct {
|
|
||||||
bool online;
|
|
||||||
|
|
||||||
float now;
|
|
||||||
uint64_t lask_wakeup;
|
|
||||||
float dt;
|
|
||||||
|
|
||||||
Shoot_Params_t *param; /* */
|
|
||||||
/* 模块通用 */
|
|
||||||
Shoot_State_t running_state; /* 运行状态机 */
|
|
||||||
Shoot_Mode_t mode;
|
|
||||||
/* 反馈信息 */
|
|
||||||
Shoot_Feedback_t feedback;
|
|
||||||
/* 控制信息*/
|
|
||||||
Shoot_AngleCalu_t shoot_Anglecalu;
|
|
||||||
Shoot_Output_t output;
|
|
||||||
/* 目标控制量 */
|
|
||||||
struct {
|
|
||||||
float target_rpm; /* 目标摩擦轮转速 */
|
|
||||||
float target_angle; /* 目标拨弹位置 */
|
|
||||||
}target_variable;
|
|
||||||
|
|
||||||
/* 反馈控制用的PID */
|
|
||||||
struct {
|
|
||||||
KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
|
|
||||||
KPID_t fric_err[SHOOT_FRIC_NUM]; /* */
|
|
||||||
KPID_t trig;
|
|
||||||
KPID_t trig_omg;
|
|
||||||
} pid;
|
|
||||||
|
|
||||||
/* 滤波器 */
|
|
||||||
struct {
|
|
||||||
struct{
|
|
||||||
LowPassFilter2p_t in[SHOOT_FRIC_NUM]; /* 反馈值滤波器 */
|
|
||||||
LowPassFilter2p_t out[SHOOT_FRIC_NUM]; /* 输出值滤波器 */
|
|
||||||
}fric;
|
|
||||||
struct{
|
|
||||||
LowPassFilter2p_t in; /* 反馈值滤波器 */
|
|
||||||
LowPassFilter2p_t out; /* 输出值滤波器 */
|
|
||||||
}trig;
|
|
||||||
} filter;
|
|
||||||
|
|
||||||
float errtosee; /*调试用*/
|
|
||||||
|
|
||||||
} Shoot_t;
|
|
||||||
|
|
||||||
/* Exported functions prototypes -------------------------------------------- */
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 初始化发射
|
|
||||||
*
|
|
||||||
* \param s 包含发射数据的结构体
|
|
||||||
* \param param 包含发射参数的结构体指针
|
|
||||||
* \param target_freq 任务预期的运行频率
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 更新反馈
|
|
||||||
*
|
|
||||||
* \param s 包含发射数据的结构体
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Shoot_UpdateFeedback(Shoot_t *s);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* \brief 初始化发射
|
|
||||||
*
|
|
||||||
* \param s 包含发射数据的结构体
|
|
||||||
* \param cmd 包含发射命令的结构体
|
|
||||||
*
|
|
||||||
* \return 函数运行结果
|
|
||||||
*/
|
|
||||||
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@ -1,44 +0,0 @@
|
|||||||
/*
|
|
||||||
ai Task
|
|
||||||
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "task/user_task.h"
|
|
||||||
/* USER INCLUDE BEGIN */
|
|
||||||
|
|
||||||
/* USER INCLUDE END */
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
|
||||||
/* Private define ----------------------------------------------------------- */
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
|
||||||
/* Private variables -------------------------------------------------------- */
|
|
||||||
/* USER STRUCT BEGIN */
|
|
||||||
|
|
||||||
/* USER STRUCT END */
|
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
|
||||||
void Task_ai(void *argument) {
|
|
||||||
(void)argument; /* 未使用argument，消除警告 */
|
|
||||||
|
|
||||||
|
|
||||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
|
|
||||||
|
|
||||||
osDelay(AI_INIT_DELAY); /* 延时一段时间再开启任务 */
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
|
||||||
/* USER CODE INIT BEGIN */
|
|
||||||
|
|
||||||
/* USER CODE INIT END */
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
|
||||||
/* USER CODE BEGIN */
|
|
||||||
|
|
||||||
/* USER CODE END */
|
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -1,17 +1,15 @@
|
|||||||
/*
|
/*
|
||||||
atti_esti Task
|
atti_esti Task
|
||||||
陀螺仪解算任务
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
#include "cmsis_os2.h"
|
|
||||||
#include "task/user_task.h"
|
#include "task/user_task.h"
|
||||||
/* USER INCLUDE BEGIN */
|
/* USER INCLUDE BEGIN */
|
||||||
#include "bsp/pwm.h"
|
#include "bsp/can.h"
|
||||||
#include "component/ahrs.h"
|
#include "device/dm_imu.h"
|
||||||
#include "component/pid.h"
|
#include "module/config.h"
|
||||||
#include "device/bmi088.h"
|
#include "module/balance_chassis.h"
|
||||||
#include "module/gimbal.h"
|
|
||||||
/* USER INCLUDE END */
|
/* USER INCLUDE END */
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
@ -19,21 +17,8 @@
|
|||||||
/* Private macro ------------------------------------------------------------ */
|
/* Private macro ------------------------------------------------------------ */
|
||||||
/* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
/* USER STRUCT BEGIN */
|
/* USER STRUCT BEGIN */
|
||||||
BMI088_t bmi088;
|
DM_IMU_t dm_imu;
|
||||||
|
Chassis_IMU_t chassis_imu_send;
|
||||||
AHRS_t gimbal_ahrs;
|
|
||||||
AHRS_Magn_t magn;
|
|
||||||
AHRS_Eulr_t eulr_to_send;
|
|
||||||
|
|
||||||
KPID_t imu_temp_ctrl_pid;
|
|
||||||
|
|
||||||
Gimbal_IMU_t gimbal_to_send;
|
|
||||||
|
|
||||||
BMI088_Cali_t cali_bmi088= {
|
|
||||||
.gyro_offset = {0.0f,0.0f,0.0f},
|
|
||||||
};
|
|
||||||
|
|
||||||
|
|
||||||
/* USER STRUCT END */
|
/* USER STRUCT END */
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
/* Private function --------------------------------------------------------- */
|
||||||
@ -42,44 +27,31 @@ void Task_atti_esti(void *argument) {
|
|||||||
(void)argument; /* 未使用argument，消除警告 */
|
(void)argument; /* 未使用argument，消除警告 */
|
||||||
|
|
||||||
|
|
||||||
|
/* 计算任务运行到指定频率需要等待的tick数 */
|
||||||
|
const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
|
||||||
|
|
||||||
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
|
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||||
|
|
||||||
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||||
/* USER CODE INIT BEGIN */
|
/* USER CODE INIT BEGIN */
|
||||||
BMI088_Init(&bmi088,&cali_bmi088);
|
BSP_CAN_Init();
|
||||||
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
|
// 初始化DM IMU设备
|
||||||
|
DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
|
||||||
/* USER CODE INIT END */
|
/* USER CODE INIT END */
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
/* USER CODE BEGIN */
|
/* USER CODE BEGIN */
|
||||||
BMI088_WaitNew();
|
|
||||||
BMI088_AcclStartDmaRecv();
|
|
||||||
BMI088_AcclWaitDmaCplt();
|
|
||||||
|
|
||||||
BMI088_GyroStartDmaRecv();
|
|
||||||
BMI088_GyroWaitDmaCplt();
|
|
||||||
|
|
||||||
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
|
|
||||||
osKernelLock();
|
|
||||||
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
|
|
||||||
BMI088_ParseAccl(&bmi088);
|
|
||||||
BMI088_ParseGyro(&bmi088);
|
|
||||||
// IST8310_Parse(&ist8310);
|
|
||||||
|
|
||||||
/* 根据设备接收到的数据进行姿态解析 */
|
|
||||||
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
|
|
||||||
|
|
||||||
/* 根据解析出来的四元数计算欧拉角 */
|
|
||||||
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
|
|
||||||
osKernelUnlock();
|
|
||||||
|
|
||||||
gimbal_to_send.eulr = eulr_to_send;
|
|
||||||
gimbal_to_send.gyro = bmi088.gyro;
|
|
||||||
|
|
||||||
osMessageQueueReset(task_runtime.msgq.gimbal.imu);
|
|
||||||
osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0);
|
|
||||||
|
|
||||||
|
if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
|
||||||
|
osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
|
||||||
|
chassis_imu_send.accl = dm_imu.data.accl;
|
||||||
|
chassis_imu_send.gyro = dm_imu.data.gyro;
|
||||||
|
chassis_imu_send.euler = dm_imu.data.euler;
|
||||||
|
osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
|
||||||
|
}
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
|
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
@ -33,24 +33,3 @@
|
|||||||
function: Task_ctrl_gimbal
|
function: Task_ctrl_gimbal
|
||||||
name: ctrl_gimbal
|
name: ctrl_gimbal
|
||||||
stack: 256
|
stack: 256
|
||||||
- delay: 0
|
|
||||||
description: ''
|
|
||||||
freq_control: true
|
|
||||||
frequency: 500.0
|
|
||||||
function: Task_ctrl_shoot
|
|
||||||
name: ctrl_shoot
|
|
||||||
stack: 256
|
|
||||||
- delay: 0
|
|
||||||
description: ''
|
|
||||||
freq_control: true
|
|
||||||
frequency: 500.0
|
|
||||||
function: Task_monitor
|
|
||||||
name: monitor
|
|
||||||
stack: 256
|
|
||||||
- delay: 0
|
|
||||||
description: ''
|
|
||||||
freq_control: true
|
|
||||||
frequency: 500.0
|
|
||||||
function: Task_ai
|
|
||||||
name: ai
|
|
||||||
stack: 256
|
|
||||||
|
|||||||
@ -58,7 +58,6 @@ void Task_ctrl_chassis(void *argument) {
|
|||||||
if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) != osOK) {
|
if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) != osOK) {
|
||||||
// 没有新命令，保持上次命令
|
// 没有新命令，保持上次命令
|
||||||
}
|
}
|
||||||
osMessageQueueGet(task_runtime.msgq.chassis_yaw, &chassis.feedback.yaw, NULL, 0);
|
|
||||||
|
|
||||||
Chassis_UpdateFeedback(&chassis);
|
Chassis_UpdateFeedback(&chassis);
|
||||||
|
|
||||||
|
|||||||
@ -4,12 +4,16 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
#include "cmsis_os2.h"
|
#include "device/motor_rm.h"
|
||||||
#include "task/user_task.h"
|
#include "task/user_task.h"
|
||||||
/* USER INCLUDE BEGIN */
|
/* USER INCLUDE BEGIN */
|
||||||
#include "component/ahrs.h"
|
#include "bsp/can.h"
|
||||||
#include "module/gimbal.h"
|
#include "device/motor_dm.h"
|
||||||
#include "module/config.h"
|
#include "device/motor_rm.h"
|
||||||
|
#include "device/motor.h"
|
||||||
|
#include "component/pid.h"
|
||||||
|
#include <stdbool.h>
|
||||||
|
|
||||||
/* USER INCLUDE END */
|
/* USER INCLUDE END */
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
@ -17,9 +21,45 @@
|
|||||||
/* Private macro ------------------------------------------------------------ */
|
/* Private macro ------------------------------------------------------------ */
|
||||||
/* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
/* USER STRUCT BEGIN */
|
/* USER STRUCT BEGIN */
|
||||||
Gimbal_t gimbal;
|
MOTOR_DM_Param_t dm_4310_param = {
|
||||||
Gimbal_IMU_t gimbal_imu;
|
.can = BSP_CAN_1,
|
||||||
Gimbal_CMD_t gimbal_cmd;
|
.master_id = 0x11, // 主站ID
|
||||||
|
.can_id = 0x01, // 反馈ID 10进制是85
|
||||||
|
.module = MOTOR_DM_J4310,
|
||||||
|
.reverse = false,
|
||||||
|
};
|
||||||
|
|
||||||
|
// MOTOR_RM_Param_t m2006 = {
|
||||||
|
// .can = BSP_CAN_1,
|
||||||
|
// .id = 0x201,
|
||||||
|
// .module = MOTOR_M2006,
|
||||||
|
// .reverse = false,
|
||||||
|
// .gear = true,
|
||||||
|
// };
|
||||||
|
|
||||||
|
MOTOR_MIT_Output_t gimbal_output = {
|
||||||
|
.angle = 0.0f,
|
||||||
|
.velocity = 0.0f,
|
||||||
|
.torque = 0.0f,
|
||||||
|
.kp = 0.0f,
|
||||||
|
.kd = 0.0f,
|
||||||
|
};
|
||||||
|
MOTOR_DM_t *dm_4310_motor = NULL;
|
||||||
|
|
||||||
|
// MOTOR_Feedback_t m2006_fb;
|
||||||
|
MOTOR_Feedback_t dm_4310_fb;
|
||||||
|
|
||||||
|
// KPID_Params_t gimbal_angle_pid = {
|
||||||
|
// .leg_theta={
|
||||||
|
// .k=1.0f,
|
||||||
|
// .p=5.0f, /* 摆角比例系数 */
|
||||||
|
// .i=0.0f, /* 摆角积分系数 */
|
||||||
|
// .d=0.0f, /* 摆角微分系数 */
|
||||||
|
// .i_limit=0.0f, /* 积分限幅 */
|
||||||
|
// .out_limit=0.05f, /* 输出限幅，腿长差值限制 */
|
||||||
|
// .d_cutoff_freq=30.0f, /* 微分截止频率 */
|
||||||
|
// .range=-1.0f, /* 不使用循环误差处理 */
|
||||||
|
// };
|
||||||
/* USER STRUCT END */
|
/* USER STRUCT END */
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
/* Private function --------------------------------------------------------- */
|
||||||
@ -35,27 +75,24 @@ void Task_ctrl_gimbal(void *argument) {
|
|||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
||||||
/* USER CODE INIT BEGIN */
|
/* USER CODE INIT BEGIN */
|
||||||
Gimbal_Init(&gimbal, &Config_GetRobotParam()->gimbal_param, CTRL_GIMBAL_FREQ);
|
BSP_CAN_Init();
|
||||||
/* USER CODE INIT END */
|
// MOTOR_DM_Register(&dm_4310_param);
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
||||||
/* USER CODE BEGIN */
|
/* USER CODE BEGIN */
|
||||||
if (osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0) == osOK) {
|
// MOTOR_DM_Update(&dm_4310_param);
|
||||||
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
|
// MOTOR_DM_t *dm_4310_motor = MOTOR_DM_GetMotor(&dm_4310_param);
|
||||||
}
|
// if (dm_4310_motor) {
|
||||||
osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
|
// dm_4310_fb = dm_4310_motor->motor.feedback;
|
||||||
|
// }
|
||||||
|
// if (dm_4310_motor->motor.header.online == false) {
|
||||||
|
// MOTOR_DM_Enable(&dm_4310_param); // 使能电机
|
||||||
|
// } else {
|
||||||
|
// MOTOR_DM_MITCtrl(&dm_4310_param, &gimbal_output);
|
||||||
|
// }
|
||||||
|
|
||||||
|
|
||||||
Gimbal_UpdateFeedback(&gimbal);
|
|
||||||
|
|
||||||
// osMessageQueueReset(task_runtime.msgq.chassis_yaw);
|
|
||||||
osMessageQueuePut(task_runtime.msgq.chassis_yaw, &gimbal.feedback.motor.yaw, 0, 0);
|
|
||||||
|
|
||||||
Gimbal_Control(&gimbal, &gimbal_cmd);
|
|
||||||
|
|
||||||
Gimbal_Output(&gimbal);
|
|
||||||
|
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
||||||
}
|
}
|
||||||
|
|||||||
@ -1,48 +0,0 @@
|
|||||||
/*
|
|
||||||
ctrl_shoot Task
|
|
||||||
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "task/user_task.h"
|
|
||||||
/* USER INCLUDE BEGIN */
|
|
||||||
#include "module/shoot.h"
|
|
||||||
#include "module/config.h"
|
|
||||||
/* USER INCLUDE END */
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
|
||||||
/* Private define ----------------------------------------------------------- */
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
|
||||||
/* Private variables -------------------------------------------------------- */
|
|
||||||
/* USER STRUCT BEGIN */
|
|
||||||
Shoot_t shoot;
|
|
||||||
Shoot_CMD_t shoot_cmd;
|
|
||||||
/* USER STRUCT END */
|
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
|
||||||
void Task_ctrl_shoot(void *argument) {
|
|
||||||
(void)argument; /* 未使用argument，消除警告 */
|
|
||||||
|
|
||||||
|
|
||||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_SHOOT_FREQ;
|
|
||||||
|
|
||||||
osDelay(CTRL_SHOOT_INIT_DELAY); /* 延时一段时间再开启任务 */
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
|
||||||
/* USER CODE INIT BEGIN */
|
|
||||||
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,CTRL_SHOOT_FREQ);
|
|
||||||
/* USER CODE INIT END */
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
|
||||||
/* USER CODE BEGIN */
|
|
||||||
osMessageQueueGet(task_runtime.msgq.shoot.shoot_cmd, &shoot_cmd, NULL, 0);
|
|
||||||
Shoot_UpdateFeedback(&shoot);
|
|
||||||
Shoot_Control(&shoot,&shoot_cmd);
|
|
||||||
/* USER CODE END */
|
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -7,11 +7,10 @@
|
|||||||
#include "task/user_task.h"
|
#include "task/user_task.h"
|
||||||
|
|
||||||
/* USER INCLUDE BEGIN */
|
/* USER INCLUDE BEGIN */
|
||||||
#include "device/motor.h"
|
#include "component/ahrs.h"
|
||||||
|
#include "module/config.h"
|
||||||
#include "module/balance_chassis.h"
|
#include "module/balance_chassis.h"
|
||||||
#include "module/gimbal.h"
|
/* USER INCLUDE BEGIN */
|
||||||
#include "module/shoot.h"
|
|
||||||
/* USER INCLUDE END */
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
/* Private typedef ---------------------------------------------------------- */
|
||||||
/* Private define ----------------------------------------------------------- */
|
/* Private define ----------------------------------------------------------- */
|
||||||
@ -37,20 +36,13 @@ void Task_Init(void *argument) {
|
|||||||
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
|
||||||
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
|
||||||
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
|
||||||
task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
|
// task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
|
||||||
task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
|
|
||||||
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
|
|
||||||
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
|
|
||||||
|
|
||||||
// 创建消息队列
|
// 创建消息队列
|
||||||
/* USER MESSAGE BEGIN */
|
/* USER MESSAGE BEGIN */
|
||||||
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
|
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
|
||||||
task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
|
task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
|
||||||
task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
|
task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
|
||||||
task_runtime.msgq.chassis_yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
|
|
||||||
task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
|
|
||||||
task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
|
|
||||||
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
|
|
||||||
/* USER MESSAGE END */
|
/* USER MESSAGE END */
|
||||||
|
|
||||||
osKernelUnlock(); // 解锁内核
|
osKernelUnlock(); // 解锁内核
|
||||||
|
|||||||
@ -1,44 +0,0 @@
|
|||||||
/*
|
|
||||||
monitor Task
|
|
||||||
|
|
||||||
*/
|
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
|
||||||
#include "task/user_task.h"
|
|
||||||
/* USER INCLUDE BEGIN */
|
|
||||||
|
|
||||||
/* USER INCLUDE END */
|
|
||||||
|
|
||||||
/* Private typedef ---------------------------------------------------------- */
|
|
||||||
/* Private define ----------------------------------------------------------- */
|
|
||||||
/* Private macro ------------------------------------------------------------ */
|
|
||||||
/* Private variables -------------------------------------------------------- */
|
|
||||||
/* USER STRUCT BEGIN */
|
|
||||||
|
|
||||||
/* USER STRUCT END */
|
|
||||||
|
|
||||||
/* Private function --------------------------------------------------------- */
|
|
||||||
/* Exported functions ------------------------------------------------------- */
|
|
||||||
void Task_monitor(void *argument) {
|
|
||||||
(void)argument; /* 未使用argument，消除警告 */
|
|
||||||
|
|
||||||
|
|
||||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / MONITOR_FREQ;
|
|
||||||
|
|
||||||
osDelay(MONITOR_INIT_DELAY); /* 延时一段时间再开启任务 */
|
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
|
||||||
/* USER CODE INIT BEGIN */
|
|
||||||
|
|
||||||
/* USER CODE INIT END */
|
|
||||||
|
|
||||||
while (1) {
|
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
|
||||||
/* USER CODE BEGIN */
|
|
||||||
|
|
||||||
/* USER CODE END */
|
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
@ -4,13 +4,14 @@
|
|||||||
*/
|
*/
|
||||||
|
|
||||||
/* Includes ----------------------------------------------------------------- */
|
/* Includes ----------------------------------------------------------------- */
|
||||||
|
#include "cmsis_os2.h"
|
||||||
#include "task/user_task.h"
|
#include "task/user_task.h"
|
||||||
/* USER INCLUDE BEGIN */
|
/* USER INCLUDE BEGIN */
|
||||||
#include "device/dr16.h"
|
#include "device/dr16.h"
|
||||||
#include "module/config.h"
|
#include "module/config.h"
|
||||||
#include "module/balance_chassis.h"
|
#include "module/balance_chassis.h"
|
||||||
#include "module/gimbal.h"
|
// #include "module/shoot.h"
|
||||||
#include "module/shoot.h"
|
// #include "module/gimbal.h"
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
// #include
|
// #include
|
||||||
/* USER INCLUDE END */
|
/* USER INCLUDE END */
|
||||||
@ -21,9 +22,7 @@
|
|||||||
/* Private variables -------------------------------------------------------- */
|
/* Private variables -------------------------------------------------------- */
|
||||||
/* USER STRUCT BEGIN */
|
/* USER STRUCT BEGIN */
|
||||||
DR16_t dr16;
|
DR16_t dr16;
|
||||||
Shoot_CMD_t for_shoot;
|
|
||||||
Chassis_CMD_t cmd_for_chassis;
|
Chassis_CMD_t cmd_for_chassis;
|
||||||
Gimbal_CMD_t cmd_for_gimbal;
|
|
||||||
// RC_CAN_t rc_can;
|
// RC_CAN_t rc_can;
|
||||||
// Shoot_CMD_t for_shoot;
|
// Shoot_CMD_t for_shoot;
|
||||||
// Gimbal_CMD_t cmd_for_gimbal;
|
// Gimbal_CMD_t cmd_for_gimbal;
|
||||||
@ -34,19 +33,13 @@ Gimbal_CMD_t cmd_for_gimbal;
|
|||||||
void Task_rc(void *argument) {
|
void Task_rc(void *argument) {
|
||||||
(void)argument; /* 未使用argument，消除警告 */
|
(void)argument; /* 未使用argument，消除警告 */
|
||||||
|
|
||||||
|
|
||||||
/* 计算任务运行到指定频率需要等待的tick数 */
|
|
||||||
const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
|
|
||||||
|
|
||||||
osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
|
osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
|
||||||
|
|
||||||
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
|
|
||||||
/* USER CODE INIT BEGIN */
|
/* USER CODE INIT BEGIN */
|
||||||
DR16_Init(&dr16);
|
DR16_Init(&dr16);
|
||||||
/* USER CODE INIT END */
|
/* USER CODE INIT END */
|
||||||
|
|
||||||
while (1) {
|
while (1) {
|
||||||
tick += delay_tick; /* 计算下一个唤醒时刻 */
|
|
||||||
/* USER CODE BEGIN */
|
/* USER CODE BEGIN */
|
||||||
DR16_StartDmaRecv(&dr16);
|
DR16_StartDmaRecv(&dr16);
|
||||||
if (DR16_WaitDmaCplt(20)) {
|
if (DR16_WaitDmaCplt(20)) {
|
||||||
@ -55,9 +48,8 @@ void Task_rc(void *argument) {
|
|||||||
} else {
|
} else {
|
||||||
DR16_Offline(&dr16);
|
DR16_Offline(&dr16);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
switch (dr16.data.sw_l) {
|
switch (dr16.data.sw_l) {
|
||||||
|
|
||||||
case DR16_SW_UP:
|
case DR16_SW_UP:
|
||||||
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
|
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
|
||||||
break;
|
break;
|
||||||
@ -80,57 +72,26 @@ void Task_rc(void *argument) {
|
|||||||
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0, 0); // 非阻塞发送底盘控制命令
|
osMessageQueuePut(task_runtime.msgq.Chassis_cmd, &cmd_for_chassis, 0, 0); // 非阻塞发送底盘控制命令
|
||||||
|
|
||||||
|
|
||||||
switch (dr16.data.sw_l) {
|
// for_shoot.online = dr16.header.online;
|
||||||
case DR16_SW_UP:
|
// switch (dr16.data.sw_r) {
|
||||||
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
|
// case DR16_SW_UP:
|
||||||
cmd_for_gimbal.delta_yaw = 0.0f;
|
// for_shoot.ready = false;
|
||||||
cmd_for_gimbal.delta_pit = 0.0f;
|
// for_shoot.firecmd = false;
|
||||||
break;
|
// break;
|
||||||
case DR16_SW_MID:
|
// case DR16_SW_MID:
|
||||||
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
// for_shoot.ready = true;
|
||||||
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x*5.0f;
|
// for_shoot.firecmd = false;
|
||||||
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
|
// break;
|
||||||
break;
|
// case DR16_SW_DOWN:
|
||||||
case DR16_SW_DOWN:
|
// for_shoot.ready = true;
|
||||||
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
|
// for_shoot.firecmd = true;
|
||||||
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x* 5.0f;
|
// break;
|
||||||
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y*5.0f;
|
// default:
|
||||||
break;
|
// for_shoot.ready = false;
|
||||||
default:
|
// for_shoot.firecmd = false;
|
||||||
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
|
// break;
|
||||||
cmd_for_gimbal.delta_yaw = 0.0f;
|
// }
|
||||||
cmd_for_gimbal.delta_pit = 0.0f;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
|
|
||||||
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
|
|
||||||
|
|
||||||
|
|
||||||
for_shoot.online = dr16.header.online;
|
|
||||||
switch (dr16.data.sw_r) {
|
|
||||||
case DR16_SW_UP:
|
|
||||||
for_shoot.ready = false;
|
|
||||||
for_shoot.firecmd = false;
|
|
||||||
break;
|
|
||||||
case DR16_SW_MID:
|
|
||||||
for_shoot.ready = true;
|
|
||||||
for_shoot.firecmd = false;
|
|
||||||
break;
|
|
||||||
case DR16_SW_DOWN:
|
|
||||||
for_shoot.ready = true;
|
|
||||||
for_shoot.firecmd = true;
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
for_shoot.ready = false;
|
|
||||||
for_shoot.firecmd = false;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
osMessageQueueReset(task_runtime.msgq.shoot.shoot_cmd);
|
|
||||||
osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &for_shoot, 0, 0);
|
|
||||||
|
|
||||||
/* USER CODE END */
|
/* USER CODE END */
|
||||||
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
|
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
@ -34,18 +34,3 @@ const osThreadAttr_t attr_ctrl_gimbal = {
|
|||||||
.priority = osPriorityNormal,
|
.priority = osPriorityNormal,
|
||||||
.stack_size = 256 * 4,
|
.stack_size = 256 * 4,
|
||||||
};
|
};
|
||||||
const osThreadAttr_t attr_ctrl_shoot = {
|
|
||||||
.name = "ctrl_shoot",
|
|
||||||
.priority = osPriorityNormal,
|
|
||||||
.stack_size = 256 * 4,
|
|
||||||
};
|
|
||||||
const osThreadAttr_t attr_monitor = {
|
|
||||||
.name = "monitor",
|
|
||||||
.priority = osPriorityNormal,
|
|
||||||
.stack_size = 256 * 4,
|
|
||||||
};
|
|
||||||
const osThreadAttr_t attr_ai = {
|
|
||||||
.name = "ai",
|
|
||||||
.priority = osPriorityNormal,
|
|
||||||
.stack_size = 256 * 4,
|
|
||||||
};
|
|
||||||
@ -18,9 +18,6 @@ extern "C" {
|
|||||||
#define ATTI_ESTI_FREQ (1000.0)
|
#define ATTI_ESTI_FREQ (1000.0)
|
||||||
#define CTRL_CHASSIS_FREQ (500.0)
|
#define CTRL_CHASSIS_FREQ (500.0)
|
||||||
#define CTRL_GIMBAL_FREQ (500.0)
|
#define CTRL_GIMBAL_FREQ (500.0)
|
||||||
#define CTRL_SHOOT_FREQ (500.0)
|
|
||||||
#define MONITOR_FREQ (500.0)
|
|
||||||
#define AI_FREQ (500.0)
|
|
||||||
|
|
||||||
/* 任务初始化延时ms */
|
/* 任务初始化延时ms */
|
||||||
#define TASK_INIT_DELAY (100u)
|
#define TASK_INIT_DELAY (100u)
|
||||||
@ -29,9 +26,6 @@ extern "C" {
|
|||||||
#define ATTI_ESTI_INIT_DELAY (0)
|
#define ATTI_ESTI_INIT_DELAY (0)
|
||||||
#define CTRL_CHASSIS_INIT_DELAY (500)
|
#define CTRL_CHASSIS_INIT_DELAY (500)
|
||||||
#define CTRL_GIMBAL_INIT_DELAY (0)
|
#define CTRL_GIMBAL_INIT_DELAY (0)
|
||||||
#define CTRL_SHOOT_INIT_DELAY (0)
|
|
||||||
#define MONITOR_INIT_DELAY (0)
|
|
||||||
#define AI_INIT_DELAY (0)
|
|
||||||
|
|
||||||
/* Exported defines --------------------------------------------------------- */
|
/* Exported defines --------------------------------------------------------- */
|
||||||
/* Exported macro ----------------------------------------------------------- */
|
/* Exported macro ----------------------------------------------------------- */
|
||||||
@ -46,9 +40,6 @@ typedef struct {
|
|||||||
osThreadId_t atti_esti;
|
osThreadId_t atti_esti;
|
||||||
osThreadId_t ctrl_chassis;
|
osThreadId_t ctrl_chassis;
|
||||||
osThreadId_t ctrl_gimbal;
|
osThreadId_t ctrl_gimbal;
|
||||||
osThreadId_t ctrl_shoot;
|
|
||||||
osThreadId_t monitor;
|
|
||||||
osThreadId_t ai;
|
|
||||||
} thread;
|
} thread;
|
||||||
|
|
||||||
/* USER MESSAGE BEGIN */
|
/* USER MESSAGE BEGIN */
|
||||||
@ -57,14 +48,7 @@ typedef struct {
|
|||||||
|
|
||||||
osMessageQueueId_t chassis_imu;
|
osMessageQueueId_t chassis_imu;
|
||||||
osMessageQueueId_t Chassis_cmd;
|
osMessageQueueId_t Chassis_cmd;
|
||||||
osMessageQueueId_t chassis_yaw;
|
|
||||||
struct {
|
|
||||||
osMessageQueueId_t imu;
|
|
||||||
osMessageQueueId_t cmd;
|
|
||||||
}gimbal;
|
|
||||||
struct {
|
|
||||||
osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
|
|
||||||
}shoot;
|
|
||||||
} msgq;
|
} msgq;
|
||||||
/* USER MESSAGE END */
|
/* USER MESSAGE END */
|
||||||
|
|
||||||
@ -86,9 +70,6 @@ typedef struct {
|
|||||||
UBaseType_t atti_esti;
|
UBaseType_t atti_esti;
|
||||||
UBaseType_t ctrl_chassis;
|
UBaseType_t ctrl_chassis;
|
||||||
UBaseType_t ctrl_gimbal;
|
UBaseType_t ctrl_gimbal;
|
||||||
UBaseType_t ctrl_shoot;
|
|
||||||
UBaseType_t monitor;
|
|
||||||
UBaseType_t ai;
|
|
||||||
} stack_water_mark;
|
} stack_water_mark;
|
||||||
|
|
||||||
/* 各任务运行频率 */
|
/* 各任务运行频率 */
|
||||||
@ -98,9 +79,6 @@ typedef struct {
|
|||||||
float atti_esti;
|
float atti_esti;
|
||||||
float ctrl_chassis;
|
float ctrl_chassis;
|
||||||
float ctrl_gimbal;
|
float ctrl_gimbal;
|
||||||
float ctrl_shoot;
|
|
||||||
float monitor;
|
|
||||||
float ai;
|
|
||||||
} freq;
|
} freq;
|
||||||
|
|
||||||
/* 任务最近运行时间 */
|
/* 任务最近运行时间 */
|
||||||
@ -110,9 +88,6 @@ typedef struct {
|
|||||||
float atti_esti;
|
float atti_esti;
|
||||||
float ctrl_chassis;
|
float ctrl_chassis;
|
||||||
float ctrl_gimbal;
|
float ctrl_gimbal;
|
||||||
float ctrl_shoot;
|
|
||||||
float monitor;
|
|
||||||
float ai;
|
|
||||||
} last_up_time;
|
} last_up_time;
|
||||||
|
|
||||||
} Task_Runtime_t;
|
} Task_Runtime_t;
|
||||||
@ -127,9 +102,6 @@ extern const osThreadAttr_t attr_rc;
|
|||||||
extern const osThreadAttr_t attr_atti_esti;
|
extern const osThreadAttr_t attr_atti_esti;
|
||||||
extern const osThreadAttr_t attr_ctrl_chassis;
|
extern const osThreadAttr_t attr_ctrl_chassis;
|
||||||
extern const osThreadAttr_t attr_ctrl_gimbal;
|
extern const osThreadAttr_t attr_ctrl_gimbal;
|
||||||
extern const osThreadAttr_t attr_ctrl_shoot;
|
|
||||||
extern const osThreadAttr_t attr_monitor;
|
|
||||||
extern const osThreadAttr_t attr_ai;
|
|
||||||
|
|
||||||
/* 任务函数声明 */
|
/* 任务函数声明 */
|
||||||
void Task_Init(void *argument);
|
void Task_Init(void *argument);
|
||||||
@ -138,9 +110,6 @@ void Task_rc(void *argument);
|
|||||||
void Task_atti_esti(void *argument);
|
void Task_atti_esti(void *argument);
|
||||||
void Task_ctrl_chassis(void *argument);
|
void Task_ctrl_chassis(void *argument);
|
||||||
void Task_ctrl_gimbal(void *argument);
|
void Task_ctrl_gimbal(void *argument);
|
||||||
void Task_ctrl_shoot(void *argument);
|
|
||||||
void Task_monitor(void *argument);
|
|
||||||
void Task_ai(void *argument);
|
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
|
|||||||
@ -17,10 +17,10 @@ function K = get_k_length(leg_length)
|
|||||||
R1=0.072; %驱动轮半径
|
R1=0.072; %驱动轮半径
|
||||||
L1=leg_length/2; %摆杆重心到驱动轮轴距离
|
L1=leg_length/2; %摆杆重心到驱动轮轴距离
|
||||||
LM1=leg_length/2; %摆杆重心到其转轴距离
|
LM1=leg_length/2; %摆杆重心到其转轴距离
|
||||||
l1=-0.01; %机体质心距离转轴距离
|
l1=0.03; %机体质心距离转轴距离
|
||||||
mw1=0.80; %驱动轮质量
|
mw1=0.80; %驱动轮质量
|
||||||
mp1=1.11; %杆质量
|
mp1=1.11; %杆质量
|
||||||
M1=12.0; %机体质量
|
M1=2.0; %机体质量
|
||||||
Iw1=mw1*R1^2; %驱动轮转动惯量
|
Iw1=mw1*R1^2; %驱动轮转动惯量
|
||||||
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
|
Ip1=mp1*((L1+LM1)^2+0.05^2)/12.0; %摆杆转动惯量
|
||||||
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
|
IM1=M1*(0.3^2+0.12^2)/12.0; %机体绕质心转动惯量
|
||||||
@ -48,7 +48,7 @@ function K = get_k_length(leg_length)
|
|||||||
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
||||||
B=double(B);
|
B=double(B);
|
||||||
|
|
||||||
Q=diag([700 1 600 200 1000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
Q=diag([100 1 500 100 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||||
R=[240 0;0 25]; %T Tp
|
R=[240 0;0 25]; %T Tp
|
||||||
|
|
||||||
K=lqr(A,B,Q,R);
|
K=lqr(A,B,Q,R);
|
||||||
|
|||||||
@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
|
|||||||
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
|
||||||
B=double(B);
|
B=double(B);
|
||||||
|
|
||||||
Q=diag([2000 1 600 200 2000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
Q=diag([700 100 2000 1500 2300 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
|
||||||
R=[240 0;0 50]; %T Tp
|
R=[140 0;0 50]; %T Tp
|
||||||
|
|
||||||
K=lqr(A,B,Q,R);
|
K=lqr(A,B,Q,R);
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user