Issue #863 fixed (firmware part)