dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fix mavlink message sending, make thread priority default

This commit is contained in:
Roman Bapst 2015-05-15 17:49:20 +02:00
parent a3a0d0612c
commit 4a84215a8f
2 changed files with 11 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/simulator/simulator.h
View File

@ -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();

23
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 30;
param.sched_priority = SCHED_PRIORITY_DEFAULT;
(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
pthread_attr_destroy(&sender_thread_attr);