Update mw.c

dynpid calculation stripped down