#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);
}
关于bit的插入算法
最新推荐文章于 2022-01-27 08:00:00 发布