Fixed servo test functionality in fmu.cpp.