pigcs: several fixes to SMO controller and position interpolation