/* Read from the DMP memory. This function prevents I2C reads past the bank boundaries. The DMP memory is only accessible when the chip is awake.  */
int mpu_read_mem(unsigned short mem_addr, unsigned short length, unsigned char *data)
/* Read from the DMP memory. This function prevents I2C reads past the bank boundaries. The DMP memory is only accessible when the chip is awake.  */
int mpu_read_mem(unsigned short mem_addr, unsigned short length, unsigned char *data)
{
    unsigned char tmp[2];
    if (!data)
        return -1;
    if (!st.chip_cfg.sensors)
        return -1;
    tmp[0] = (unsigned char)(mem_addr >> 8);
    tmp[1] = (unsigned char)(mem_addr & 0xFF);
    if (tmp[1] + length > st.hw->bank_size)
        return -1;
    if (i2c_write(st.hw->addr, st.reg->bank_sel, 2, tmp))
        return -1;
    if (i2c_read(st.hw->addr, st.reg->mem_r_w, length, data))
        return -1;
    return 0;
}