diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-11 08:20:31 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-06-11 08:20:31 +0200 |
commit | 251760679a4bed86fdb10746062fde2cc23e460f (patch) | |
tree | 6cbe632887fa0cfa83d4d2fa370cf5787a84d77b | |
parent | 92766a8626fdf143e46159d9a7e367ade6ae4376 (diff) | |
parent | 064a75a3c289a32fa4d5234e3874712e7981f238 (diff) | |
download | px4-firmware-251760679a4bed86fdb10746062fde2cc23e460f.tar.gz px4-firmware-251760679a4bed86fdb10746062fde2cc23e460f.tar.bz2 px4-firmware-251760679a4bed86fdb10746062fde2cc23e460f.zip |
Merge pull request #1054 from PX4/hil_pos_sp
Enable global position setpoint over USB
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.usb | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 16 |
2 files changed, 10 insertions, 8 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 76593881d..d6e638c0a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -17,6 +17,8 @@ mavlink stream -d /dev/ttyACM0 -s ATTITUDE_CONTROLS -r 30 usleep 100000 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 usleep 100000 +mavlink stream -d /dev/ttyACM0 -s GLOBAL_POSITION_SETPOINT_INT -r 20 +usleep 100000 # Exit shell to make it available to MAVLink exit diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index fd41b723a..d94e7fc7e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -938,14 +938,14 @@ protected: void send(const hrt_abstime t) { - if (pos_sp_triplet_sub->update(t)) { - mavlink_msg_global_position_setpoint_int_send(_channel, - MAV_FRAME_GLOBAL, - (int32_t)(pos_sp_triplet->current.lat * 1e7), - (int32_t)(pos_sp_triplet->current.lon * 1e7), - (int32_t)(pos_sp_triplet->current.alt * 1000), - (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); - } + /* always send this message, even if it has not been updated */ + pos_sp_triplet_sub->update(t); + mavlink_msg_global_position_setpoint_int_send(_channel, + MAV_FRAME_GLOBAL, + (int32_t)(pos_sp_triplet->current.lat * 1e7), + (int32_t)(pos_sp_triplet->current.lon * 1e7), + (int32_t)(pos_sp_triplet->current.alt * 1000), + (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f)); } }; |