关于bit的插入算法

#include "rf_data_combination.h"
#include "upload_data.h"


#define BIT_PACK_WORD uint32_t
#define WORD BIT_PACK_WORD
#define BITS_PER_WORD (sizeof(BIT_PACK_WORD) * 8u)
   

#define MASK(bits) ((1u << (bits)) - 1u)

void BitWriteUnsigned(WORD* buffer, int bit_offset, int bit_size, WORD value) 
{
 
  int word_offset = bit_offset / BITS_PER_WORD; //²åÈëÔÚµÚ¼¸¸ö×ÖÉÏ
  int bit_offset_in_word = bit_offset % BITS_PER_WORD; //ÔÚ²åÈëµ½µÚ¼¸¸ö×ÖÉϵĵڼ¸Î»

  // Case 1: Writing across a word boundary.
  if (bit_offset_in_word + bit_size > BITS_PER_WORD) 
  {
    int upper_bit_size = BITS_PER_WORD - bit_offset_in_word; //ÔÚÉÏÒ»¸ö×Ö½ÚÉϲåÈ뼸λ
    int lower_bit_size = bit_offset_in_word + bit_size - BITS_PER_WORD;
     
    // Upper bits - write in the lowest bits of the first word.
    WORD upper_bits = value >> lower_bit_size;
    buffer[word_offset] = (buffer[word_offset] & ~MASK(upper_bit_size)) | upper_bits;

    // Lower bits - write in the highest bits of the next word.
    WORD lower_bits = value & MASK(lower_bit_size);
    int shift = BITS_PER_WORD - lower_bit_size;
    buffer[word_offset + 1] = (buffer[word_offset + 1] & ~(MASK(lower_bit_size) << shift)) | (lower_bits << shift);
    return;
  }

  // Case 2: Writing within a word boundary.
  unsigned int shift = BITS_PER_WORD - bit_offset_in_word - bit_size;
  buffer[word_offset] = (buffer[word_offset] & ~(MASK(bit_size) << shift)) | (value << shift);
}

void BitWriteSigned(WORD* buffer, int bit_offset, int bit_size, BIT_PACK_WORD_SIGNED value) 
{
  // Writing integers with more bits than the word size is not supported.
 // assert(bit_size <= BITS_PER_WORD);

  // The value is larger than what is supported by the given word size.
//  if (bit_size != BITS_PER_WORD) {
//    assert(value < (1 << (bit_size - 1)));
//    assert(value >= -(1 << (bit_size - 1)));
//  }

  // Chop off the leading bits and then use unsigned writing.
  BitWriteUnsigned(buffer, bit_offset, bit_size, value & MASK(bit_size));
}


// Take a floating point value, clamp it to within [-1, 1], and convert it to a
// fixed point number using the number of specified bits.
// TODO(keir): Test that this does the right thing in saturation.
static inline uint32_t FloatToNormalizedData(float value, int bits) 
{
    int ret = 0, ret1 = 0;
    
    if (value > 1.0f) 
    {
        value = 1.0f;
    }
    if (value < -1.0f) 
    {
        value = -1.0f;
    }
    
    ret1 = MASK(bits - 1);
    ret = (int) (value * MASK(bits - 1));  //albert add test using
  return (int) (value * MASK(bits - 1));
}

static inline uint32_t ScaleAngleAxis(float value) 
{
    // Original precision estimates took +- 2*pi as the max value along any
    // axis, which gives more total rotation than we need.
 //   return FloatToNormalizedData(value / (2.0f * M_PI), ANGLE_AXIS_BITS);
      return FloatToNormalizedData(value, ANGLE_AXIS_BITS); //value µÄÖµ´«¹ýÀ´Ö±½ÓÊÇ-1 ---- +1

}

static inline uint32_t ScaleAccel(float valueMs2)
{
   
    return FloatToNormalizedData(valueMs2 / (16.0f * 9.80f), ACCELEROMETER_BITS); //¿´×ÅûÓбØÒª³ýÒÔ9.8
}

