rt_mutex_take(status_mutex, RT_WAITING_FOREVER);
        
        status.chassis.motor_lf.v_feedback = v_lf;
        status.chassis.motor_lb.v_feedback = v_lb;
        status.chassis.motor_rf.v_feedback = v_rf;
        status.chassis.motor_rb.v_feedback = v_rb;
        
        /* 里程计数据解算 */
        #define     L   0.23f    /* 左右轮间距 */
        
        v_l = (v_lf + v_lb)/2.0f;
        v_r = (v_rf + v_rb)/2.0f; 

        status.info_send.speed_x = (v_l + v_r) / 2.0f;
        status.info_send.speed_y = 0.0f;
        status.info_send.speed_angular = -(v_l - v_r) / L / 2.0f;

        status.info_send.pose_angula += 0.93f * status.info_send.speed_angular*DELAY_TIME;
        status.info_send.pose_angula = angle_to_limit(status.info_send.pose_angula);
        status.info_send.position_x  += status.info_send.speed_x*cosf(status.info_send.pose_angula)*DELAY_TIME;
        status.info_send.position_y  += status.info_send.speed_x*sinf(status.info_send.pose_angula)*DELAY_TIME;

        rt_mutex_release(status_mutex);