@Siya, @Arbutus , Benewake support has direclty provided a new firmware 02.00.03 for TFmini plus and I have tested it with 5 TFM+ in I2C with arduino nano (post here) and it works very well. Maybe they will publish this new firmware soon?
Now, I think that using an arduino or teensy is useless with I2C sensors, so I hope someone mastering C++ could help to continue the development of the driver directly in ardupilot firmware.
@lucasdemarchi has created the files here for a direct connection to an I2C port on the FC, but I don’t know if it’s a good implementation (the .c file seem to run in UART mode, and the good request is an “on-demand” request/response (I²C sensors can’t speak on the bus when they want!), and no need to parameter the frequency of measurements (UART mode only).
Then, we would need to “map” I²C sensor(s) address(es) to an avoidance direction/quaternion in mission planner.