fix some i2c bug.and backup for rewrite i2c.
add ahrs module

Signed-off-by: tcz717 <tcz717@hotmail.com>