diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-01 14:00:54 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-01 14:00:54 +0200 |
commit | 4b9f9281f5d2b425f0de83801eb38224395c0f12 (patch) | |
tree | b0a5332543de1e6f4d46901a83c85bfebf6b4795 /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 5c6a5411288ea181503269a711de2e626ddb5185 (diff) | |
parent | 8f957aeb5f8f7527d1ef2a5583f4de7870c60513 (diff) | |
download | px4-firmware-4b9f9281f5d2b425f0de83801eb38224395c0f12.tar.gz px4-firmware-4b9f9281f5d2b425f0de83801eb38224395c0f12.tar.bz2 px4-firmware-4b9f9281f5d2b425f0de83801eb38224395c0f12.zip |
Merged master into vision_estimate
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 76 |
1 files changed, 72 insertions, 4 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6b73a4bfa..77226cd20 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -107,12 +107,17 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _telemetry_heartbeat_time(0), + _radio_status_available(false), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0) { memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + + // make sure the FTP server is started + (void)MavlinkFTP::getServer(); } MavlinkReceiver::~MavlinkReceiver() @@ -155,6 +160,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + MavlinkFTP::getServer()->handle_message(_mavlink, msg); + break; + default: break; } @@ -455,6 +472,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) memset(&tstatus, 0, sizeof(tstatus)); tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -470,6 +488,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } + + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } void @@ -496,6 +517,54 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); + + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } + } +} + +void +MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) +{ + mavlink_request_data_stream_t req; + mavlink_msg_request_data_stream_decode(msg, &req); + + if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) { + float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_mavlink->get_streams(), stream) { + if (req.req_stream_id == stream->get_id()) { + _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + } + } + } +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; @@ -711,12 +780,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s @@ -996,7 +1065,6 @@ MavlinkReceiver::receive_start(Mavlink *parent) (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2900); - pthread_t thread; pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent); |