set_roll_pitch_yaw_thrust flight mode

Hello to all:
I´m doing a high level autopilot based in ROS, but I need to use an autonomous mode of the APM that use roll, altitude, speed as set poins input, that will be send via mavlink in a new message or using the message SET_ROLL_PITCH_YAW_THRUST. Can anybody tell which is the best way to start???
Thanks

I think, you’d be better off at drones-discuss.