diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-11-24 07:41:26 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-11-24 07:41:26 +0100 |
commit | c37ff71e625310cdc777719a04c3702d9afa1f7f (patch) | |
tree | 714ecb7b510e7ff53080ce3f0caebe8b128a26f5 /src/modules/mavlink/mavlink_main.cpp | |
parent | f36f54c621cb5b36d345c5a26f72e89fc948fa65 (diff) | |
parent | 512779907e06f059a15d54c88d71b73aad9aced0 (diff) | |
download | px4-firmware-c37ff71e625310cdc777719a04c3702d9afa1f7f.tar.gz px4-firmware-c37ff71e625310cdc777719a04c3702d9afa1f7f.tar.bz2 px4-firmware-c37ff71e625310cdc777719a04c3702d9afa1f7f.zip |
Merge remote-tracking branch 'upstream/master' into ros
Conflicts:
makefiles/config_px4fmu-v2_test.mk
Diffstat (limited to 'src/modules/mavlink/mavlink_main.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 14 |
1 files changed, 8 insertions, 6 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 6b4edff78..fb9f65cf5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -167,8 +167,10 @@ Mavlink::Mavlink() : _param_initialized(false), _param_system_id(0), _param_component_id(0), - _param_system_type(0), + _param_system_type(MAV_TYPE_FIXED_WING), _param_use_hil_gps(0), + _param_forward_externalsp(0), + _system_type(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void) param_get(_param_system_type, &system_type); if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) { - mavlink_system.type = system_type; + _system_type = system_type; } int32_t use_hil_gps; @@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); uint8_t payload_len = mavlink_message_lengths[msgid]; unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES; @@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg) pthread_mutex_lock(&_send_mutex); - int buf_free = get_free_tx_buf(); + unsigned buf_free = get_free_tx_buf(); _last_write_try_time = hrt_absolute_time(); unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; /* check if there is space in the buffer, let it overflow else */ - if (buf_free < TX_BUFFER_GAP) { + if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) { /* no enough space in buffer to send */ count_txerr(); count_txerrbytes(packet_len); @@ -1634,7 +1636,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2900, + 2800, (main_t)&Mavlink::start_helper, (const char **)argv); |