Heartbeat not received regularly & compare positions

ArduCopter Copter Simulation
,
1

My code sends MAVLink messages to MAVProxy via UDP. Everything is working fine but I don’t receive heartbeat signals from MAVProxy regularly. Sometimes I run the code and it doesn’t receive any heartbeat message from MAVProxy. Sometimes I run the code and it receives two or three heartbeats in 2 minutes. My program sends heartbeat message every second. I tried calling the function that receive messages from MAVProxy every 1000 ms, 500 ms and 100 ms. I think I should receive heartbeat in regular basis, right?

The other point is, I send SET_POSITION_TARGET_LOCAL_NED to make the copter fly 10 meters North for example. How can I check if the copter reached its destination. I tried comparing NED and INT positions before sending the command and afterwards but this doesn’t look like the right way to do that. Is there some other way?

2

I think the answer for my second question is here.

3

Hi,

ArduPilot should send about one heartbeat per second. You can see the heartbeat packets in MAVProxy console by writing watch HEARTBEAT.

Regarding knowing the reached destination, as talked earlier, you need to rely on the information ArduPilot sends regularly, Guided mode doesn’t have any message telling it reached a location.

4

Thanks @OXINARF for your reply.
I run watch HEARTBEAT in MAVProxy and saw the in and out hearbeat but I don’t know why I’m not receiving them regularly. As I said, sometimes I receive them every 1 seconds, sometimes I receive 2 heartbeats in 2 minutes and sometimes I don’t receive any heartbeat. Although I receive other messages, like POSITION_NED regularly!
Here is my receiving function

void mavReceive(void)
{
        ...
	// Receive message from server
	bytesReceived = recvfrom(clientSocket,rxBuf,1024,0,NULL, NULL);

        while(bufIndex < bytesReceived)
	{
		ch = rxBuf[bufIndex++];

		if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status))
		{
			switch(msg.msgid)
			{
				case MAVLINK_MSG_ID_HEARTBEAT:
					printf(ANSI_COLOR_GREEN     "Hearbeat received ..    "     ANSI_COLOR_RESET);
			                  
                                       ...
			}
		}
	    }
}

void timer_start(std::function<void(void)> func, unsigned int interval)
{
	std::thread([func, interval]()
			{ 
			while (true)
			{ 
			auto x = std::chrono::steady_clock::now() + std::chrono::milliseconds(interval);
			func();
			std::this_thread::sleep_until(x);
			}
			}).detach();
}

int main(void)
{
        ...
      	// Start a timer to receive messages from the other side each second
	timer_start(mavReceive, 1000);
        ...
}

What can be the problem?

5

I’ve got no idea :confused:

6

Please provide a complete working (minimal!) example program which demonstrates the problem.

I can see bufIndex isn’t zeroed in the fragment you gave, but without having all the code it is hard to tell if that’s a fault or not!

7

Thanks @peterbarker for your reply. I removed all parts irrelevant to the heartbeat problem and following is the minimal working example program. When I run it, onetime the heartbeat message is received each 1 second regularly. One other time I re-run the program and it receives heartbeat but in irregular basis, e.g., once each 3 or 4 or 2 seconds. One other time I re-run the program and it doesn’t receive any heartbeat messages.

NavControl.h

mavlink_system_t mavlink_system;

// Ardupilot system (the target system) has SYSID = 1 
// if not sending message to a specific component, then use CompID = 1
uint16_t targetSysId = 1;
uint16_t targetCompId = 1;

const uint8_t MSG_SIZE  = MAVLINK_MAX_PAYLOAD_LEN;

// Define the system type, in this case generic
uint8_t system_type = MAV_TYPE_GENERIC;
uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC;
 
uint8_t system_mode = MAV_MODE_PREFLIGHT;    // Booting up
uint32_t custom_mode = 0;                    // Custom mode, can be defined by user/adopter
uint8_t system_state = MAV_STATE_STANDBY;    // System ready for flight

// Initialize the required buffers
mavlink_message_t msg;
mavlink_status_t status;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint8_t *pbuf = buf;

// Get the length of the message to be sent
uint16_t len;
  
// File Descriptor for serial port handling
int fd;

uint8_t rxBuf[MSG_SIZE];                      // A buffer to receive the mavlink message in
uint8_t bytesReceived = 0;                    // Number of received bytes
uint8_t bufIndex = 0;                         // Index for the receiver buffer
uint8_t ch = 0;                               // Character to hold one byte from the receiver buffer

enum armStatus {ARM = 1, DISARM = 0};

// Variables for UDP connection
int clientSocket, portNum, nBytes;
struct sockaddr_in serverAddr;
socklen_t addr_size;

NavControl.cpp

#include "serialUart.h"
#include "NavControl.h"

uint8_t heartBeatReceived = 0;
uint16_t heartBaseMode;
uint16_t heartCustomMode;

void timer_start(std::function<void(void)> func, unsigned int interval)
{
	std::thread([func, interval]()
			{ 
			while (true)
			{ 
			auto x = std::chrono::steady_clock::now() + std::chrono::milliseconds(interval);
			func();
			std::this_thread::sleep_until(x);
			}
			}).detach();
}

