diff options
author | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-06 13:56:06 +0100 |
---|---|---|
committer | Johan Jansen <jnsn.johan@gmail.com> | 2015-01-15 14:37:51 +0100 |
commit | 57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e (patch) | |
tree | 046c63b3faa885a9693796df23ce8236efc5a9c0 /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 546b5727b442ac7520d7ce72e15732378a1a0799 (diff) | |
download | px4-firmware-57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e.tar.gz px4-firmware-57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e.tar.bz2 px4-firmware-57ed27304a394fb9fec8e2ae4bfca9b2a77d6c7e.zip |
HIL: Added land detector to HIL simulation
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; |