mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-25 00:31:36 +08:00
fix MISSION_ITEM REACHED message broadcast (#8332)
This commit is contained in:
lamping7
Daniel Agar
+3
-2
@@ -1,5 +1,6 @@
|
||||
int32 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
|
||||
uint32 count # count of the missions stored in the dataman
|
||||
int8 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
|
||||
|
||||
uint16 count # count of the missions stored in the dataman
|
||||
int32 current_seq # default -1, start at the one changed latest
|
||||
|
||||
# TOPICS mission offboard_mission onboard_mission
|
||||
|
||||
@@ -1,12 +1,11 @@
|
||||
uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified
|
||||
|
||||
uint32 seq_reached # Sequence of the mission item which has been reached
|
||||
uint32 seq_current # Sequence of the current mission item
|
||||
uint32 seq_total # Total number of mission items
|
||||
int32 seq_reached # Sequence of the mission item which has been reached, default -1
|
||||
uint16 seq_current # Sequence of the current mission item
|
||||
uint16 seq_total # Total number of mission items
|
||||
|
||||
bool valid # true if mission is valid
|
||||
bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings
|
||||
bool reached # true if mission has been reached
|
||||
bool finished # true if mission has been completed
|
||||
bool failure # true if the mission cannot continue or be completed for some reason
|
||||
|
||||
@@ -14,5 +13,5 @@ bool stay_in_failsafe # true if the commander should not switch out of the fail
|
||||
bool flight_termination # true if the navigator demands a flight termination from the commander app
|
||||
|
||||
bool item_do_jump_changed # true if the number of do jumps remaining has changed
|
||||
uint32 item_changed_index # indicate which item has changed
|
||||
uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item
|
||||
uint16 item_changed_index # indicate which item has changed
|
||||
uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item
|
||||
|
||||
@@ -593,6 +593,8 @@ void Logger::add_default_topics()
|
||||
add_topic("estimator_status", 200);
|
||||
add_topic("input_rc", 200);
|
||||
add_topic("manual_control_setpoint", 200);
|
||||
add_topic("mission_result");
|
||||
add_topic("offboard_mission");
|
||||
add_topic("optical_flow", 50);
|
||||
add_topic("position_setpoint_triplet", 200);
|
||||
add_topic("sensor_combined", 100);
|
||||
|
||||
@@ -54,13 +54,12 @@
|
||||
#include <uORB/topics/mission.h>
|
||||
#include <uORB/topics/mission_result.h>
|
||||
|
||||
int MavlinkMissionManager::_dataman_id = 0;
|
||||
int8_t MavlinkMissionManager::_dataman_id = 0;
|
||||
bool MavlinkMissionManager::_dataman_init = false;
|
||||
unsigned MavlinkMissionManager::_count[3] = { 0, 0, 0 };
|
||||
int MavlinkMissionManager::_current_seq = 0;
|
||||
int MavlinkMissionManager::_last_reached = -1;
|
||||
uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
|
||||
int32_t MavlinkMissionManager::_current_seq = 0;
|
||||
bool MavlinkMissionManager::_transfer_in_progress = false;
|
||||
constexpr unsigned MavlinkMissionManager::MAX_COUNT[];
|
||||
constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
|
||||
uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
|
||||
|
||||
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
|
||||
@@ -69,27 +68,6 @@ uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
|
||||
(_msg.target_component == MAV_COMP_ID_ALL)))
|
||||
|
||||
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
|
||||
_state(MAVLINK_WPM_STATE_IDLE),
|
||||
_mission_type(MAV_MISSION_TYPE_MISSION),
|
||||
_time_last_recv(0),
|
||||
_time_last_sent(0),
|
||||
_time_last_reached(0),
|
||||
_action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT),
|
||||
_retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT),
|
||||
_int_mode(false),
|
||||
_filesystem_errcount(0),
|
||||
_my_dataman_id(0),
|
||||
_transfer_dataman_id(0),
|
||||
_transfer_count(0),
|
||||
_transfer_seq(0),
|
||||
_transfer_current_seq(-1),
|
||||
_transfer_partner_sysid(0),
|
||||
_transfer_partner_compid(0),
|
||||
_offboard_mission_sub(-1),
|
||||
_mission_result_sub(-1),
|
||||
_offboard_mission_pub(nullptr),
|
||||
_geofence_locked(false),
|
||||
_slow_rate_limiter(100 * 1000), // Rate limit sending of the current WP sequence to 10 Hz
|
||||
_verbose(mavlink->verbose()),
|
||||
_mavlink(mavlink)
|
||||
{
|
||||
@@ -116,7 +94,7 @@ MavlinkMissionManager::init_offboard_mission()
|
||||
|
||||
if (ret > 0) {
|
||||
_dataman_id = mission_state.dataman_id;
|
||||
_count[(uint8_t)MAV_MISSION_TYPE_MISSION] = mission_state.count;
|
||||
_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
|
||||
_current_seq = mission_state.current_seq;
|
||||
|
||||
} else if (ret < 0) {
|
||||
@@ -139,7 +117,7 @@ MavlinkMissionManager::load_geofence_stats()
|
||||
int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
|
||||
|
||||
if (ret == sizeof(mission_stats_entry_s)) {
|
||||
_count[(uint8_t)MAV_MISSION_TYPE_FENCE] = stats.num_items;
|
||||
_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
|
||||
_geofence_update_counter = stats.update_counter;
|
||||
}
|
||||
|
||||
@@ -154,7 +132,7 @@ MavlinkMissionManager::load_safepoint_stats()
|
||||
int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
|
||||
|
||||
if (ret == sizeof(mission_stats_entry_s)) {
|
||||
_count[(uint8_t)MAV_MISSION_TYPE_RALLY] = stats.num_items;
|
||||
_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
|
||||
}
|
||||
|
||||
return ret;
|
||||
@@ -164,10 +142,10 @@ MavlinkMissionManager::load_safepoint_stats()
|
||||
* Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes.
|
||||
*/
|
||||
int
|
||||
MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq)
|
||||
MavlinkMissionManager::update_active_mission(int8_t dataman_id, uint16_t count, int32_t seq)
|
||||
{
|
||||
struct mission_s mission;
|
||||
|
||||
mission_s mission;
|
||||
mission.timestamp = hrt_absolute_time();
|
||||
mission.dataman_id = dataman_id;
|
||||
mission.count = count;
|
||||
mission.current_seq = seq;
|
||||
@@ -178,7 +156,7 @@ MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int
|
||||
if (res == sizeof(mission_s)) {
|
||||
/* update active mission state */
|
||||
_dataman_id = dataman_id;
|
||||
_count[(uint8_t)MAV_MISSION_TYPE_MISSION] = count;
|
||||
_count[MAV_MISSION_TYPE_MISSION] = count;
|
||||
_current_seq = seq;
|
||||
_my_dataman_id = _dataman_id;
|
||||
|
||||
@@ -213,7 +191,7 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
|
||||
int res = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
|
||||
|
||||
if (res == sizeof(mission_stats_entry_s)) {
|
||||
_count[(uint8_t)MAV_MISSION_TYPE_FENCE] = count;
|
||||
_count[MAV_MISSION_TYPE_FENCE] = count;
|
||||
|
||||
} else {
|
||||
PX4_ERR("WPM: can't save mission state");
|
||||
@@ -239,7 +217,7 @@ MavlinkMissionManager::update_safepoint_count(unsigned count)
|
||||
int res = dm_write(DM_KEY_SAFE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
|
||||
|
||||
if (res == sizeof(mission_stats_entry_s)) {
|
||||
_count[(uint8_t)MAV_MISSION_TYPE_RALLY] = count;
|
||||
_count[MAV_MISSION_TYPE_RALLY] = count;
|
||||
|
||||
} else {
|
||||
PX4_ERR("WPM: can't save mission state");
|
||||
@@ -273,7 +251,7 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
|
||||
void
|
||||
MavlinkMissionManager::send_mission_current(uint16_t seq)
|
||||
{
|
||||
unsigned item_count = _count[(uint8_t)MAV_MISSION_TYPE_MISSION];
|
||||
unsigned item_count = _count[MAV_MISSION_TYPE_MISSION];
|
||||
|
||||
if (seq < item_count) {
|
||||
mavlink_mission_current_t wpc;
|
||||
@@ -409,26 +387,26 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
|
||||
}
|
||||
}
|
||||
|
||||
unsigned
|
||||
uint16_t
|
||||
MavlinkMissionManager::current_max_item_count()
|
||||
{
|
||||
if ((unsigned)_mission_type >= sizeof(MAX_COUNT) / sizeof(MAX_COUNT[0])) {
|
||||
PX4_ERR("WPM: MAX_COUNT out of bounds (%u)", (unsigned)_mission_type);
|
||||
if (_mission_type >= sizeof(MAX_COUNT) / sizeof(MAX_COUNT[0])) {
|
||||
PX4_ERR("WPM: MAX_COUNT out of bounds (%u)", _mission_type);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return MAX_COUNT[(unsigned)_mission_type];
|
||||
return MAX_COUNT[_mission_type];
|
||||
}
|
||||
|
||||
unsigned
|
||||
uint16_t
|
||||
MavlinkMissionManager::current_item_count()
|
||||
{
|
||||
if ((unsigned)_mission_type >= sizeof(_count) / sizeof(_count[0])) {
|
||||
PX4_ERR("WPM: _count out of bounds (%u)", (unsigned)_mission_type);
|
||||
if (_mission_type >= sizeof(_count) / sizeof(_count[0])) {
|
||||
PX4_ERR("WPM: _count out of bounds (%u)", _mission_type);
|
||||
return 0;
|
||||
}
|
||||
|
||||
return _count[(unsigned)_mission_type];
|
||||
return _count[_mission_type];
|
||||
}
|
||||
|
||||
void
|
||||
@@ -495,17 +473,21 @@ MavlinkMissionManager::send(const hrt_abstime now)
|
||||
mission_result_s mission_result;
|
||||
orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result);
|
||||
|
||||
_current_seq = mission_result.seq_current;
|
||||
if (_current_seq != mission_result.seq_current) {
|
||||
_current_seq = mission_result.seq_current;
|
||||
|
||||
if (_verbose) { PX4_INFO("WPM: got mission result, new current_seq: %d", _current_seq); }
|
||||
if (_verbose) { PX4_INFO("WPM: got mission result, new current_seq: %u", _current_seq); }
|
||||
}
|
||||
|
||||
if (mission_result.reached) {
|
||||
_time_last_reached = now;
|
||||
if (_last_reached != mission_result.seq_reached) {
|
||||
_last_reached = mission_result.seq_reached;
|
||||
send_mission_item_reached((uint16_t)mission_result.seq_reached);
|
||||
_reached_sent_count = 0;
|
||||
|
||||
} else {
|
||||
_last_reached = -1;
|
||||
if (_last_reached >= 0) {
|
||||
send_mission_item_reached((uint16_t)mission_result.seq_reached);
|
||||
}
|
||||
|
||||
if (_verbose) { PX4_INFO("WPM: got mission result, new seq_reached: %d", _last_reached); }
|
||||
}
|
||||
|
||||
send_mission_current(_current_seq);
|
||||
@@ -520,21 +502,24 @@ MavlinkMissionManager::send(const hrt_abstime now)
|
||||
if (_slow_rate_limiter.check(now)) {
|
||||
send_mission_current(_current_seq);
|
||||
|
||||
// send the reached message a couple of times after reaching the waypoint
|
||||
if (_last_reached >= 0 && (now - _time_last_reached) < 300 * 1000) {
|
||||
// send the reached message another 10 times
|
||||
if (_last_reached >= 0 && (_reached_sent_count < 10)) {
|
||||
send_mission_item_reached((uint16_t)_last_reached);
|
||||
_reached_sent_count++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* check for timed-out operations */
|
||||
if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0)
|
||||
&& hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
|
||||
&& hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) {
|
||||
|
||||
// try to request item again after timeout
|
||||
send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
|
||||
|
||||
} else if (_state == MAVLINK_WPM_STATE_SENDLIST && (_time_last_sent > 0)
|
||||
&& hrt_elapsed_time(&_time_last_sent) > _retry_timeout) {
|
||||
&& hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) {
|
||||
|
||||
if (_transfer_seq == 0) {
|
||||
/* try to send items count again after timeout */
|
||||
send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count, _mission_type);
|
||||
@@ -547,7 +532,8 @@ MavlinkMissionManager::send(const hrt_abstime now)
|
||||
}
|
||||
|
||||
} else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0)
|
||||
&& hrt_elapsed_time(&_time_last_recv) > _action_timeout) {
|
||||
&& hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) {
|
||||
|
||||
_mavlink->send_statustext_critical("Operation timeout");
|
||||
|
||||
if (_verbose) { PX4_INFO("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); }
|
||||
@@ -665,8 +651,8 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
|
||||
if (_state == MAVLINK_WPM_STATE_IDLE) {
|
||||
_time_last_recv = hrt_absolute_time();
|
||||
|
||||
if (wpc.seq < _count[(uint8_t)MAV_MISSION_TYPE_MISSION]) {
|
||||
if (update_active_mission(_dataman_id, _count[(uint8_t)MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) {
|
||||
if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) {
|
||||
if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) {
|
||||
if (_verbose) { PX4_INFO("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); }
|
||||
|
||||
} else {
|
||||
@@ -725,13 +711,12 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg)
|
||||
|
||||
if (_transfer_count > 0) {
|
||||
if (_verbose) {
|
||||
PX4_INFO("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count,
|
||||
(int)_mission_type);
|
||||
PX4_INFO("WPM: MISSION_REQUEST_LIST OK, %u mission items to send, mission type=%i", _transfer_count, _mission_type);
|
||||
}
|
||||
|
||||
} else {
|
||||
if (_verbose) {
|
||||
PX4_INFO("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", (int)_mission_type);
|
||||
PX4_INFO("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty, mission type=%i", _mission_type);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -780,8 +765,8 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
|
||||
if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) {
|
||||
if (_state == MAVLINK_WPM_STATE_SENDLIST) {
|
||||
|
||||
if ((uint8_t)_mission_type != wpr.mission_type) {
|
||||
PX4_WARN("WPM: Unexpected mission type (%u %u)", (int)wpr.mission_type, (int)_mission_type);
|
||||
if (_mission_type != wpr.mission_type) {
|
||||
PX4_WARN("WPM: Unexpected mission type (%u %u)", wpr.mission_type, _mission_type);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -819,7 +804,7 @@ MavlinkMissionManager::handle_mission_request_both(const mavlink_message_t *msg)
|
||||
send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq);
|
||||
|
||||
} else {
|
||||
if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", (unsigned)wpr.seq, (unsigned)wpr.seq, (unsigned)current_item_count() - 1); }
|
||||
if (_verbose) { PX4_ERR("WPM: MISSION_ITEM_REQUEST(_INT) ERROR: seq %u out of bound [%u, %u]", wpr.seq, wpr.seq, current_item_count() - 1); }
|
||||
|
||||
switch_to_idle_state();
|
||||
|
||||
@@ -892,7 +877,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("mission type %u not handled", (unsigned)_mission_type);
|
||||
PX4_ERR("mission type %u not handled", _mission_type);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -1002,7 +987,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
||||
if (CHECK_SYSID_COMPID_MISSION(wp)) {
|
||||
|
||||
if (wp.mission_type != _mission_type) {
|
||||
PX4_WARN("WPM: Unexpected mission type (%u %u)", (int)wp.mission_type, (int)_mission_type);
|
||||
PX4_WARN("WPM: Unexpected mission type (%u %u)", wp.mission_type, _mission_type);
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -1165,7 +1150,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("mission type %u not handled", (unsigned)_mission_type);
|
||||
PX4_ERR("mission type %u not handled", _mission_type);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -1225,7 +1210,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("mission type %u not handled", (unsigned)_mission_type);
|
||||
PX4_ERR("mission type %u not handled", _mission_type);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -1626,7 +1611,7 @@ void MavlinkMissionManager::check_active_mission()
|
||||
if (_verbose) { PX4_INFO("WPM: New mission detected (possibly over different Mavlink instance) Updating"); }
|
||||
|
||||
_my_dataman_id = _dataman_id;
|
||||
send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[(uint8_t)MAV_MISSION_TYPE_MISSION],
|
||||
send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _count[MAV_MISSION_TYPE_MISSION],
|
||||
MAV_MISSION_TYPE_MISSION);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -67,8 +67,8 @@ enum MAVLINK_WPM_CODES {
|
||||
MAVLINK_WPM_CODE_ENUM_END
|
||||
};
|
||||
|
||||
#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds
|
||||
#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds
|
||||
static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT = 5000000; ///< Protocol action timeout in us
|
||||
static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 500000; ///< Protocol retry timeout in us
|
||||
|
||||
class Mavlink;
|
||||
|
||||
@@ -92,45 +92,48 @@ public:
|
||||
void check_active_mission(void);
|
||||
|
||||
private:
|
||||
enum MAVLINK_WPM_STATES _state; ///< Current state
|
||||
enum MAV_MISSION_TYPE _mission_type; ///< mission type of current transmission (only one at a time possible)
|
||||
enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state
|
||||
enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible)
|
||||
|
||||
uint64_t _time_last_recv;
|
||||
uint64_t _time_last_sent;
|
||||
uint64_t _time_last_reached; ///< last time when the vehicle reached a waypoint
|
||||
uint64_t _time_last_recv{0};
|
||||
uint64_t _time_last_sent{0};
|
||||
|
||||
uint32_t _action_timeout;
|
||||
uint32_t _retry_timeout;
|
||||
uint8_t _reached_sent_count{0}; ///< last time when the vehicle reached a waypoint
|
||||
|
||||
bool _int_mode; ///< Use accurate int32 instead of float
|
||||
bool _int_mode{false}; ///< Use accurate int32 instead of float
|
||||
|
||||
unsigned _filesystem_errcount; ///< File system error count
|
||||
unsigned _filesystem_errcount{0}; ///< File system error count
|
||||
|
||||
static int _dataman_id; ///< Global Dataman storage ID for active mission
|
||||
int _my_dataman_id; ///< class Dataman storage ID
|
||||
static int8_t _dataman_id; ///< Global Dataman storage ID for active mission
|
||||
int8_t _my_dataman_id{0}; ///< class Dataman storage ID
|
||||
static bool _dataman_init; ///< Dataman initialized
|
||||
|
||||
static unsigned _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE
|
||||
static int _current_seq; ///< Current item sequence in active mission
|
||||
static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE
|
||||
static int32_t _current_seq; ///< Current item sequence in active mission
|
||||
|
||||
static int _last_reached; ///< Last reached waypoint in active mission (-1 means nothing reached)
|
||||
int32_t _last_reached{-1}; ///< Last reached waypoint in active mission (-1 means nothing reached)
|
||||
|
||||
int8_t _transfer_dataman_id{0}; ///< Dataman storage ID for current transmission
|
||||
|
||||
uint16_t _transfer_count{0}; ///< Items count in current transmission
|
||||
uint16_t _transfer_seq{0}; ///< Item sequence in current transmission
|
||||
|
||||
int32_t _transfer_current_seq{-1}; ///< Current item ID for current transmission (-1 means not initialized)
|
||||
|
||||
uint8_t _transfer_partner_sysid{0}; ///< Partner system ID for current transmission
|
||||
uint8_t _transfer_partner_compid{0}; ///< Partner component ID for current transmission
|
||||
|
||||
int _transfer_dataman_id; ///< Dataman storage ID for current transmission
|
||||
unsigned _transfer_count; ///< Items count in current transmission
|
||||
unsigned _transfer_seq; ///< Item sequence in current transmission
|
||||
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;
|
||||
orb_advert_t _offboard_mission_pub;
|
||||
int _offboard_mission_sub{-1};
|
||||
int _mission_result_sub{-1};
|
||||
|
||||
static uint16_t _geofence_update_counter;
|
||||
bool _geofence_locked; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress)
|
||||
orb_advert_t _offboard_mission_pub{nullptr};
|
||||
|
||||
MavlinkRateLimiter _slow_rate_limiter;
|
||||
static uint16_t _geofence_update_counter;
|
||||
bool _geofence_locked{false}; ///< if true, we currently hold the dm_lock for the geofence (transaction in progress)
|
||||
|
||||
MavlinkRateLimiter _slow_rate_limiter{100 * 1000}; ///< Rate limit sending of the current WP sequence to 10 Hz
|
||||
|
||||
bool _verbose;
|
||||
|
||||
@@ -138,7 +141,7 @@ private:
|
||||
|
||||
static constexpr unsigned int FILESYSTEM_ERRCOUNT_NOTIFY_LIMIT =
|
||||
2; ///< Error count limit before stopping to report FS errors
|
||||
static constexpr unsigned MAX_COUNT[] = {
|
||||
static constexpr uint16_t MAX_COUNT[] = {
|
||||
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
|
||||
DM_KEY_FENCE_POINTS_MAX - 1,
|
||||
DM_KEY_SAFE_POINTS_MAX - 1
|
||||
@@ -146,10 +149,10 @@ private:
|
||||
(fence & save points use the first item for the stats) */
|
||||
|
||||
/** get the maximum number of item count for the current _mission_type */
|
||||
inline unsigned current_max_item_count();
|
||||
uint16_t current_max_item_count();
|
||||
|
||||
/** get the number of item count for the current _mission_type */
|
||||
inline unsigned current_item_count();
|
||||
uint16_t current_item_count();
|
||||
|
||||
/* do not allow top copying this class */
|
||||
MavlinkMissionManager(MavlinkMissionManager &);
|
||||
@@ -157,7 +160,7 @@ private:
|
||||
|
||||
void init_offboard_mission();
|
||||
|
||||
int update_active_mission(int dataman_id, unsigned count, int seq);
|
||||
int update_active_mission(int8_t dataman_id, uint16_t count, int32_t seq);
|
||||
|
||||
/** store the geofence count to dataman */
|
||||
int update_geofence_count(unsigned count);
|
||||
|
||||
@@ -74,7 +74,6 @@ Land::on_activation()
|
||||
{
|
||||
/* set current mission item to Land */
|
||||
set_land_item(&_mission_item, true);
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->finished = false;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
|
||||
@@ -139,6 +139,9 @@ Mission::on_inactive()
|
||||
|
||||
/* reset so current mission item gets restarted if mission was paused */
|
||||
_work_item_type = WORK_ITEM_TYPE_DEFAULT;
|
||||
|
||||
/* reset so MISSION_ITEM_REACHED isn't published */
|
||||
_navigator->get_mission_result()->seq_reached = -1;
|
||||
}
|
||||
|
||||
void
|
||||
@@ -329,9 +332,8 @@ Mission::update_onboard_mission()
|
||||
/* reset mission failure if we have an updated valid mission */
|
||||
_navigator->get_mission_result()->failure = false;
|
||||
|
||||
/* reset reached info as well */
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->seq_reached = 0;
|
||||
/* reset sequence info as well */
|
||||
_navigator->get_mission_result()->seq_reached = -1;
|
||||
_navigator->get_mission_result()->seq_total = _onboard_mission.count;
|
||||
|
||||
/* reset work item if new mission has been accepted */
|
||||
@@ -384,9 +386,8 @@ Mission::update_offboard_mission()
|
||||
/* reset mission failure if we have an updated valid mission */
|
||||
_navigator->get_mission_result()->failure = false;
|
||||
|
||||
/* reset reached info as well */
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->seq_reached = 0;
|
||||
/* reset sequence info as well */
|
||||
_navigator->get_mission_result()->seq_reached = -1;
|
||||
_navigator->get_mission_result()->seq_total = _offboard_mission.count;
|
||||
|
||||
/* reset work item if new mission has been accepted */
|
||||
@@ -1406,7 +1407,6 @@ Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
|
||||
void
|
||||
Mission::set_mission_item_reached()
|
||||
{
|
||||
_navigator->get_mission_result()->reached = true;
|
||||
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
@@ -1415,7 +1415,6 @@ Mission::set_mission_item_reached()
|
||||
void
|
||||
Mission::set_current_offboard_mission_item()
|
||||
{
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->finished = false;
|
||||
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
|
||||
_navigator->set_mission_result_updated();
|
||||
|
||||
@@ -1131,7 +1131,6 @@ Navigator::publish_mission_result()
|
||||
//_mission_result.seq_current = 0;
|
||||
|
||||
/* reset some of the flags */
|
||||
_mission_result.reached = false;
|
||||
_mission_result.item_do_jump_changed = false;
|
||||
_mission_result.item_changed_index = 0;
|
||||
_mission_result.item_do_jump_remaining = 0;
|
||||
|
||||
@@ -142,7 +142,6 @@ Takeoff::set_takeoff_position()
|
||||
|
||||
// set current mission item to takeoff
|
||||
set_takeoff_item(&_mission_item, abs_altitude);
|
||||
_navigator->get_mission_result()->reached = false;
|
||||
_navigator->get_mission_result()->finished = false;
|
||||
_navigator->set_mission_result_updated();
|
||||
reset_mission_item_reached();
|
||||
|
||||
Reference in New Issue
Block a user