Hello,
i have a Raspberry PI on my Quadrocopter . The connection between the Raspberry PI and the Ardupilot works fine. I get the Heardbeat and a lot of other informations. Now i try to send a ping request just to check, if i can speak with the ardupilot, but it doesn´t ping back.
Can i choose the System ID and Component ID of the Rasperry PI or where can i figure out the right numbers?
I use the mavlink header libraries.
This is the sector where i try to send the ping. Below is my complete code.
//sending the ping
mavlink_msg_ping_pack(mavlink_system.sysid, mavlink_system.compid, &msg, 93372036854775807ULL, 963497880, received_sysid, received_compid);
len = mavlink_msg_to_send_buffer(buffer, &msg);
sendport(len);
My Code:
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <termios.h>
#include <stdio.h>
#include </DIST/home/burghart/Mavlink/mavlink/include/ardupilotmega/mavlink.h>
#define BAUDRATE B57600
#define MODEMDEVICE "/dev/ttyUSB0"
#define _POSIX_SOURCE 1 /* POSIX compliant source */
#define FALSE 0
#define TRUE 1
//Initial the required buffers
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Define the system type, in this case an airplane
int system_type = MAV_TYPE_QUADROTOR;
int autopilot_type = MAV_AUTOPILOT_GENERIC;
//Define the type of my PC {id,comp,nc,nc}
mavlink_system_t mavlink_system = {7,1,0,0};
//Declaration
void openport(void);
void sendport(int len);
int fd,k,res;
uint16_t len;
struct termios oldtp, newtp;
uint8_t buffer[2041];
mavlink_message_t msg;
mavlink_sys_status_t mavlink_sys_status;
mavlink_status_t status;
volatile int STOP=FALSE;
main(int argc, char **argv)
{
openport(); // opens ttyUSB0
while (STOP==FALSE) { /* loop for input */
res = read(fd,buffer,255);
for (k=0; k<res;k++) {
if(mavlink_parse_char(MAVLINK_COMM_0, buffer[k], &msg, &status))
{
printf("\nmsgid: %d \n\n",msg.msgid);
switch(msg.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
{
// save the sysid and compid of the received heartbeat from the ARDUPILOT for use in sending new messages
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(&msg, &packet);
uint8_t received_sysid = msg.sysid;
uint8_t received_compid = msg.compid;
//sending the heartbeat
mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, system_type, autopilot_type, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buffer, &msg);
sendport(len);
//sending the ping
mavlink_msg_ping_pack(mavlink_system.sysid, mavlink_system.compid, &msg, 93372036854775807ULL, 963497880, received_sysid, received_compid);
len = mavlink_msg_to_send_buffer(buffer, &msg);
sendport(len);
}
break;
case MAVLINK_MSG_ID_PING:
{
printf("It works\n");
sleep(2);
}
break;
default:
//Do nothing
break;
}
}
}
}
}
void sendport(int len)
{
int n;
newtp.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
n = write(fd, buffer, len);
write(fd,"\n",1);
if (n != len)
fputs("write() of bytes failed!\n", stderr);
else
printf("Image sent successfully %d byte.\n",n);
}
void openport(void)
{
fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY);
if (fd <0)
perror(MODEMDEVICE);
fcntl(fd,F_SETFL,0);
tcgetattr(fd,&oldtp); // save current serial port settings
tcgetattr(fd,&newtp); // save current serial port settings
bzero(&newtp, sizeof(newtp)); // place sizeof(newtp) zero-valued bytes in the area pointed to by newtp.
bzero(&oldtp, sizeof(oldtp)); // place sizeof(oldtp) zero-valued bytes in the area pointed to by oldtp.
newtp.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
newtp.c_iflag = IGNPAR | ICRNL;
newtp.c_lflag = ICANON;
newtp.c_cc[VMIN] = 1; /* blocking read until 1 character arrives */
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtp);
}
Thanks for helping!
Tobi