#define RADIANS_TO_DEGREES (float)(180.0 / 3.1415926535)

static inline int16_t ScaleGyro(float valueDegreePerSecond) //´«ÈëµÄ²»ÊÇ»¡¶ÈÖµ¶øÊǽǶÈÿÃë
{
  
    return FloatToNormalizedData(valueDegreePerSecond / 2000.0f, GYROSCOPE_BITS); //2000ÊÇ×î´óµÄ·¶Î§Öµ
}

static inline void ByteOrder(uint32_t* word_buffer, int num_words, uint8_t* output_buffer) 
{
    for (int i = 0; i < num_words; i++) 
    {
        uint32_t word = word_buffer[i];
        *(output_buffer++) =  word >> 24u;
        *(output_buffer++) = (word >> 16u) & 0xffu;
        *(output_buffer++) = (word >>  8u) & 0xffu;
        *(output_buffer++) =  word         & 0xffu;
  }
}

void PackData(upload_data_t * controller_state, uint8_t packet_counter, uint8_t* buffer) 
{
    uint32_t word_buffer[5] = {0x00};
    int bit_cursor = 0;
#define WRITE(signedornot, bit_size, value) \
    BitWrite ## signedornot ## igned(word_buffer, bit_cursor, bit_size, value); \
    bit_cursor += bit_size;

    controller_state->timestamp = 0;
    packet_counter = 0;
    // ʱ¼ä´ÁºÍÊý¾Ý°üµÄÌî³ä
    WRITE(Uns, 9, controller_state->timestamp & 0x1ff);
    WRITE(Uns, 5, packet_counter & 0x1f);

//    // Orientation. test albert add
    float angle_axis[3] = {0x00};
//    //QuaternionToAngleAxis(&(controller_state->orientation_quat), &angle_axis[0]);
    angle_axis[0] = 0.6223344;
    angle_axis[1] = 1.0;
    angle_axis[2] = -1.0;
  
    WRITE(S, ANGLE_AXIS_BITS, ScaleAngleAxis(angle_axis[0]));
    WRITE(S, ANGLE_AXIS_BITS, ScaleAngleAxis(angle_axis[1]));
    WRITE(S, ANGLE_AXIS_BITS, ScaleAngleAxis(angle_axis[2]));


    
    // Acceleration.   ¼ÓËÙ¶Èԭʼֵ13λµÄÌî³ä
    // FusionOutput_t const* fusion_output = &controller_state->fusion_output;
    WRITE(S, ACCELEROMETER_BITS, ScaleAccel(controller_state->acceleration_ms2[0]));
    WRITE(S, ACCELEROMETER_BITS, ScaleAccel(controller_state->acceleration_ms2[1]));
    WRITE(S, ACCELEROMETER_BITS, ScaleAccel(controller_state->acceleration_ms2[2]));

    // Rotation.   ÍÓÂÝÒÇԭʼֵ13λµÄÌî³ä
    WRITE(S, GYROSCOPE_BITS, ScaleGyro(controller_state->rotation_rps[0]));
    WRITE(S, GYROSCOPE_BITS, ScaleGyro(controller_state->rotation_rps[1]));
    WRITE(S, GYROSCOPE_BITS, ScaleGyro(controller_state->rotation_rps[2]));
  
    // Touchpad.
    WRITE(Uns, 8, controller_state->touchpad_x);
    WRITE(Uns, 8, controller_state->touchpad_y);

   
    // Buttons.
    WRITE(Uns, 1, controller_state->button_pressed[BUTTON_VOL_UP]);
    WRITE(Uns, 1, controller_state->button_pressed[BUTTON_VOL_DOWN]);
    WRITE(Uns, 1, controller_state->button_pressed[BUTTON_MENU]);
    WRITE(Uns, 1, controller_state->button_pressed[BUTTON_HOME]);
    WRITE(Uns, 1, controller_state->button_pressed[BUTTON_TP]);
    
    ByteOrder(word_buffer, 5, buffer);
 }

 


  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值