void mavSendHeartbeat(void)
{
	mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, MAV_TYPE_ONBOARD_CONTROLLER,
			MAV_AUTOPILOT_INVALID, 0, 0, 0);

	len = mavlink_msg_to_send_buffer(buf, &msg);

	sendto(clientSocket,buf,len,0,(struct sockaddr *)&serverAddr,addr_size);
}

uint8_t mavReceive(void)
{
	bufIndex = 0;
	static long double i = 0;

	bytesReceived = recvfrom(clientSocket, rxBuf, 1024, 0, NULL, NULL);

	// Process the received bytes to form a message
	while(bufIndex < bytesReceived)
	{
		ch = rxBuf[bufIndex++];

		if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status))
		{
			switch(msg.msgid)
			{
				case MAVLINK_MSG_ID_HEARTBEAT:
					heartBeatReceived = 1; 
					printf("Hearbeat received ..    \n");
					break;

				
				default:
					//printf("Received .. MSG ID = %d", msg.msgid);
					break;
			}
		}
	}

	return 1;
}


int main(void)
{
	mavlink_system.sysid = 20; 
	mavlink_system.compid = 20;

	clientSocket = socket(PF_INET, SOCK_DGRAM, 0);

	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons(14551);
	serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
	memset(serverAddr.sin_zero, '\0', sizeof serverAddr.sin_zero);  

	addr_size = sizeof serverAddr;

	// Start a timer to send heartbeat signal each second
	timer_start(mavSendHeartbeat, 2000);
	// Start a timer to receive messages from the other side each second
	timer_start(mavReceive, 10);
	
	while(1);

}
8

Sorry, your example doesn’t compile for me - missing header. Please also provide build instructions suitable for Linux; std::function isn’t defined for me.

I did notice that you’re incorrectly using MAVLINK_MAX_PAYLOAD_LEN where you should be using MAVLINK_MAX_PACKET_LEN

You also need to use uint16_t for that offsets into a buffer containing a mavlink packet.

Also, you’re not checking recvfrom()'s return value for -1

9

Sorry for the missing headers. Now this is a complete version. I’ve changed MAVLINK_MAX_PAYLOAD_LEN to be MAVLINK_MAX_PACKET_LEN and I’m checking recfrom()'s return value. I don’t understand what do you mean with th offsets part.
Running this version, I don’t receive any heartbeat messages but I receive other messages from MAVProxy.

NavControl.h

#include "/home/salahuddin/Git/MAVLink/ardupilotmega/mavlink.h"

#include <chrono>
#include <thread>
#include <time.h>

#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <arpa/inet.h>

#include <iostream>
#include <functional>

// For monitoring time elapsed
using namespace std::chrono;

mavlink_system_t mavlink_system;

// Ardupilot system (the target system) has SYSID = 1 
// if not sending message to a specific component, then use CompID = 1
uint16_t targetSysId = 1;
uint16_t targetCompId = 1;

const uint16_t MSG_SIZE  = MAVLINK_MAX_PACKET_LEN;

// Initialize the required buffers
mavlink_message_t msg;
mavlink_status_t status;
uint8_t buf[MAVLINK_MAX_PACKET_LEN] = {0};
uint8_t *pbuf = buf;

// Get the length of the message to be sent
uint16_t len = 0;
  
uint8_t rxBuf[MSG_SIZE] = {0};                // A buffer to receive the mavlink message in
uint8_t bytesReceived = 0;                    // Number of received bytes
uint8_t bufIndex = 0;                         // Index for the receiver buffer
uint8_t ch = 0;                               // Character to hold one byte from the receiver buffer

enum armStatus {ARM = 1, DISARM = 0};

// Variables for UDP connection
int clientSocket, portNum, nBytes;
struct sockaddr_in serverAddr;
socklen_t addr_size;

NavControl.cpp

#include "NavControl.h"

void timer_start(std::function<void(void)> func, unsigned int interval)
{
	std::thread([func, interval]()
			{ 
			while (true)
			{ 
			auto x = std::chrono::steady_clock::now() + std::chrono::milliseconds(interval);
			func();
			std::this_thread::sleep_until(x);
			}
			}).detach();
}

void mavSendHeartbeat(void)
{
	mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, MAV_TYPE_ONBOARD_CONTROLLER,
			MAV_AUTOPILOT_INVALID, 0, 0, 0);

	len = mavlink_msg_to_send_buffer(buf, &msg);

	printf("Heartbeat sent\n");

	sendto(clientSocket,buf,len,0,(struct sockaddr *)&serverAddr,addr_size);
}

uint8_t mavReceive(void)
{
	bufIndex = 0;
	bytesReceived = 0;

	bytesReceived = recvfrom(clientSocket, rxBuf, 1024, 0, NULL, NULL);

	if(bytesReceived < 0)
	{
		printf("Error receiveing UDP\n");
	}

	// Process the received bytes to form a message
	while(bufIndex < bytesReceived)
	{
		ch = rxBuf[bufIndex++];

		if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status))
		{
			switch(msg.msgid)
			{
				case MAVLINK_MSG_ID_HEARTBEAT:
					printf("Hearbeat received .. \n");
					break;

				default:
					//printf("Received .. MSG ID = %d\n", msg.msgid);
					break;
			}
		}
	}

	return 1;
}


