Publish i2c failure count
Use GPS HDOP and altitude from GPS message
Put IMU in base_link frame