dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge pull request #1297 from PX4/mission_fix 

mavlink: missions manager compID cleanup
This commit is contained in:
Lorenz Meier 2014-08-21 14:40:34 +02:00
parent 29dba8dff3 9c75b9562e
commit aa8fcceea2
4 changed files with 21 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/mavlink/mavlink_main.cpp
View File

@ -1229,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[])
_mode = MAVLINK_MODE_CUSTOM;
} else if (strcmp(optarg, "camera") == 0) {
_mode = MAVLINK_MODE_CAMERA;
// left in here for compatibility
_mode = MAVLINK_MODE_ONBOARD;
} else if (strcmp(optarg, "onboard") == 0) {
_mode = MAVLINK_MODE_ONBOARD;
}
break;
@ -1289,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[])
warnx("mode: CUSTOM");
break;
case MAVLINK_MODE_CAMERA:
warnx("mode: CAMERA");
case MAVLINK_MODE_ONBOARD:
warnx("mode: ONBOARD");
break;
default:
@ -1393,9 +1396,10 @@ Mavlink::task_main(int argc, char *argv[])
configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
configure_stream("ATTITUDE_TARGET", 3.0f);
configure_stream("DISTANCE_SENSOR", 0.5f);
configure_stream("OPTICAL_FLOW", 0.5f);
break;
case MAVLINK_MODE_CAMERA:
case MAVLINK_MODE_ONBOARD:
configure_stream("SYS_STATUS", 1.0f);
configure_stream("ATTITUDE", 15.0f);
configure_stream("GLOBAL_POSITION_INT", 15.0f);

2
src/modules/mavlink/mavlink_main.h
View File

@ -127,7 +127,7 @@ public:
enum MAVLINK_MODE {
MAVLINK_MODE_NORMAL = 0,
MAVLINK_MODE_CUSTOM,
MAVLINK_MODE_CAMERA
MAVLINK_MODE_ONBOARD
};
void set_mode(enum MAVLINK_MODE);

21
src/modules/mavlink/mavlink_mission.cpp
View File

@ -58,6 +58,10 @@
#endif
static const int ERROR = -1;
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
((_msg.target_component == mavlink_system.compid) || \
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
(_msg.target_component == MAV_COMP_ID_ALL)))
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink),
_state(MAVLINK_WPM_STATE_IDLE),
@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_mission_result_sub(-1),
_offboard_mission_pub(-1),
_slow_rate_limiter(_interval / 10.0f),
_verbose(false),
_comp_id(MAV_COMP_ID_MISSIONPLANNER)
_verbose(false)
{
_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg)
mavlink_mission_ack_t wpa;
mavlink_msg_mission_ack_decode(msg, &wpa);
if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpa)) {
if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
mavlink_mission_set_current_t wpc;
mavlink_msg_mission_set_current_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
mavlink_mission_request_list_t wprl;
mavlink_msg_mission_request_list_decode(msg, &wprl);
if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wprl)) {
if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg)
mavlink_mission_request_t wpr;
mavlink_msg_mission_request_decode(msg, &wpr);
if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpr)) {
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
_time_last_recv = hrt_absolute_time();
@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
mavlink_mission_count_t wpc;
mavlink_msg_mission_count_decode(msg, &wpc);
if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) {
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
mavlink_mission_item_t wp;
mavlink_msg_mission_item_decode(msg, &wp);
if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) {
if (CHECK_SYSID_COMPID_MISSION(wp)) {
if (_state == MAVLINK_WPM_STATE_GETLIST) {
_time_last_recv = hrt_absolute_time();
@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
mavlink_mission_clear_all_t wpca;
mavlink_msg_mission_clear_all_decode(msg, &wpca);
if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) {
if (CHECK_SYSID_COMPID_MISSION(wpca)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
/* don't touch mission items storage itself, but only items count in mission state */

2
src/modules/mavlink/mavlink_mission.h
View File

@ -126,8 +126,6 @@ private:
bool _verbose;
uint8_t _comp_id;
/* do not allow top copying this class */
MavlinkMissionManager(MavlinkMissionManager &);
MavlinkMissionManager& operator = (const MavlinkMissionManager &);