MAVLink support for sensor level UAV simulation

edit

I'm trying to add native mavlink support to gazebo to send simulated sensor data to the px4 autopilot. We have already created a bunch of plugins to do all this, but I"m trying to reintegrate with upstream. Is anyone else work on this? I just posted an issue here: https://bitbucket.org/osrf/gazebo/iss...

The recently added Arducopter plugin almost provides what I would like, but it is too limited in that is provides the vehicle state with some added noise instead of individual sensor measurements.