Sending commands from Arduino Mega 2560 to APM 2.6

For my project, we have a quadcopter that must become autonomous. We have decided to do this by using an onboard Arduino Mega 2560 and send MAVLink messages to the APM 2.6 through a telemetry connection. The problem is: I have no idea how to make/code MAVLink commands in the Arduino IDE to control the APM 2.6. The final objective is to have sensor data from sonar and pixy camera, and fly the quadcopter autonomously towards a certain color picked up by the camera and avoiding collision using the sonar sensors. To begin, I would like some sort of control over the APM 2.6 using the Arduino and MAVLink. I have no experience with MAVLink and minimal experience with the Arduino IDE. Please point me in the right direction to accomplish my goal.

Would really appreciate some help

here is a freebee :wink:
Basic Obstacle Stop System-Build Log and firmware