int main(int argc, char* argv[]) {
char help[] = "--help";
char target_ip[100];
float position[6] = {};
int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
//struct sockaddr_in fromAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen;
int bytes_sent;
mavlink_message_t msg;
uint16_t len;
int i = 0;
//int success = 0;
unsigned int temp = 0;
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0)) {
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2) {
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) {
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) {
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
for (;;) {
printf("heartbeat\n");
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(SYSID, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(SYSID, 200, &msg, 0, 0, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(SYSID, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(SYSID, 200, &msg, microsSinceEpoch(), toRad(1.2), toRad(1.7), toRad(3.14), 0.01, 0.02, 0.03);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0) {
printf("Something received\n");
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i) {
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) {
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
sleep(1); // Sleep one second
}
}
void *MavlinkStatusTask(void *ptr) {
MavlinkStruct *Mavlink = (MavlinkStruct *) ptr;
AttitudeData *AttitudeDesire = Control.AttitudeDesire;
AttitudeData *AttitudeMesure = Mavlink->AttitudeMesure;
AttData Data, Speed;
AttData DataD, SpeedD;
AttData DataM, SpeedM;
double Error[4] = {0.0, 0.0, 0.0, 0.0};
uint32_t TimeStamp;
mavlink_message_t msg;
uint16_t len;
uint8_t buf[BUFFER_LENGTH];
int bytes_sent;
uint32_t sensor = 0xf00f;
printf("%s : Mavlink Status démarré\n", __FUNCTION__);
pthread_barrier_wait(&(MavlinkStartBarrier));
while (MavlinkActivated) {
sem_wait(&MavlinkStatusTimerSem);
if (MavlinkActivated == 0)
break;
memset(buf, 0, BUFFER_LENGTH);
pthread_spin_lock(&(AttitudeMesure->AttitudeLock));
memcpy((void *) &Data, (void *) &(AttitudeMesure->Data), sizeof(AttData));
memcpy((void *) &Speed, (void *) &(AttitudeMesure->Speed), sizeof(AttData));
TimeStamp = AttitudeMesure->timestamp_s*1000 + AttitudeMesure->timestamp_n/1000000L;
pthread_spin_unlock(&(AttitudeMesure->AttitudeLock));
//Send Heartbeat
mavlink_msg_heartbeat_pack(SYSTEM_ID, COMPONENT_ID, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in));
// Send Status
mavlink_msg_sys_status_pack(SYSTEM_ID, COMPONENT_ID, &msg, sensor, sensor, 0, 500, 11000, -1, -1, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof (struct sockaddr_in));
// Send Local Position
mavlink_msg_local_position_ned_pack(SYSTEM_ID, COMPONENT_ID, &msg, TimeStamp, 0, 0, (float) Data.Elevation, 0, 0, (float) Speed.Elevation);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in));
pthread_spin_lock(&(AttitudeDesire->AttitudeLock));
memcpy((void *) &DataD, (void *) &(AttitudeDesire->Data), sizeof(AttData));
memcpy((void *) &SpeedD, (void *) &(AttitudeDesire->Speed), sizeof(AttData));
pthread_spin_unlock(&(AttitudeDesire->AttitudeLock));
pthread_spin_lock(&(AttitudeMesure->AttitudeLock));
memcpy((void *) &DataM, (void *) &(AttitudeMesure->Data), sizeof(AttData));
memcpy((void *) &SpeedM, (void *) &(AttitudeMesure->Speed), sizeof(AttData));
pthread_spin_unlock(&(AttitudeMesure->AttitudeLock));
Error[HEIGHT] = DataD.Elevation - DataM.Elevation;
Error[ROLL] = DataD.Roll - DataM.Roll;
Error[PITCH] = DataD.Pitch - DataM.Pitch;
Error[YAW] = DataD.Yaw - DataM.Yaw;
// Send Attitude
mavlink_msg_attitude_pack(SYSTEM_ID, COMPONENT_ID, &msg, TimeStamp, (float) Data.Roll, (float) Data.Pitch, (float) Data.Yaw, (float) Speed.Roll, (float) Speed.Pitch, (float) Speed.Yaw);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(Mavlink->sock, buf, len, 0, (struct sockaddr *)&Mavlink->gcAddr, sizeof(struct sockaddr_in));
}
printf("%s : Mavlink Status Arrêté\n", __FUNCTION__);
pthread_exit(NULL);
}
struct mavlink_drone *mavlink_drone_new(struct mavlink_drone_cfg *cfg)
{
struct mavlink_drone *c;
struct epoll_event ev;
struct epoll_event events[3];
struct itimerspec timeout;
struct mavlink_comm_cfg comm_cfg = { .local_port = 14551,
.remote_port = 14550,
.cb = &callback};
if (!cfg)
return NULL;
c = calloc(1, sizeof(struct mavlink_drone));
if (!c)
return NULL;
c->remote_system_id = cfg->remote_system_id;
c->remote_component_id = cfg->remote_component_id;
c->update_status_cb = cfg->update_status_cb;
comm_cfg.remote_addr = NULL;
c->comm = mavlink_comm_new(&comm_cfg);
if (!c->comm) {
printf("bad init comm\n");
goto error;
}
c->exitfd = eventfd(0,0);
bzero(&timeout, sizeof(timeout));
timeout.it_value.tv_sec = 1;
timeout.it_value.tv_nsec = 0;
timeout.it_interval.tv_sec = 1;
timeout.it_interval.tv_nsec = 0;
c->fd_1hz = timerfd_init(&timeout);
if (c->fd_1hz < 0) {
printf("Failed to create timerfd 1hz\n");
goto error;
}
printf("size %lu\n", sizeof(events) / sizeof(events[0]));
c->epollfd = epoll_create(sizeof(events) / sizeof(events[0]));
if (c->epollfd < 0) {
printf("epoll_create failed\n");
goto error;
}
ev.events = EPOLLIN;
ev.data.fd = c->fd_1hz;
if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, c->fd_1hz, &ev) == -1) {
printf("failed to add fd_1hz\n");
goto error;
}
ev.events = EPOLLIN;
ev.data.fd = mavlink_comm_get_sockfd(c->comm);
if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, ev.data.fd, &ev) == -1) {
printf ("failed to add sockfd\n");
goto error;
}
ev.events = EPOLLIN;
ev.data.fd = c->exitfd;
if (epoll_ctl(c->epollfd, EPOLL_CTL_ADD, c->exitfd, &ev) == -1) {
printf("failed to add eventfd\n");
goto error;
}
return c;
error:
if (c->comm)
mavlink_comm_destroy(c->comm);
if (c->fd_1hz > 0)
close(c->fd_1hz);
if (c->epollfd > 0)
close(c->epollfd);
free(c);
return NULL;
}
void mavlink_drone_destroy(struct mavlink_drone *c)
{
if (!c)
return;
if (c->epollfd > 0)
close(c->epollfd);
free(c);
}
static void mavlink_drone_send_status(struct mavlink_drone *ctrl)
{
mavlink_message_t msg;
struct mavlink_info_cache *c = &ctrl->cache;
int ret;
if (ctrl->update_status_cb)
(ctrl->update_status_cb)(c);
mavlink_msg_heartbeat_pack(ctrl->remote_system_id,
ctrl->remote_component_id,
&msg, c->hb.type, c->hb.autopilot, c->hb.base_mode,
0, c->hb.system_status);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
/* Send Status */
mavlink_msg_sys_status_pack(ctrl->remote_system_id,
ctrl->remote_component_id,
&msg, c->sys_stat.onboard_control_sensors_present,
c->sys_stat.onboard_control_sensors_enabled,
c->sys_stat.onboard_control_sensors_health,
c->sys_stat.load,
c->sys_stat.voltage_battery,
c->sys_stat.current_battery,
c->sys_stat.drop_rate_comm,
c->sys_stat.errors_comm,
c->sys_stat.errors_count1,
c->sys_stat.errors_count2,
c->sys_stat.errors_count3,
c->sys_stat.errors_count4,
c->sys_stat.battery_remaining);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
/* Send Local Position */
mavlink_msg_local_position_ned_pack(ctrl->remote_system_id,
ctrl->remote_component_id, &msg, c->lpn.time_boot_ms,
c->lpn.x, c->lpn.y, c->lpn.z,
c->lpn.vx, c->lpn.vy, c->lpn.vz);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
/* Send attitude */
mavlink_msg_attitude_pack(ctrl->remote_system_id,
ctrl->remote_component_id,
&msg, c->att.time_boot_ms, c->att.roll, c->att.pitch,
c->att.yaw, c->att.rollspeed, c->att.pitchspeed,
c->att.yawspeed);
ret = mavlink_comm_send_msg(ctrl->comm, &msg);
if (ret < 0)
printf("%d error %d\n", __LINE__, ret);
}
int main(int argc, char* argv[])
{
// Initialize MAVLink
mavlink_wpm_init(&wpm);
mavlink_system.sysid = 5;
mavlink_system.compid = 20;
mavlink_pm_reset_params(&pm);
int32_t ground_distance;
uint32_t time_ms;
// Create socket
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
for (;;)
{
bytes_sent = 0;
/* Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE, 500, 7500, 0, 0, 0, 0, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_ned_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send global position */
if (hilEnabled)
{
mavlink_msg_global_position_int_pack(mavlink_system.sysid, 200, &msg, time_ms, latitude, longitude, altitude, ground_distance, speedx, speedy, speedz, (yaw/M_PI)*180*100);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
}
/* Send attitude */
mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send HIL outputs */
float roll_ailerons = 0; // -1 .. 1
float pitch_elevator = 0.2; // -1 .. 1
float yaw_rudder = 0.1; // -1 .. 1
float throttle = 0.9; // 0 .. 1
mavlink_msg_hil_controls_pack_chan(mavlink_system.sysid, mavlink_system.compid, MAVLINK_COMM_0, &msg, microsSinceEpoch(), roll_ailerons, pitch_elevator, yaw_rudder, throttle, mavlink_system.mode, mavlink_system.nav_mode, 0, 0, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent += sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
// Handle packet with waypoint component
mavlink_wpm_message_handler(&msg);
// Handle packet with parameter component
mavlink_pm_message_handler(MAVLINK_COMM_0, &msg);
// Print HIL values sent to system
if (msg.msgid == MAVLINK_MSG_ID_HIL_STATE)
{
mavlink_hil_state_t hil;
mavlink_msg_hil_state_decode(&msg, &hil);
printf("Received HIL state:\n");
printf("R: %f P: %f Y: %f\n", hil.roll, hil.pitch, hil.yaw);
roll = hil.roll;
pitch = hil.pitch;
yaw = hil.yaw;
rollspeed = hil.rollspeed;
pitchspeed = hil.pitchspeed;
yawspeed = hil.yawspeed;
speedx = hil.vx;
speedy = hil.vy;
speedz = hil.vz;
latitude = hil.lat;
longitude = hil.lon;
altitude = hil.alt;
hilEnabled = true;
}
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
usleep(10000); // Sleep 10 ms
// Send one parameter
mavlink_pm_queued_send();
}
}
