修改下位机发送的云台陀螺仪信息，角度制改为弧度制
This commit is contained in:
parent
3beffa3dea
commit
ba3879f815
|
|
@ -36,30 +36,26 @@ static float RefTemp = 40; // 恒温设定温度
|
|||
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
|
||||
|
||||
static void IMUPWMSet(uint16_t pwm)
|
||||
{
|
||||
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
|
||||
static void IMUPWMSet(uint16_t pwm) {
|
||||
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 温度控制
|
||||
*
|
||||
*/
|
||||
static void IMU_Temperature_Ctrl(void)
|
||||
{
|
||||
static void IMU_Temperature_Ctrl(void) {
|
||||
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
|
||||
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
|
||||
}
|
||||
|
||||
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
|
||||
static void InitQuaternion(float *init_q4)
|
||||
{
|
||||
static void InitQuaternion(float *init_q4) {
|
||||
float acc_init[3] = {0};
|
||||
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
|
||||
float axis_rot[3] = {0}; // 旋转轴
|
||||
// 读取100次加速度计数据,取平均值作为初始值
|
||||
for (uint8_t i = 0; i < 100; ++i)
|
||||
{
|
||||
for (uint8_t i = 0; i < 100; ++i) {
|
||||
BMI088_Read(&BMI088);
|
||||
acc_init[X] += BMI088.Accel[X];
|
||||
acc_init[Y] += BMI088.Accel[Y];
|
||||
|
|
@ -78,17 +74,15 @@ static void InitQuaternion(float *init_q4)
|
|||
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
|
||||
}
|
||||
|
||||
attitude_t *INS_Init(void)
|
||||
{
|
||||
attitude_t *INS_Init(void) {
|
||||
if (!INS.init)
|
||||
INS.init = 1;
|
||||
else
|
||||
return (attitude_t *)&INS.Gyro;
|
||||
return (attitude_t *) &INS.Gyro;
|
||||
|
||||
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
|
||||
|
||||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR)
|
||||
;
|
||||
while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR);
|
||||
IMU_Param.scale[X] = 1;
|
||||
IMU_Param.scale[Y] = 1;
|
||||
IMU_Param.scale[Z] = 1;
|
||||
|
|
@ -102,23 +96,22 @@ attitude_t *INS_Init(void)
|
|||
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
|
||||
// imu heat init
|
||||
PID_Init_Config_s config = {.MaxOut = 2000,
|
||||
.IntegralLimit = 300,
|
||||
.DeadBand = 0,
|
||||
.Kp = 1000,
|
||||
.Ki = 20,
|
||||
.Kd = 0,
|
||||
.Improve = 0x01}; // enable integratiaon limit
|
||||
.IntegralLimit = 300,
|
||||
.DeadBand = 0,
|
||||
.Kp = 1000,
|
||||
.Ki = 20,
|
||||
.Kd = 0,
|
||||
.Improve = 0x01}; // enable integratiaon limit
|
||||
PIDInit(&TempCtrl, &config);
|
||||
|
||||
// noise of accel is relatively big and of high freq,thus lpf is used
|
||||
INS.AccelLPF = 0.0085;
|
||||
DWT_GetDeltaT(&INS_DWT_Count);
|
||||
return (attitude_t *)&INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||||
return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
|
||||
}
|
||||
|
||||
/* 注意以1kHz的频率运行此任务 */
|
||||
void INS_Task(void)
|
||||
{
|
||||
void INS_Task(void) {
|
||||
static uint32_t count = 0;
|
||||
const float gravity[3] = {0, 0, 9.81f};
|
||||
|
||||
|
|
@ -126,8 +119,7 @@ void INS_Task(void)
|
|||
t += dt;
|
||||
|
||||
// ins update
|
||||
if ((count % 1) == 0)
|
||||
{
|
||||
if ((count % 1) == 0) {
|
||||
BMI088_Read(&BMI088);
|
||||
|
||||
INS.Accel[X] = BMI088.Accel[X];
|
||||
|
|
@ -159,7 +151,8 @@ void INS_Task(void)
|
|||
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
|
||||
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
|
||||
{
|
||||
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
|
||||
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) +
|
||||
INS.MotionAccel_b[i] * INS.AccelLPF / (INS.AccelLPF + dt);
|
||||
}
|
||||
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
|
||||
|
||||
|
|
@ -169,18 +162,16 @@ void INS_Task(void)
|
|||
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
|
||||
|
||||
//VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
|
||||
VisionSetAltitude(INS.Yaw, INS.Pitch);
|
||||
VisionSetAltitude(INS.Yaw * PI / 180, INS.Pitch * PI / 180);
|
||||
}
|
||||
|
||||
// temperature control
|
||||
if ((count % 2) == 0)
|
||||
{
|
||||
if ((count % 2) == 0) {
|
||||
// 500hz
|
||||
IMU_Temperature_Ctrl();
|
||||
}
|
||||
|
||||
if ((count++ % 1000) == 0)
|
||||
{
|
||||
if ((count++ % 1000) == 0) {
|
||||
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
|
||||
}
|
||||
}
|
||||
|
|
@ -191,8 +182,7 @@ void INS_Task(void)
|
|||
* @param[2] vector in EarthFrame
|
||||
* @param[3] quaternion
|
||||
*/
|
||||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
|
||||
{
|
||||
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q) {
|
||||
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
|
||||
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
|
||||
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
|
||||
|
|
@ -212,8 +202,7 @@ void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
|
|||
* @param[2] vector in BodyFrame
|
||||
* @param[3] quaternion
|
||||
*/
|
||||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
|
||||
{
|
||||
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q) {
|
||||
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
|
||||
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
|
||||
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
|
||||
|
|
@ -235,16 +224,14 @@ void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
|
|||
* @param gyro 角速度
|
||||
* @param accel 加速度
|
||||
*/
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
|
||||
{
|
||||
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]) {
|
||||
static float lastYawOffset, lastPitchOffset, lastRollOffset;
|
||||
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
|
||||
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
|
||||
|
||||
if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
|
||||
fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
|
||||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
|
||||
{
|
||||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag) {
|
||||
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
|
||||
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
|
||||
cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
|
||||
|
|
@ -304,8 +291,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
|
|||
/**
|
||||
* @brief Update quaternion
|
||||
*/
|
||||
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
|
||||
{
|
||||
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt) {
|
||||
float qa, qb, qc;
|
||||
|
||||
gx *= 0.5f * dt;
|
||||
|
|
@ -323,8 +309,7 @@ void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
|
|||
/**
|
||||
* @brief Convert quaternion to eular angle
|
||||
*/
|
||||
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
|
||||
{
|
||||
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll) {
|
||||
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
|
||||
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
|
||||
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
|
||||
|
|
@ -333,8 +318,7 @@ void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
|
|||
/**
|
||||
* @brief Convert eular angle to quaternion
|
||||
*/
|
||||
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
|
||||
{
|
||||
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q) {
|
||||
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
|
||||
Yaw /= 57.295779513f;
|
||||
Pitch /= 57.295779513f;
|
||||
|
|
|
|||
Loading…
Reference in New Issue