修改下位机发送的云台陀螺仪信息，角度制改为弧度制

修改了CRC校验码未初始化问题
2024-01-19 17:58:40 +08:00 · 2024-01-19 15:38:20 +08:00
4 changed files with 88 additions and 103 deletions
--- a/bsp/usb/bsp_usb.c
+++ b/bsp/usb/bsp_usb.c
@ -27,5 +27,5 @@ uint8_t *USBInit(USB_Init_Config_s usb_conf)

 void USBTransmit(uint8_t *buffer, uint16_t len)
 {
-    //CDC_Transmit_FS(buffer, len); // 发送
+    CDC_Transmit_FS(buffer, len); // 发送
 }
--- a/modules/algorithm/crc16.c
+++ b/modules/algorithm/crc16.c
@ -24,6 +24,7 @@ static uint16_t crc_tab16[256] = {
        0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9,
        0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
        0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
+
 /*
 * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes );
 *
@ -32,21 +33,29 @@ static uint16_t crc_tab16[256] = {
 *要检查的字节也是一个参数。字符串中的字节数为
 *受恒定大小最大值的限制。
 */
-uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
-{
-    uint16_t crc;
-    const uint8_t *ptr;
-    uint16_t a;
-//    if (!crc_tab16_init)
-//        init_crc16_tab();
-    crc = CRC_START_16;
-    ptr = input_str;
-    if (ptr != NULL)
-        for (a = 0; a < num_bytes; a++)
-        {
-            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF];
+uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) {
+//    uint16_t crc;
+//    const uint8_t *ptr;
+//    uint16_t a;
+////    if (!crc_tab16_init)
+////        init_crc16_tab();
+//    crc = CRC_START_16;
+//    ptr = input_str;
+//    if (ptr != NULL)
+//        for (a = 0; a < num_bytes; a++) {
+//            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
+//        }
+//    return crc;
+    uint8_t ch_data;
+    uint16_t wCRC = 0xFFFF;
+
+    if (input_str == NULL) return 0xFFFF;
+    while (num_bytes--) {
+        ch_data = *input_str++;
+        (wCRC) =
+                ((uint16_t)(wCRC) >> 8) ^ crc_tab16[((uint16_t)(wCRC) ^ (uint16_t)(ch_data)) & 0x00ff];
    }
-    return crc;
+    return wCRC;
 }

 /*
@ -57,8 +66,7 @@ uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes)
 *要检查的字节数也是一个参数。
 */

-uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
-{
+uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) {
    uint16_t crc;
    const uint8_t *ptr;
    uint16_t a;
@ -69,8 +77,7 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
    crc = CRC_START_MODBUS;
    ptr = input_str;
    if (ptr != NULL)
-        for (a = 0; a < num_bytes; a++)
-        {
+        for (a = 0; a < num_bytes; a++) {

            crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) *ptr++) & 0x00FF];
        }
@ -83,8 +90,7 @@ uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes)
 *函数update_crc_16()根据
 *前一个循环冗余校验值和下一个要检查的数据字节。
 */
-uint16_t update_crc_16(uint16_t crc, uint8_t c)
-{
+uint16_t update_crc_16(uint16_t crc, uint8_t c) {
    if (!crc_tab16_init)
        init_crc16_tab();
    return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t) c) & 0x00FF];
@ -98,18 +104,15 @@ uint16_t update_crc_16(uint16_t crc, uint8_t c)
 *查找表首次由init_crc16_tab()例程计算
 *调用循环冗余校验函数。
 */
-void init_crc16_tab(void)
-{
+void init_crc16_tab(void) {
    uint16_t i;
    uint16_t j;
    uint16_t crc;
    uint16_t c;
-    for (i = 0; i < 256; i++)
-    {
+    for (i = 0; i < 256; i++) {
        crc = 0;
        c = i;
-        for (j = 0; j < 8; j++)
-        {
+        for (j = 0; j < 8; j++) {
            if ((crc ^ c) & 0x0001)
                crc = (crc >> 1) ^ CRC_POLY_16;
            else
@ -121,11 +124,9 @@ void init_crc16_tab(void)
    crc_tab16_init = 1;
 }

-uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength)
-{
+uint32_t VerifyCRC16CheckSum(uint8_t *pchMessage, uint32_t dwLength) {
    uint16_t wExpected = 0;
-    if ((pchMessage == NULL) || (dwLength <= 2))
-    {
+    if ((pchMessage == NULL) || (dwLength <= 2)) {
        return 0;
    }
    wExpected = crc_16(pchMessage, dwLength - 2);
--- a/modules/imu/ins_task.c
+++ b/modules/imu/ins_task.c
@ -36,8 +36,7 @@ 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)
-{
+static void IMUPWMSet(uint16_t pwm) {
            __HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
 }

@ -45,21 +44,18 @@ static void IMUPWMSet(uint16_t 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,8 +74,7 @@ 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
@ -87,8 +82,7 @@ attitude_t *INS_Init(void)

    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;
@ -117,8 +111,7 @@ attitude_t *INS_Init(void)
 }

 /* 注意以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;
--- a/modules/vofa/vofa.c
+++ b/modules/vofa/vofa.c
@ -30,6 +30,6 @@ void vofa_justfloat_output(float *data, uint8_t num , UART_HandleTypeDef *huart
    send_data[4 * num + 2] = 0x80;
    send_data[4 * num + 3] = 0x7f; //加上协议要求的4个尾巴

-    //HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
-    CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
+    HAL_UART_Transmit(huart, (uint8_t *)send_data, 4 * num + 4, 100);
+    //CDC_Transmit_FS((uint8_t *)send_data,4 * num + 4);
 }
Author	SHA1	Message	Date
shmily744	ba3879f815	修改下位机发送的云台陀螺仪信息，角度制改为弧度制	2024-01-19 17:58:40 +08:00
shmily744	3beffa3dea	修改了CRC校验码未初始化问题	2024-01-19 15:38:20 +08:00