Setting Guided mode and Waypoint by MAVLink

ArduCopter
, ,
1

I’m trying to set mode to Guided via MAVLink command but it fails. Following is my code

mavlink_msg_command_long_pack(mavlink_system.sysid, mavlink_system.compid, &msg, targetSysId, targetCompId,	MAV_CMD_DO_SET_MODE, 0, MAV_MODE_GUIDED_ARMED, 0, 0, 0, 0, 	0, 0);
	len = mavlink_msg_to_send_buffer(buf, &msg);

I also tried MAV_MODE_GUIDED_DISARMED but in both cases the result is
Got MAVLink msg: COMMAND_ACK {command : 176, result : 3} which means MAV_MISSION_UNSUPPORTED. How can I fix this?

I’m also trying to fly the drone to a position relative to the current position of the drone, i.e., moving to just one waypoint and not doing a full mission. I thought that SET_POSITION_TARGET_LOCAL_NED is the way to do it but it didn’t work. Then I tried to follow the instructions here and here. Here is my code:

uint16_t targetSysId = 1;
uint16_t targetCompId = 1;

void mavMissionCount(uint8_t count)
{
	mavlink_msg_mission_count_pack(mavlink_system.sysid, mavlink_system.compid, &msg, targetSysId, targetCompId, count);
	len = mavlink_msg_to_send_buffer(buf, &msg);
       //Sending
}

void mavMissionClearAll(void)
{
     mavlink_msg_mission_clear_all_pack(mavlink_system.sysid, mavlink_system.compid, 	&msg, targetSysId, targetCompId);
     len = mavlink_msg_to_send_buffer(buf, &msg);
     //Sending
}

void mavMissionItem(int32_t xNed, int32_t yNed, int32_t zNed)
{
	mavlink_msg_mission_item_pack(mavlink_system.sysid, mavlink_system.compid, &msg, targetSysId, targetCompId, 1, MAV_FRAME_LOCAL_OFFSET_NED, MAV_CMD_NAV_WAYPOINT, 1, 1, 0, 2, 0, NAN, xNed,		yNed, zNed);
        len = mavlink_msg_to_send_buffer(buf, &msg);
        //Sending
}

int main(void)
{
    mavlink_system.sysid = 20;
    mavlink_system.compid = 20;
    ...
    mavMissionClearAll();
    mavMissionCount(1);
    mavReceive();
    //mavMissionItem(5, 0, 0);
    ...
}

But the output on MAVProxy console is
Got MAVLink msg: MISSION_ACK {target_system : 20, target_component : 200, type : 0}
not loading waypoints
Got MAVLink msg: MISSION_ACK {target_system : 1, target_component : 1, type : 0}
APM: Flight plan received
Got MAVLink msg: MISSION_ACK {target_system : 1, target_component : 1, type : 1}

And I receive COMMAND_ACK from MAVProxy and not MISSION_REQUEST.
Also, when I tried to get HEARTBEAT before sending these messages, I didn’t receive anything from MAVProxy
How can I fix this? And is sending waypoint the only way to do it? As I said this is not an auto mission, it’s just moving from current position to another and that’s it.

Many thanks

2

Hi,

Regarding changing mode, I see you are using a MAV_MODE enum value in the first parameter, but that doesn’t apply to ArduPilot. You need to be using param 2 (custom mode), with the value matching the mode number in Copter: https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/defines.h#L88-L105

Regarding heartbeats, are you sending them to autopilot? You need to.

I’ll need to look up the rest, I’ll try to do it later today.

3

@OXINARF, thanks a lot for your support. This is kind of you.
I’ve updated mavGuided() function as you said but it doesn’t work. MAVProxy gives the same output as before. I set custome_mode to 4 (Guided). Here is my new pack function:

void mavGuided(void)
{
    mavlink_msg_command_long_pack(mavlink_system.sysid,  mavlink_system.compid, &msg, targetSysId, targetCompId, MAV_CMD_DO_SET_MODE, 0, 0, 4, 0, 0, 0, 0, 0);
    len = mavlink_msg_to_send_buffer(buf, &msg);
    sendto(clientSocket,buf,len,0,(struct sockaddr *)&serverAddr,addr_size);
}

