Updated FDM/control algorithm and fixed error in sensor.cpp