Fixed bug in firmware