Add camera test code, Mbed test code and some fixes.