diff options
author | Julian Oes <julian@oes.ch> | 2014-04-26 23:08:11 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-04-26 23:08:11 +0200 |
commit | e8531e8360e4f061f3cd69db90365f64837a7c76 (patch) | |
tree | fda98a4bf7bd67f339757f921a01976b45e37236 /src/modules/mavlink | |
parent | 3a12cb46487980dbf85f4606e316d9643a2b3b23 (diff) | |
parent | 13dfe0447ccfa4f75b551d02b5c979a6ade4c81a (diff) | |
download | px4-firmware-e8531e8360e4f061f3cd69db90365f64837a7c76.tar.gz px4-firmware-e8531e8360e4f061f3cd69db90365f64837a7c76.tar.bz2 px4-firmware-e8531e8360e4f061f3cd69db90365f64837a7c76.zip |
Merge remote-tracking branch 'px4/ekf_home_init' into navigator_cleanup_ekf_home_init
Conflicts:
src/modules/commander/commander.cpp
src/modules/mc_pos_control/mc_pos_control_main.cpp
src/modules/navigator/navigator_main.cpp
src/modules/uORB/topics/vehicle_global_position.h
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/mavlink_main.cpp | 13 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 15 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.h | 1 |
3 files changed, 22 insertions, 7 deletions
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 1ba3d70c6..be4114e73 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { - ssize_t ret = write(uart, ch, desired); + + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (desired > buf_free) { + desired = buf_free; + } + } + + ssize_t ret = write(uart, ch, desired); if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + warnx("TX FAIL"); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 22cf9d7a6..33a4fef12 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -257,6 +257,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) memset(&f, 0, sizeof(f)); f.timestamp = hrt_absolute_time(); + f.flow_timestamp = flow.time_usec; f.flow_raw_x = flow.flow_x; f.flow_raw_y = flow.flow_y; f.flow_comp_x_m = flow.flow_comp_m_x; @@ -751,7 +752,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -759,6 +759,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; if (_global_pos_pub < 0) { _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); @@ -770,19 +772,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* local position */ { + double lat = hil_state.lat * 1e-7; + double lon = hil_state.lon * 1e-7; + if (!_hil_local_proj_inited) { _hil_local_proj_inited = true; _hil_local_alt0 = hil_state.alt / 1000.0f; - map_projection_init(hil_state.lat, hil_state.lon); + map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon); hil_local_pos.ref_timestamp = timestamp; - hil_local_pos.ref_lat = hil_state.lat; - hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_lat = lat; + hil_local_pos.ref_lon = lon; hil_local_pos.ref_alt = _hil_local_alt0; } float x; float y; - map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y); + map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y); hil_local_pos.timestamp = timestamp; hil_local_pos.xy_valid = true; hil_local_pos.z_valid = true; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 72ce4560f..9ab84b58a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -142,4 +142,5 @@ private: uint64_t _old_timestamp; bool _hil_local_proj_inited; float _hil_local_alt0; + struct map_projection_reference_s _hil_local_proj_ref; }; |