Fixed bugs after first flight test