/**
* @brief 获取ADXL345传感器数值，各方向LSB RAW数据.
* @param data_out:存储传感器采集结果
* @retval None
**/
void ADXL345_GETXYZ(int16_t data_out[3])
{
float f1, f2, f3;
ADXL345_init(); //每次读写寄存器之前都要先读device ID
Hal_I2C_MutiRead(I2C2, data, ADXL345_ADDRESS, 0x32, 6);
data_out[0] = (int16_t)(data[0] + ((uint16_t)data[1] << 8));
data_out[1] = (int16_t)(data[2] + ((uint16_t)data[3] << 8));
data_out[2] = (int16_t)(data[4] + ((uint16_t)data[5] << 8));
#if 0
if(data_out[0] < 0)
{
data_out[0] = -data_out[0];
f1 = (float)(data_out[0] * 3.9);
printf("%s f1 -%f\n", __func__, f1);
}
if(data_out[1] < 0)
{
data_out[1] = -data_out[1];
f2 = (float)(data_out[1] * 3.9);
printf("%s f2 -%f\n", __func__, f2);
}
if(data_out[2] < 0)
{
data_out[2] = -data_out[2];
f3 = (float)(data_out[2] * 3.9);
printf("%s f3- %f\n", __func__, f3);
}
#endif
printf("ADXL345 ******X=%d,Y=%d,Z=%d*****\n", data_out[0], data_out[1], data_out[2]);
}
int main (void) {
setup();
set_light(1,1);
while(1){
if(!ADXL345_init())
error_red_flash();
util_delay_ms(10);
}while(1);
// error_green_flash();
//for(;;) loop();
}
int init_all(I2CVariables *i2c_var) {
if (!bcm2835_init()) return ERROR_BCM2835_INIT;
bcm2835_i2c_begin();
bcm2835_i2c_setClockDivider(BCM2835_I2C_CLOCK_DIVIDER_626); // 400 kHz
ADXL345_init();
L3G4200D_init();
HMC5883L_init();
BMP085_init();
PCA9685PW_init(1);
I2CVariables_init(i2c_var);
return 0;
}
int main()
{
int rfbargc = 0;
char **rfbargv = 0;
int bpp = 4;
rfbScreenInfoPtr server = rfbGetScreen(&rfbargc, rfbargv, FRAMEBUFFER_WIDTH, FRAMEBUFFER_HEIGHT, 8, 3, bpp);
fb = new unsigned char[FRAMEBUFFER_WIDTH * FRAMEBUFFER_HEIGHT * bpp];
server->frameBuffer = (char *)fb;
server->kbdAddEvent = handleKey;
server->ptrAddEvent = handlePointer;
rfbInitServer(server);
int file;
file = open(devname, O_RDWR);
if (file < 0) {
perror("open");
exit(EXIT_FAILURE);
}
int addr = ADXL345_ADDRESS; /* The I2C address */
if (ioctl(file, I2C_SLAVE, addr) < 0) {
perror("ioctl");
exit(EXIT_FAILURE);
}
uint16_t id = read_u16(file, 0x00);
printf("id is 0x%04X\n", id);
ADXL345_init(file);
int tilt_was_valid = false;
while(1) {
int16_t xdata = read_u16_le(file, ADXL345_DATAX0);
int16_t ydata = read_u16_le(file, ADXL345_DATAY0);
int16_t zdata = read_u16_le(file, ADXL345_DATAZ0);
float xg = xdata / 256.0;
float yg = ydata / 256.0;
float zg = zdata / 256.0;
float theta_y = -atan2(yg, zg);
float theta_x = atan2(xg, zg);
float y_angle_center = 40.0 / 180.0 * M_PI;
float y_angle_range = 25.0 / 180.0 * M_PI;
float x_angle_center = 0.0 / 180.0 * M_PI;
float x_angle_range = 25.0 / 180.0 * M_PI;
int tilt_is_valid =
(theta_y > y_angle_center - y_angle_range) &&
(theta_y < y_angle_center + y_angle_range) &&
(theta_x > x_angle_center - x_angle_range) &&
(theta_x < x_angle_center + x_angle_range);
if(tilt_is_valid != tilt_was_valid) {
if(tilt_is_valid)
printf("TILT_VALID\n");
else
printf("TILT_INVALID\n");
tilt_was_valid = tilt_is_valid;
}
if(tilt_is_valid) {
float tilt_x_valuator = (theta_x - x_angle_center) / x_angle_range;
float tilt_y_valuator = (theta_y - y_angle_center) / y_angle_range;
// printf(" %f, %f\n", tilt_x_valuator, tilt_y_valuator);
draw_new_circle(server,
(tilt_x_valuator + 1) * .5 * FRAMEBUFFER_WIDTH,
(tilt_y_valuator + 1) * .5 * FRAMEBUFFER_HEIGHT,
1.0);
}
// usleep(100000);
rfbProcessEvents(server, 10000);
}
}