int main(void)
{
	mavlink_system.sysid = 20; 
	mavlink_system.compid = 20;

	clientSocket = socket(PF_INET, SOCK_DGRAM, 0);

	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons(14551);
	serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
	memset(serverAddr.sin_zero, '\0', sizeof serverAddr.sin_zero);  

	addr_size = sizeof serverAddr;

	// Start a timer to send heartbeat signal each second
	timer_start(mavSendHeartbeat, 2000);
	// Start a timer to receive messages from the other side each second
	timer_start(mavReceive, 10);
	
	while(1);
}

CMakeLists.txt

cmake_minimum_required (VERSION 2.8)
project (NavControl)

FIND_PACKAGE(Boost COMPONENTS system thread filesystem REQUIRED)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -std=c++11 -pthread -lboost_system")

set( MY_CPP_FILES 
    src/NavControl.cpp
    src/NavControl.h)

add_executable(nav ${MY_CPP_FILES} )

TARGET_LINK_LIBRARIES(nav ${Boost_LIBRARIES})

To build the project

mkdir build
cd build
cmake ..
make
./nav
10

OK, a small amount of hackery required this time to get your example running.

There were a few issues with your example code. I’ve hacked it around a bit and the modified example below quite happily shows a regular heartbeat for a SITL instance running on the same machine. Suggest you use diff to see the changes I’ve made to your NavControl.cpp

#include "NavControl.h"

#include <sys/time.h>

void timer_start(std::function<void(void)> func, unsigned int interval)
{
	std::thread([func, interval]()
			{ 
			while (true)
			{ 
			auto x = std::chrono::steady_clock::now() + std::chrono::milliseconds(interval);
			func();
			std::this_thread::sleep_until(x);
			}
			}).detach();
}

struct sockaddr_in src_addr;

void mavSendHeartbeat(void)
{
    if (src_addr.sin_family == 0) {
        printf("No source address yet\n");
        return;
    }
	mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, MAV_TYPE_ONBOARD_CONTROLLER,
			MAV_AUTOPILOT_INVALID, 0, 0, 0);

	len = mavlink_msg_to_send_buffer(buf, &msg);

	printf("Heartbeat sent\n");

	sendto(clientSocket,buf,len,0,(struct sockaddr *)&src_addr,addr_size);
}

struct timeval last_received;

void heartbeat_received()
{
    struct timeval now;
    gettimeofday(&now, NULL);
    struct timeval delta;
    timersub(&now, &last_received, &delta);
    char ip[INET_ADDRSTRLEN];
    inet_ntop(AF_INET, &(src_addr.sin_addr), ip, sizeof(ip));
    printf("Hearbeat received from (%s:%u)..  (delta=%f)\n", ip,src_addr.sin_port, (delta.tv_sec+delta.tv_usec/1000000.0));
    last_received = now;
}

uint8_t mavReceive(void)
{
    while (true) {
	bufIndex = 0;
	bytesReceived = 0;

        socklen_t socklen = sizeof(sizeof(src_addr));
	bytesReceived = recvfrom(clientSocket, rxBuf, 1024, 0, (struct sockaddr*)&src_addr, &socklen);
	// bytesReceived = recvfrom(clientSocket, rxBuf, 1024, 0, NULL, NULL);

	if(bytesReceived < 0)
	{
		printf("Error receiveing UDP\n");
                continue;
	}

	// Process the received bytes to form a message
	while(bufIndex < bytesReceived)
	{
		ch = rxBuf[bufIndex++];

		if(mavlink_parse_char(MAVLINK_COMM_0, ch, &msg, &status))
		{
			switch(msg.msgid)
			{
				case MAVLINK_MSG_ID_HEARTBEAT:
                                    heartbeat_received();
					break;

				default:
					// printf("Received .. MSG ID = %d\n", msg.msgid);
					break;
			}
		}
	}
    }
	return 1;
}

int main(void)
{
	mavlink_system.sysid = 20; 
	mavlink_system.compid = 20;

	clientSocket = socket(PF_INET, SOCK_DGRAM, 0);

	serverAddr.sin_family = AF_INET;
	serverAddr.sin_port = htons(14551);
	serverAddr.sin_addr.s_addr = inet_addr("127.0.0.1");
	memset(serverAddr.sin_zero, '\0', sizeof serverAddr.sin_zero);  

	addr_size = sizeof serverAddr;

        if (bind(clientSocket, (struct sockaddr*)&serverAddr, addr_size) < 0) {
            printf("Bind failed: %m");
            exit(1);
        }

        gettimeofday(&last_received, NULL);

	// Start a timer to send heartbeat signal each second
	timer_start(mavSendHeartbeat, 1000);
	// Start a timer to receive messages from the other side each second
	timer_start(mavReceive, 10);
	
	while(1);
}

Note that this is just hackery to get something that works :slight_smile:

11

Many thanks Peter for your time. I really appreciate your help