Copter: Conflicting received Heartbeat messages!?

ArduCopter
,
1

I have been successful in arming and setting mode in simulation environment. Now, I’m trying to do the same in reality. My companion computer, ODroid-XU4 is connected to Pixhawk via UART, using MAVLink protocol. I can send and receive messages successfully.

I’m connecting Pixhawk to my PC having QGroundControl running.
When I try to arm the vehicle, sometimes it’s armed. Other times, my code says that it’s armed while it’s not armed in QGroundControl. Some times, one heartbeat messages says that the vehicle is armed, while the next received heartbeat messages says it is not armed. I don’t know why does it behave like this.

The code for arming the vehicle is:

void mavArmDisarm(armStatus cmd)
{
    mavlink_msg_command_long_pack(	
            mavlink_system.sysid, mavlink_system.compid, &msg, targetSysId, targetCompId,          MAV_CMD_COMPONENT_ARM_DISARM,  0, cmd,    0, 0, 0, 0, 0, 0);

    len = mavlink_msg_to_send_buffer(buf, &msg);

    serial.serialWriteBytes(pbuf, len);
}

The other point is, when I set the mode to, say, GUIDED, I also receive one heartbeat message saying the current flight mode is GUIEDE while the next messages says the current flight mode is STABILIZE.
The code for setting the mode is:

void mavSetMode(FlightModes mode)
{
    mavlink_msg_set_mode_pack(mavlink_system.sysid, mavlink_system.compid, &msg, targetSysId, 1, mode);

    len = mavlink_msg_to_send_buffer(buf, &msg);

    serial.serialWriteBytes(pbuf, len);

    do
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }
    while(currentFlightMode != mode);

    printf(ANSI_COLOR_GREEN "Successfully setting mode to %s\n" ANSI_COLOR_RESET, controlMode[mode]);
}

The code for receiving the messages is:

void mavReceive(void)
{
	static uint16_t heartBaseMode;
	static char byte;

	while(serial.serialGetBytesAvailable() > 0)
	{
		byte = serial.serialReadByte();

		if(mavlink_parse_char(MAVLINK_COMM_0, byte, &msg, &status))
		{
			switch(msg.msgid)
			{
				case MAVLINK_MSG_ID_HEARTBEAT:
					heartBaseMode = mavlink_msg_heartbeat_get_base_mode(&msg);
					armed = (heartBaseMode & MAV_MODE_FLAG_SAFETY_ARMED);

	                            if(armed == MAV_MODE_FLAG_SAFETY_ARMED)
		                     {
			                     printf(ANSI_COLOR_GREEN "Vehicle Armed\n" ANSI_COLOR_RESET);
                  	    		}					
                  	    		else
                  	    		{ 
                  	    			printf(ANSI_COLOR_GREEN "Vehicle Disarmed\n" ANSI_COLOR_RESET);
                  	    		}
					currentFlightMode = mavlink_msg_heartbeat_get_custom_mode(&msg);

					printf(ANSI_COLOR_BLUE "Current flight mode: %s\n" ANSI_COLOR_RESET, controlMode[currentFlightMode]);

					break;
			}
		}
	}
}

The main function is:

int main(void)
{
	mavlink_system.sysid = 20;      // ID 20 for this system (Companion Computer)
	mavlink_system.compid = 20;     // The Id of the component sending the message

	serial.serialOpen(SERIAL_PORT, BAUD_RATE);

	// Start a timer to send heartbeat signal each second
	timerStart(mavSendHeartbeat, 1000);

	// Start a timer to receive messages from the other side each second
	timerStart(mavReceive, 10);

	do
	{
		mavArmDisarm(ARM);

		std::this_thread::sleep_for(std::chrono::milliseconds(2000));

	}while(armed != MAV_MODE_FLAG_SAFETY_ARMED);  // Check if vehicle armed successfully

	printf(ANSI_COLOR_GREEN "\nVehicle armed successfully \n" ANSI_COLOR_RESET);

	mavSetMode(GUIDED);

	while(1);
}

Could someone please tell me where is the problem?