AP_HAL_PX4: Adding functionality to the I2CDriver