static void *heartbeatloop(void * arg)
{
/* initialize waypoint manager */
mavlink_wpm_init(&wpm);
/* initialize parameter storage */
mavlink_pm_reset_params(&pm);
int lowspeed_counter = 0;
int result_sys_status_trylock;
while(1) {
// sleep
usleep(50000);
// 1 Hz
if (lowspeed_counter == 10)
{
/* Send heartbeat */
mavlink_msg_heartbeat_send(chan,system_type,MAV_AUTOPILOT_GENERIC,MAV_MODE_PREFLIGHT,custom_mode,MAV_STATE_UNINIT);
/* Send status */
result_sys_status_trylock = global_data_trylock(&global_data_sys_status.access_conf);
if(0 == result_sys_status_trylock) mavlink_msg_sys_status_send(chan, global_data_sys_status.onboard_control_sensors_present, global_data_sys_status.onboard_control_sensors_enabled, global_data_sys_status.onboard_control_sensors_health, global_data_sys_status.load, global_data_sys_status.voltage_battery, global_data_sys_status.current_battery, global_data_sys_status.battery_remaining, global_data_sys_status.drop_rate_comm, global_data_sys_status.errors_comm, global_data_sys_status.errors_count1, global_data_sys_status.errors_count2, global_data_sys_status.errors_count3, global_data_sys_status.errors_count4);
global_data_unlock(&global_data_sys_status.access_conf);
if (!(mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED)) {
// System is not armed, blink at 1 Hz
led_toggle(LED_BLUE);
}
lowspeed_counter = 0;
}
lowspeed_counter++;
// Send one parameter
mavlink_pm_queued_send();
if (mavlink_system.mode & MAV_MODE_FLAG_SAFETY_ARMED) {
// System is armed, blink at 10 Hz
led_toggle(LED_BLUE);
}
usleep(50000);
}
}
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();
}
}
