Fix minor bugs and running full simulation on H7