aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink
diff options
context:
space:
mode:
authorThomas Gubler <thomasgubler@gmail.com>2015-01-21 14:41:03 +0100
committerThomas Gubler <thomasgubler@gmail.com>2015-01-21 14:41:03 +0100
commitdcdde8ea88bf57b0432d2b64ed3ce606795c8d00 (patch)
tree0bf6e997ffe7f7a9cda98c832dbce39601a9242f /src/modules/mavlink
parent3a38b0fe359296aa19ec43ab82743aebeadb335c (diff)
parente8e4a3b5da058bd2ab8575c095dd74a5484333be (diff)
downloadpx4-firmware-dcdde8ea88bf57b0432d2b64ed3ce606795c8d00.tar.gz
px4-firmware-dcdde8ea88bf57b0432d2b64ed3ce606795c8d00.tar.bz2
px4-firmware-dcdde8ea88bf57b0432d2b64ed3ce606795c8d00.zip
Merge remote-tracking branch 'upstream/master' into ros_messagelayer_merge
Conflicts: src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp src/modules/uORB/topics/vehicle_attitude.h
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp21
-rw-r--r--src/modules/mavlink/mavlink_receiver.h3
2 files changed, 21 insertions, 3 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index ce3f2ae0e..f4e3dc441 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;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 4afc9b372..699996860 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -50,6 +50,7 @@
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/offboard_control_setpoint.h>
@@ -145,6 +146,7 @@ private:
mavlink_status_t status;
struct vehicle_local_position_s hil_local_pos;
+ struct vehicle_land_detected_s hil_land_detector;
struct vehicle_control_mode_s _control_mode;
orb_advert_t _global_pos_pub;
orb_advert_t _local_pos_pub;
@@ -171,6 +173,7 @@ private:
orb_advert_t _telemetry_status_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
+ orb_advert_t _land_detector_pub;
int _control_mode_sub;
int _hil_frames;
uint64_t _old_timestamp;