Keep zero speed as is

diff --git a/arducontrol/mfx.c b/arducontrol/mfx.c
index 6e843aa288baae240120ccfb0d527bfe278e148d..293765fba9d7c8933106e48c6cdbc5ab4923cfa2 100644 (file)
--- a/arducontrol/mfx.c
+++ b/arducontrol/mfx.c
@@ -429,7 +429,9 @@ uint8_t mfx_command(const uint8_t *cmd, uint8_t length)
                uint8_t speed_dir = cmd[3];
                if((speed_dir&0x7F)==0x7F)
                        return INVALID_VALUE;
-               ++speed_dir;
+               // Skip emergency stop
+               if(speed_dir>0)
+                       ++speed_dir;
 
                if(cmd[0]==MFX_SPEED)
                        mfx_speed_packet(addr, speed_dir);