fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 07:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'navigator_rewrite' into navigator_rewrite_estimator

Browse Source
This commit is contained in:
Julian Oes
2014-06-27 14:41:19 +02:00
parent dbd99b6494 cc8f7f4c97
commit b8d07532a7
7 changed files with 59 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+14 -12
View File
@@ -258,19 +258,21 @@ protected:
struct vehicle_status_s status;
struct position_setpoint_triplet_s pos_sp_triplet;
if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) {
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
/* always send the heartbeat, independent of the update status of the topics */
(void)status_sub->update(&status);
(void)pos_sp_triplet_sub->update(&pos_sp_triplet);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
MAV_AUTOPILOT_PX4,
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
}
};
src/modules/mavlink/mavlink_orb_subscription.cpp
+13 -17
View File
@@ -44,8 +44,6 @@
#include <uORB/uORB.h>
#include <stdio.h>
#include <systemlib/err.h>
#include "mavlink_orb_subscription.h"
MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
@@ -79,23 +77,21 @@ MavlinkOrbSubscription::update(uint64_t *time, void* data)
time_topic = 0;
}
if (time_topic != *time) {
if (orb_copy(_topic, _fd, data)) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
//warnx("err copy, fd: %d, obj: %s, size: %d", _fd, _topic->o_name, _topic->o_size);
return false;
} else {
/* data copied successfully */
_published = true;
*time = time_topic;
return true;
}
if (orb_copy(_topic, _fd, data)) {
/* error copying topic data */
memset(data, 0, _topic->o_size);
return false;
} else {
return false;
/* data copied successfully */
_published = true;
if (time_topic != *time) {
*time = time_topic;
return true;
} else {
return false;
}
}
}