diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
| -rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 21 |
1 files changed, 18 insertions, 3 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 97108270c..dfbf00b66 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -92,6 +92,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), status{}, hil_local_pos{}, + hil_land_detector{}, _control_mode{}, _global_pos_pub(-1), _local_pos_pub(-1), @@ -118,6 +119,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _land_detector_pub(-1), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -1353,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_local_pos.xy_global = true; hil_local_pos.z_global = true; - bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? - hil_local_pos.landed = landed; - if (_local_pos_pub < 0) { _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); @@ -1364,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } + /* land detector */ + { + bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve? + if(hil_land_detector.landed != landed) { + hil_land_detector.landed = landed; + hil_land_detector.timestamp = hrt_absolute_time(); + + if (_land_detector_pub < 0) { + _land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector); + + } else { + orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector); + } + } + } + /* accelerometer */ { struct accel_report accel; |