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

mavlink/mavlink_mission: Dirty fix for setting waypoints over multiple mavlink link

This commit is contained in:
oberion 2015-08-05 16:14:17 +02:00
parent da4d8a5c2b
commit a985b37c80
4 changed files with 57 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitmodules vendored
View File

@ -4,7 +4,7 @@
[submodule "NuttX"]
path = NuttX
url = git://github.com/PX4/NuttX.git
[submodule "libuavcan"]
[submodule "src/lib/uavcan"]
path = src/lib/uavcan
url = git://github.com/UAVCAN/libuavcan.git
[submodule "Tools/genmsg"]

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

@ -1710,6 +1710,8 @@ Mavlink::task_main(int argc, char *argv[])
hrt_abstime t = hrt_absolute_time();
update_rate_mult();
_mission_manager->check_active_mission();
if (param_sub->update(&param_time, nullptr)) {
/* parameters updated */

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

@ -58,6 +58,12 @@
#endif
static const int ERROR = -1;
int MavlinkMissionManager::_dataman_id = 0;
bool MavlinkMissionManager::_dataman_init = false;
unsigned MavlinkMissionManager::_count = 0;
int MavlinkMissionManager::_current_seq = 0;
bool MavlinkMissionManager::_transfer_in_progress = false;
#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) || \
@ -70,14 +76,12 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m
_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
_max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX),
_dataman_id(0),
_count(0),
_current_seq(0),
_my_dataman_id(0),
_transfer_dataman_id(0),
_transfer_count(0),
_transfer_seq(0),
_transfer_current_seq(0),
_transfer_partner_sysid(0),
_transfer_partner_sysid(255),
_transfer_partner_compid(0),
_offboard_mission_sub(-1),
_mission_result_sub(-1),
@ -114,17 +118,21 @@ void
MavlinkMissionManager::init_offboard_mission()
{
mission_s mission_state;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
_dataman_id = mission_state.dataman_id;
_count = mission_state.count;
_current_seq = mission_state.current_seq;
if (!_dataman_init) {
_dataman_init = true;
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) {
_dataman_id = mission_state.dataman_id;
_count = mission_state.count;
_current_seq = mission_state.current_seq;
} else {
_dataman_id = 0;
_count = 0;
_current_seq = 0;
warnx("offboard mission init: ERROR");
} else {
_dataman_id = 0;
_count = 0;
_current_seq = 0;
warnx("offboard mission init: ERROR");
}
}
_my_dataman_id = _dataman_id;
}
/**
@ -147,6 +155,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
_dataman_id = dataman_id;
_count = count;
_current_seq = seq;
_my_dataman_id = _dataman_id;
/* mission state saved successfully, publish offboard_mission topic */
if (_offboard_mission_pub == nullptr) {
@ -564,11 +573,19 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
if (CHECK_SYSID_COMPID_MISSION(wpc)) {
if (_state == MAVLINK_WPM_STATE_IDLE) {
_time_last_recv = hrt_absolute_time();
if(_transfer_in_progress)
{
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
return;
}
_transfer_in_progress = true;
if (wpc.count > _max_count) {
if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); }
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE);
_transfer_in_progress = false;
return;
}
@ -580,6 +597,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
_mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION");
// TODO send ACK?
_transfer_in_progress = false;
return;
}
@ -661,6 +679,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret);
_state = MAVLINK_WPM_STATE_IDLE;
_transfer_in_progress = false;
return;
}
@ -672,6 +691,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
_mavlink->send_statustext_critical("Unable to write on micro SD");
_state = MAVLINK_WPM_STATE_IDLE;
_transfer_in_progress = false;
return;
}
@ -698,6 +718,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg)
} else {
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
}
_transfer_in_progress = false;
} else {
/* request next item */
@ -832,3 +853,14 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
return OK;
}
void MavlinkMissionManager::check_active_mission(void)
{
if(!(_my_dataman_id==_dataman_id))
{
if (_verbose) { warnx("WPM: New mission detected (possibly over different Mavlink instance) Updating"); }
_my_dataman_id=_dataman_id;
this->send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count);
}
}

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

@ -97,6 +97,8 @@ public:
void set_verbose(bool v) { _verbose = v; }
void check_active_mission(void);
private:
enum MAVLINK_WPM_STATES _state; ///< Current state
@ -107,9 +109,12 @@ private:
uint32_t _retry_timeout;
unsigned _max_count; ///< Maximum number of mission items
int _dataman_id; ///< Dataman storage ID for active mission
unsigned _count; ///< Count of items in active mission
int _current_seq; ///< Current item sequence in active mission
static int _dataman_id; ///< Global Dataman storage ID for active mission
int _my_dataman_id; ///< class Dataman storage ID
static bool _dataman_init; ///< Dataman initialized
static unsigned _count; ///< Count of items in active mission
static int _current_seq; ///< Current item sequence in active mission
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
unsigned _transfer_count; ///< Items count in current transmission
@ -117,6 +122,7 @@ private:
unsigned _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized)
unsigned _transfer_partner_sysid; ///< Partner system ID for current transmission
unsigned _transfer_partner_compid; ///< Partner component ID for current transmission
static bool _transfer_in_progress; ///< Global variable checking for current transmission
int _offboard_mission_sub;
int _mission_result_sub;