/* Read raw accel data directly from the registers.  */
int mpu_get_accel_reg(short *data, unsigned long *timestamp)
/* Read raw accel data directly from the registers.  */
int mpu_get_accel_reg(short *data, unsigned long *timestamp)
{
    unsigned char tmp[6];
    if (!(st.chip_cfg.sensors & INV_XYZ_ACCEL))
        return -1;
    if (i2c_read(st.hw->addr, st.reg->raw_accel, 6, tmp))
        return -1;
    data[0] = (tmp[0] << 8) | tmp[1];
    data[1] = (tmp[2] << 8) | tmp[3];
    data[2] = (tmp[4] << 8) | tmp[5];
    if (timestamp)
    	timestamp = HAL_GetTick();
    return 0;
}