What is missing?

I also tried to send heartbeat to MAVProxy, but unexpectedly, MAVProxy didn’t receive this message, although it receives all the rest of the messages from my program!! Here is my function

void mavSendHeartbeat(void)
{
	mavlink_msg_heartbeat_pack(	
			mavlink_system.sysid, mavlink_system.compid, 	&msg, MAV_TYPE_HEXAROTOR, 		MAV_AUTOPILOT_ARDUPILOTMEGA,  MAV_MODE_FLAG_GUIDED_ENABLED,	
			0,			// A bitfield - I DON'T KNOW WHAT SHOULD IT BE
			MAV_STATE_BOOT);	// System status flag - THIS ONE TOO

	len = mavlink_msg_to_send_buffer(buf, &msg);
	int bytes = sendto(clientSocket,buf,len,0,(struct sockaddr *)&serverAddr,addr_size);
	printf("Sent bytes = %d\n", bytes);      // THIS PRINTS 17
}
4

Thanks to @OXINARF, my code is working now
Regarding changing mode, Ardupilot supports SET_MODE and not command_long message.
Here is a code example. mode can be any value defined in this enum

void mavSetMode(uint8_t mode)
{
	mavlink_msg_set_mode_pack(
			mavlink_system.sysid, 	// @param system_id ID of this system
			mavlink_system.compid,	// @param component_id ID of this component (e.g. 200 for IMU)
			&msg, 			// @param msg The MAVLink message to compress the data into
			targetSysId, 		// @param target_system The system setting the mode
			1, 			// @param base_mode The new base mode (Should be 1 to use custom_mode)
			mode);			// @param custom_mode The new autopilot-specific mode. Defined in ArduCopter/defines.h
	len = mavlink_msg_to_send_buffer(buf, &msg);
	sendto(clientSocket,buf,len,0,(struct sockaddr *)&serverAddr,addr_size);
}

Regarding flying to position, here is my code

void mavGoToPosition(uint16_t north, uint16_t east, int16_t down)
{
	// @brief Pack a set_position_target_local_ned message
	// @return length of the message in bytes (excluding serial stream start sign)
	mavlink_msg_set_position_target_local_ned_pack(
			mavlink_system.sysid,		// @param system_id ID of this system                     
			mavlink_system.compid,		// @param component_id ID of this component (e.g., 200 for IMU)                                    
			&msg,				// @param msg The MAVLink message to compress the data into                                       	
			0, 			  	// @param time_boot_ms Timestamp in millisecon since system boot
			targetSysId,			// @param target_system System ID
			targetCompId,			// @param target_component Component ID
			MAV_FRAME_LOCAL_OFFSET_NED, 	// @param coordinate_frame Valid options are: MAV_FRAME_(LOCAL,LOCAL_OFFSET,BODY,BODY_OFFSET)_NED
			0b0000111111111000, 	  	// @param type_mask Bitmask to indicate which dimensions should be ignored by the vehicle. 
			// Mapping: bit 1: x, bit 2: y, bit 3: z, bit 4: vx, bit 5: vy, bit 6: vz, bit 7: ax, 
			// bit 8: ay, bit 9: az, bit 10: is force setpoint, bit 11: yaw, bit 12: yaw rate
			north, 		  		// @param x X Position in NED frame in meters
			east, 		  		// @param y Y Position in NED frame in meters
			down, 		  		// @param z Z Position in NED frame in meters (note, altitude is negative in NED) 
			0, 0, 0,		  	// @param vx X, Y, Z velocity in NED frame in meter/ s 
			0, 0, 0, 		  	// @param afx X, Y, Z acceleration or force (if bit 10 of type_mask is set) in NED frame in meter / s^2 or N
			0, 		  		// @param yaw yaw setpoint in rad
			0);		  		// @param yaw_rate yaw rate setpoint in rad/s

	len = mavlink_msg_to_send_buffer(buf, &msg);
	sendto(clientSocket,buf,len,0,(struct sockaddr *)&serverAddr,addr_size);
}
2 Likes