@usinjin The 'easy' way to do this would be have the Z compensation reported in the Object Model, and use that. However, this isn't currently reported in the Object Model. I asked @dc42, who replied:
We could consider it for 3.6. Easiest would be to report the last computed height correction, which would be the correction at the endpoint of the current segment.
Would that suit your purpose? You could enable segmentation (see M669) would also allow the compensation to be updated more frequently on long, straight moves.
If so, please raise a feature request/enhancement on the firmware Github repository: https://github.com/Duet3D/RepRapFirmware/issues
Otherwise, you're going to need something that can read the XY position, and be able to compute the Z offset (ie have the mesh .csv file), and feed that to the pen motion control, in a timely manner. Something like that might be better done by a Raspberry Pi in SBC mode.
Ian