ChibiOS Free Embedded RTOS

Code: Select all

  set_mpu_sample_rate(9);
  set_mpu_config_regsiter(EXT_SYNC_SET0, DLPF_CFG0);
  set_mpu_gyro(XG_ST_DIS, YG_ST_DIS, ZG_ST_DIS, FS_SEL_250);
  set_mpu_accel(XA_ST_DIS, YA_ST_DIS, ZA_ST_DIS, AFS_SEL_2g, ACCEL_HPF0);
  set_mpu_power_mgmt1(DEVICE_RESET_DIS, SLEEP_DIS, CYCLE_DIS, TEMPERATURE_EN, CLKSEL_XG);
  set_mpu_user_control(USER_FIFO_DIS, I2C_MST_DIS, I2C_IF_DIS, FIFO_RESET_DIS, I2C_MST_RESET_DIS, SIG_COND_RESET_DIS);

  write_mpu_power_mgmt1();
  write_mpu_gyro();
  write_mpu_accel();
  write_mpu_sample_rate();
  while (TRUE) {
     mpu_i2c_read_data(0x3B, 14); /* Read accelerometer, temperature and gyro data */
     chThdSleepMilliseconds(50);
  }

Code: Select all

int16_t complement2signed(uint8_t msb, uint8_t lsb){
  uint16_t word = 0;
  word = (msb << 8) + lsb;
  if (msb > 0x7F){
    return -1 * ((int16_t)((~word) + 1));
  }
  return (int16_t)word;
}