Merge patch from Icare for Mega2560 support