Switching to guide mode using a companion computer

Hi all,
I am new to this awesome world of UV and I am doing some research related to it.

I am using a companion computer to implement a geofence application and obstacle avoidance. My rover has a pixhawk 4 with ArduPilot 4. Right now, I am using the Rover in manual mode, and when reaching some point I want to switch to guide mode and bring the car to a given location.

To do that, I first switch to guided mode using MAVLINK_MSG_ID_SET_MODE with custom mode equals to 15 and I enable the flags MAV_MODE_FLAG_GUIDED_ENABLED|MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base mode. The rover has the GPS enabled according to the green fixed led and I ask it to send MAVLINK_MSG_GPS_RAW_INT, in which the fix_type field for this message is 3 or 4, indicating that the GPS is locked. No matter whether fix_type value is 3 or 4 the switching is not performed many times.

The MAVLINK command seems to be correctly generated since some times it correctly switches to guided mode. But many times it ignores the message no matter how many times is sent. Any idea or hints on what should I modify to make it work constantly?

To return to home, I send a mavlink message MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT with the home coordinates. In those times that the Rover switches to Guided mode, the vehicle doesn’t move, and instead I receive shortly after an ack MAVLINK_MSG_ID_MISSION_ITEM_REACHED message.

Thank you for reading all up here!