From 4a84215a8f73a307d38621ed8e8e7e8cd3421d3d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Fri, 15 May 2015 17:49:20 +0200 Subject: [PATCH] fix mavlink message sending, make thread priority default --- src/modules/simulator/simulator.h | 2 +- src/modules/simulator/simulator_mavlink.cpp | 23 +++++++++------------ 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 93f5327e83..60b60f72f8 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -223,7 +223,7 @@ private: void poll_topics(); void handle_message(mavlink_message_t *msg); void send_data(); - void pack_actuator_message(mavlink_message_t *msg); + void pack_actuator_message(mavlink_hil_controls_t &actuator_msg); void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID); static void *sending_trampoline(void *); void send(); diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index d468dd6661..9b6efb5102 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -46,7 +46,7 @@ using namespace simulator; static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -void Simulator::pack_actuator_message(mavlink_message_t *msg) { +void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) { float out[8]; const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; @@ -71,8 +71,6 @@ void Simulator::pack_actuator_message(mavlink_message_t *msg) { } } - mavlink_hil_controls_t actuator_msg; - actuator_msg.time_usec = hrt_absolute_time(); actuator_msg.roll_ailerons = out[0]; actuator_msg.pitch_elevator = out[1]; @@ -84,18 +82,15 @@ void Simulator::pack_actuator_message(mavlink_message_t *msg) { actuator_msg.aux4 = out[7]; actuator_msg.mode = 0; // need to put something here actuator_msg.nav_mode = 0; - - // encode the message - mavlink_msg_hil_controls_encode(1, MAVLINK_MSG_ID_HIL_CONTROLS, msg, &actuator_msg); } void Simulator::send_data() { // check if it's time to send new data hrt_abstime time_now = hrt_absolute_time(); - if (time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { + if (true) {//time_now - _time_last >= (hrt_abstime)(SEND_INTERVAL * 1000)) { _time_last = time_now; - mavlink_message_t msg; - pack_actuator_message(&msg); + mavlink_hil_controls_t msg; + pack_actuator_message(msg); send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200); // can add more messages here, can also setup different timings } @@ -186,7 +181,9 @@ void Simulator::handle_message(mavlink_message_t *msg) { } void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) { + component_ID = 0; uint8_t payload_len = mavlink_message_lengths[msgid]; + unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; uint8_t buf[MAVLINK_MAX_PACKET_LEN]; @@ -195,12 +192,12 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 buf[1] = payload_len; /* no idea which numbers should be here*/ buf[2] = 100; - buf[3] = 1; + buf[3] = 0; buf[4] = component_ID; buf[5] = msgid; /* payload */ - memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],&msg, payload_len); + memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len); /* checksum */ uint16_t checksum; @@ -211,7 +208,7 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8 buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF); buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8); - ssize_t len = sendto(_fd, buf, sizeof(buf), 0, (struct sockaddr *)&_srcaddr, _addrlen); + ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen); if (len <= 0) { PX4_WARN("Failed sending mavlink message"); } @@ -329,7 +326,7 @@ void Simulator::updateSamples() (void)pthread_attr_getschedparam(&sender_thread_attr, ¶m); /* low priority */ - param.sched_priority = SCHED_PRIORITY_DEFAULT - 30; + param.sched_priority = SCHED_PRIORITY_DEFAULT; (void)pthread_attr_setschedparam(&sender_thread_attr, ¶m); pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL); pthread_attr_destroy(&sender_thread_attr);