Updated & tested code with sensors, now for flight test.