STM32平衡车

      Balance_PID_Result = Position_PID_Cal(&Balance_PID, imu.Roll + 0.3f);

      if (Time_GAP_20ms) Velocity_PID_Result = Position_PID_Cal(&Velocity_PID, Velocity);



      PID_Result = Balance_PID_Result + Velocity_PID_Result;

      Set_Motor_Speed(PID_Result, PID_Result);

static void Get_IMU_Values(float *values)

{

        int16_t gyro[3],acc[3];

        IMU_readGyro_Acc(&gyro[0],&acc[0]);

        for(int i=0;i<3;i++)

        {

        //gyro range +-2000; adc accuracy 2^16=65536; 65536/4000=16.4;

                values[i]=((float) gyro[i])/16.4f;        

                values[3+i]=(float) acc[i];

        }

}

[em:24:]

void IMU_Update(void)

{

        static float q[4];

        float Values[6];        

        Get_IMU_Values(Values);        

        

        //change angle to radian,the calculate the imu with Mahony

        MahonyAHRSupdateIMU(Values[0] * PI/180, Values[1] * PI/180, Values[2] * PI/180,

                             Values[3], Values[4], Values[5]);                

        //save Quaternion

        q[0] = q0;

        q[1] = q1;

        q[2] = q2;

        q[3] = q3;

    

        imu.ax = Values[3];

        imu.ay = Values[4];

        imu.az = Values[5];

    

        imu.Pitch_v = Values[0];

        imu.Roll_v = Values[1];

        imu.Yaw_v = Values[2];



        //calculate the imu angle with quaternion

        imu.Roll = (atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),1 - 2.0f*(q[1]*q[1] + q[2]*q[2])))* 180/PI;        

        imu.Pitch = -safe_asin(2.0f*(q[0]*q[2] - q[1]*q[3]))* 180/PI;

        imu.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 180/PI;

}

void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {

        float recipNorm;        

        float halfvx, halfvy, halfvz;        //1/2 重力分量

        float halfex, halfey, halfez;        //1/2 重力误差

        float qa, qb, qc;

        //对加速度数据归一化

        recipNorm = invSqrt(ax * ax + ay * ay + az * az);

        ax *= recipNorm;

        ay *= recipNorm;

        az *= recipNorm;



        // 由四元数计算重力分量

        halfvx = q1 * q3 - q0 * q2;

        halfvy = q0 * q1 + q2 * q3;

        halfvz = q0 * q0 - 0.5f + q3 * q3;



        // 将四元数重力分量 与 加速度计重力分量 作叉积 得到误差

        halfex = (ay * halfvz - az * halfvy);

        halfey = (az * halfvx - ax * halfvz);

        halfez = (ax * halfvy - ay * halfvx);



        //对误差作积分

        integralFBx += twoKi * halfex * (1.0f / sampleFreq);        

        integralFBy += twoKi * halfey * (1.0f / sampleFreq);

        integralFBz += twoKi * halfez * (1.0f / sampleFreq);

        //反馈到角速度

        gx += integralFBx;             gy += integralFBy;       gz += integralFBz;



        // 对误差作比例运算并反馈

        gx += twoKp * halfex;    gy += twoKp * halfey;    gz += twoKp * halfez;



        // 计算1/2 dt

        gx *= (0.5f * (1.0f / sampleFreq));                

        gy *= (0.5f * (1.0f / sampleFreq));

        gz *= (0.5f * (1.0f / sampleFreq));

        qa = q0;     qb = q1;      qc = q2;

        // 更新四元数

        q0 += (-qb * gx - qc * gy - q3 * gz);

        q1 += (qa * gx + qc * gz - q3 * gy);

        q2 += (qa * gy - qb * gz + q3 * gx);

        q3 += (qa * gz + qb * gy - qc * gx);



        // 四元数归一化

        recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

        q0 *= recipNorm;     q1 *= recipNorm;     q2 *= recipNorm;     q3 *= recipNorm;

}

> void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, 

>                       float mx, float my, float mz);

int main(void)

{

        All_HardWare_init();

        while (1)

        {

                Protect_Check();

                LED_show_working();

                LCD_show_Brief_info();

        }

}