diff --git a/msg/mission.msg b/msg/mission.msg
index 323f7a4e5102a53e3899c41d7af32f82c78d4c22..c2fbaafabcd5a8033124e30834af6a2aef8140e0 100644
--- a/msg/mission.msg
+++ b/msg/mission.msg
@@ -1,6 +1,4 @@
-int8 dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1
+uint8 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
+int32 current_seq # default -1, start at the one changed latest
\ No newline at end of file
diff --git a/src/examples/bottle_drop/bottle_drop.cpp b/src/examples/bottle_drop/bottle_drop.cpp
index 6e3776c20bd57ee99998a744482ce6b9c8d33247..fb9256dbf7bb5c6a4486d6209d6165960cf8bac7 100644
--- a/src/examples/bottle_drop/bottle_drop.cpp
+++ b/src/examples/bottle_drop/bottle_drop.cpp
@@ -188,6 +188,7 @@ BottleDrop::BottleDrop() :
_onboard_mission {},
_onboard_mission_pub(nullptr)
{
+ _onboard_mission.dataman_id = DM_KEY_WAYPOINTS_ONBOARD;
}
BottleDrop::~BottleDrop()
@@ -621,14 +622,15 @@ BottleDrop::task_main()
warnx("ERROR: could not save onboard WP");
}
+ _onboard_mission.timestamp = hrt_absolute_time();
_onboard_mission.count = 2;
_onboard_mission.current_seq = 0;
if (_onboard_mission_pub != nullptr) {
- orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+ orb_publish(ORB_ID(mission), _onboard_mission_pub, &_onboard_mission);
} else {
- _onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
+ _onboard_mission_pub = orb_advertise(ORB_ID(mission), &_onboard_mission);
}
float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat,
@@ -645,8 +647,9 @@ BottleDrop::task_main()
flight_vector_e.lon);
if (distance_wp2 < distance_real) {
+ _onboard_mission.timestamp = hrt_absolute_time();
_onboard_mission.current_seq = 0;
- orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+ orb_publish(ORB_ID(mission), _onboard_mission_pub, &_onboard_mission);
} else {
@@ -683,8 +686,9 @@ BottleDrop::task_main()
flight_vector_e.lon);
if (distance_wp2 < distance_real) {
+ _onboard_mission.timestamp = hrt_absolute_time();
_onboard_mission.current_seq = 0;
- orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+ orb_publish(ORB_ID(mission), _onboard_mission_pub, &_onboard_mission);
}
}
}
@@ -702,9 +706,11 @@ BottleDrop::task_main()
mavlink_log_info(&_mavlink_log_pub, "#audio: closing bay");
// remove onboard mission
+ _onboard_mission.timestamp = hrt_absolute_time();
+ _onboard_mission.dataman_id = DM_KEY_WAYPOINTS_ONBOARD;
_onboard_mission.current_seq = -1;
_onboard_mission.count = 0;
- orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+ orb_publish(ORB_ID(mission), _onboard_mission_pub, &_onboard_mission);
}
break;
@@ -804,7 +810,8 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
_onboard_mission.current_seq = -1;
if (_onboard_mission_pub != nullptr) {
- orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+ _onboard_mission.timestamp = hrt_absolute_time();
+ orb_publish(ORB_ID(mission), _onboard_mission_pub, &_onboard_mission);
}
} else {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f7eb1f743aa8949fc66924b36d845fb564e47a9a..649d2890f8b2e622773ba39a1ce8da5573ea9e0b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1414,33 +1414,28 @@ Commander::run()
/* command ack */
orb_advert_t command_ack_pub = nullptr;
-
- /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
- mission_s mission;
-
orb_advert_t commander_state_pub = nullptr;
-
- vehicle_status_flags_s vehicle_status_flags = {};
orb_advert_t vehicle_status_flags_pub = nullptr;
+ vehicle_status_flags_s vehicle_status_flags = {};
+ /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
+ mission_s mission = {};
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
- if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+ if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
if (mission.count > 0) {
- mavlink_log_info(&mavlink_log_pub, "[cmd] Mission #%d loaded, %u WPs, curr: %d",
- mission.dataman_id, mission.count, mission.current_seq);
+ PX4_INFO("Mission #%d loaded, %u WPs, curr: %d", mission.dataman_id, mission.count, mission.current_seq);
}
} else {
- mavlink_log_critical(&mavlink_log_pub, "reading mission state failed");
+ PX4_ERR("reading mission state failed");
/* initialize mission state in dataman */
- mission.dataman_id = 0;
- mission.count = 0;
- mission.current_seq = 0;
+ mission.timestamp = hrt_absolute_time();
+ mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
}
- orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+ orb_advert_t mission_pub = orb_advertise(ORB_ID(mission), &mission);
orb_unadvertise(mission_pub);
}
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 8f50f488919c18a3ebfb96f33cc914fbb205d3df..51eb270eecec4708fd905078e8c772ebec2d47d7 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -51,15 +51,13 @@ typedef enum {
DM_KEY_SAFE_POINTS = 0, /* Safe points coordinates, safe point 0 is home point */
DM_KEY_FENCE_POINTS, /* Fence vertex coordinates */
DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */
- DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */
+ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alternate between 0 and 1) */
DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */
DM_KEY_MISSION_STATE, /* Persistent mission state */
DM_KEY_COMPAT,
DM_KEY_NUM_KEYS /* Total number of item types defined */
} dm_item_t;
-#define DM_KEY_WAYPOINTS_OFFBOARD(_id) (_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1)
-
#if defined(MEMORY_CONSTRAINED_SYSTEM)
enum {
DM_KEY_SAFE_POINTS_MAX = 8,
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 51667503fc1bf317f539d4a138104be704c5c11f..37211d5d65e176bd342b600fd0f5e27d3285897b 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -594,8 +594,8 @@ void Logger::add_default_topics()
add_topic("home_position");
add_topic("input_rc", 200);
add_topic("manual_control_setpoint", 200);
+ add_topic("mission");
add_topic("mission_result");
- add_topic("offboard_mission");
add_topic("optical_flow", 50);
add_topic("position_setpoint_triplet", 200);
add_topic("sensor_combined", 100);
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index c78e5a9043e37320309112af50575904bd6442ed..35d540c8ba7162e7c000297a9a184b858d5064f7 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -54,7 +54,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
-int8_t MavlinkMissionManager::_dataman_id = 0;
+dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
bool MavlinkMissionManager::_dataman_init = false;
uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
int32_t MavlinkMissionManager::_current_seq = 0;
@@ -70,7 +70,7 @@ uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
_mavlink(mavlink)
{
- _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+ _offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_mission_result_sub = orb_subscribe(ORB_ID(mission_result));
init_offboard_mission();
@@ -85,14 +85,26 @@ MavlinkMissionManager::~MavlinkMissionManager()
void
MavlinkMissionManager::init_offboard_mission()
{
- mission_s mission_state;
-
if (!_dataman_init) {
_dataman_init = true;
+
+ /* lock MISSION_STATE item */
+ int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+ if (dm_lock_ret != 0) {
+ PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+ }
+
+ mission_s mission_state;
int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+ /* unlock MISSION_STATE item */
+ if (dm_lock_ret == 0) {
+ dm_unlock(DM_KEY_MISSION_STATE);
+ }
+
if (ret > 0) {
- _dataman_id = mission_state.dataman_id;
+ _dataman_id = (dm_item_t)mission_state.dataman_id;
_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
_current_seq = mission_state.current_seq;
@@ -138,10 +150,10 @@ MavlinkMissionManager::load_safepoint_stats()
}
/**
- * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes.
+ * Publish mission topic to notify navigator about changes.
*/
int
-MavlinkMissionManager::update_active_mission(int8_t dataman_id, uint16_t count, int32_t seq)
+MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
{
mission_s mission;
mission.timestamp = hrt_absolute_time();
@@ -150,8 +162,21 @@ MavlinkMissionManager::update_active_mission(int8_t dataman_id, uint16_t count,
mission.current_seq = seq;
/* update mission state in dataman */
+
+ /* lock MISSION_STATE item */
+ int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+ if (dm_lock_ret != 0) {
+ PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+ }
+
int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+ /* unlock MISSION_STATE item */
+ if (dm_lock_ret == 0) {
+ dm_unlock(DM_KEY_MISSION_STATE);
+ }
+
if (res == sizeof(mission_s)) {
/* update active mission state */
_dataman_id = dataman_id;
@@ -161,10 +186,10 @@ MavlinkMissionManager::update_active_mission(int8_t dataman_id, uint16_t count,
/* mission state saved successfully, publish offboard_mission topic */
if (_offboard_mission_pub == nullptr) {
- _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission);
+ _offboard_mission_pub = orb_advertise(ORB_ID(mission), &mission);
} else {
- orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission);
+ orb_publish(ORB_ID(mission), _offboard_mission_pub, &mission);
}
return PX4_OK;
@@ -289,16 +314,13 @@ MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_
void
MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq)
{
- dm_item_t dm_item;
- struct mission_item_s mission_item {};
+ mission_item_s mission_item = {};
bool read_success = false;
switch (_mission_type) {
case MAV_MISSION_TYPE_MISSION: {
- dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_dataman_id);
- read_success = dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) ==
- sizeof(struct mission_item_s);
+ read_success = dm_read(_dataman_id, seq, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s);
}
break;
@@ -306,6 +328,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
mission_fence_point_s mission_fence_point;
read_success = dm_read(DM_KEY_FENCE_POINTS, seq + 1, &mission_fence_point, sizeof(mission_fence_point_s)) ==
sizeof(mission_fence_point_s);
+
mission_item.nav_cmd = mission_fence_point.nav_cmd;
mission_item.frame = mission_fence_point.frame;
mission_item.lat = mission_fence_point.lat;
@@ -326,6 +349,7 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
mission_save_point_s mission_save_point;
read_success = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_save_point, sizeof(mission_save_point_s)) ==
sizeof(mission_save_point_s);
+
mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
mission_item.frame = mission_save_point.frame;
mission_item.lat = mission_save_point.lat;
@@ -851,8 +875,16 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
switch (_mission_type) {
case MAV_MISSION_TYPE_MISSION:
+
/* alternate dataman ID anyway to let navigator know about changes */
- update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
+
+ if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
+ update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0);
+
+ } else {
+ update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+ }
+
break;
case MAV_MISSION_TYPE_FENCE:
@@ -880,7 +912,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
_transfer_partner_sysid = msg->sysid;
_transfer_partner_compid = msg->compid;
_transfer_count = wpc.count;
- _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission
+ _transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
+ DM_KEY_WAYPOINTS_OFFBOARD_0); // use inactive storage for transmission
_transfer_current_seq = -1;
if (_mission_type == MAV_MISSION_TYPE_FENCE) {
@@ -1032,7 +1065,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
check_failed = true;
} else {
- dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id);
+ dm_item_t dm_item = _transfer_dataman_id;
write_failed = dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item,
sizeof(struct mission_item_s)) != sizeof(struct mission_item_s);
@@ -1180,7 +1213,8 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
switch (wpca.mission_type) {
case MAV_MISSION_TYPE_MISSION:
- ret = update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
+ ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
+ DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
break;
case MAV_MISSION_TYPE_FENCE:
@@ -1192,7 +1226,8 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
break;
case MAV_MISSION_TYPE_ALL:
- ret = update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0);
+ ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
+ DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
ret = update_geofence_count(0) || ret;
ret = update_safepoint_count(0) || ret;
break;
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 1ca1de4e5e5f791d083161ff5c945a50b9087719..8fe20fb0c0e24c25448f55a5f3d697a66cfa8f7c 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -102,8 +102,9 @@ private:
unsigned _filesystem_errcount{0}; ///< File system error count
- static int8_t _dataman_id; ///< Global Dataman storage ID for active mission
- int8_t _my_dataman_id{0}; ///< class Dataman storage ID
+ static dm_item_t _dataman_id; ///< Global Dataman storage ID for active mission
+ dm_item_t _my_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_0}; ///< class Dataman storage ID
+
static bool _dataman_init; ///< Dataman initialized
static uint16_t _count[3]; ///< Count of items in (active) mission for each MAV_MISSION_TYPE
@@ -111,7 +112,7 @@ private:
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
+ dm_item_t _transfer_dataman_id{DM_KEY_WAYPOINTS_OFFBOARD_1}; ///< 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
@@ -156,7 +157,7 @@ private:
void init_offboard_mission();
- int update_active_mission(int8_t dataman_id, uint16_t count, int32_t seq);
+ int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq);
/** store the geofence count to dataman */
int update_geofence_count(unsigned count);
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 2b29f2a9f3ac00e6ad028150d420726ac3e0c179..52d6b606577a15a5554c1d0b0220529f33fdaff4 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -60,7 +60,6 @@
Mission::Mission(Navigator *navigator, const char *name) :
MissionBlock(navigator, name),
- _param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
_param_dist_1wp(this, "MIS_DIST_1WP", false),
_param_dist_between_wps(this, "MIS_DIST_WPS", false),
_param_altmode(this, "MIS_ALTMODE", false),
@@ -82,14 +81,6 @@ Mission::on_inactive()
}
if (_inited) {
- /* check if missions have changed so that feedback to ground station is given */
- bool onboard_updated = false;
- orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
-
- if (onboard_updated) {
- update_onboard_mission();
- }
-
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
@@ -175,13 +166,6 @@ Mission::on_active()
check_mission_valid(false);
/* check if anything has changed */
- bool onboard_updated = false;
- orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
-
- if (onboard_updated) {
- update_onboard_mission();
- }
-
bool offboard_updated = false;
orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
@@ -198,7 +182,7 @@ Mission::on_active()
}
/* reset mission items if needed */
- if (onboard_updated || offboard_updated) {
+ if (offboard_updated) {
set_mission_items();
}
@@ -282,7 +266,7 @@ Mission::find_offboard_land_start()
* TODO: implement full spec and find closest landing point geographically
*/
- dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+ const dm_item_t dm_current = (dm_item_t)_offboard_mission.dataman_id;
for (size_t i = 0; i < _offboard_mission.count; i++) {
struct mission_item_s missionitem = {};
@@ -334,53 +318,6 @@ Mission::landing()
return mission_valid && on_landing_stage;
}
-void
-Mission::update_onboard_mission()
-{
- /* reset triplets */
- _navigator->reset_triplets();
-
- if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
- /* accept the current index set by the onboard mission if it is within bounds */
- if (_onboard_mission.current_seq >= 0
- && _onboard_mission.current_seq < (int)_onboard_mission.count) {
-
- _current_onboard_mission_index = _onboard_mission.current_seq;
-
- } else {
- /* if less WPs available, reset to first WP */
- if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
- _current_onboard_mission_index = 0;
- /* if not initialized, set it to 0 */
-
- } else if (_current_onboard_mission_index < 0) {
- _current_onboard_mission_index = 0;
- }
-
- /* otherwise, just leave it */
- }
-
- // XXX check validity here as well
- _navigator->get_mission_result()->valid = true;
- /* reset mission failure if we have an updated valid mission */
- _navigator->get_mission_result()->failure = false;
-
- /* 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 */
- _work_item_type = WORK_ITEM_TYPE_DEFAULT;
- _navigator->increment_mission_instance_count();
- _navigator->set_mission_result_updated();
-
- } else {
- _onboard_mission.count = 0;
- _onboard_mission.current_seq = 0;
- _current_onboard_mission_index = 0;
- }
-}
-
void
Mission::update_offboard_mission()
{
@@ -389,11 +326,7 @@ Mission::update_offboard_mission()
/* reset triplets */
_navigator->reset_triplets();
- if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
- // The following is not really a warning, but it can be useful to have this message in the log file
- PX4_WARN("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id,
- _offboard_mission.count, _offboard_mission.current_seq);
-
+ if (orb_copy(ORB_ID(mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
/* determine current index */
if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
_current_offboard_mission_index = _offboard_mission.current_seq;
@@ -455,10 +388,6 @@ Mission::advance_mission()
}
switch (_mission_type) {
- case MISSION_TYPE_ONBOARD:
- _current_onboard_mission_index++;
- break;
-
case MISSION_TYPE_OFFBOARD:
_current_offboard_mission_index++;
break;
@@ -486,8 +415,6 @@ Mission::set_mission_items()
/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
_min_current_sp_distance_xy = FLT_MAX;
- struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
@@ -497,21 +424,7 @@ Mission::set_mission_items()
work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
- /* try setting onboard mission item */
- if (_param_onboard_enabled.get()
- && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
-
- /* if mission type changed, notify */
- if (_mission_type != MISSION_TYPE_ONBOARD) {
- mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing internal mission.");
- user_feedback_done = true;
- }
-
- _mission_type = MISSION_TYPE_ONBOARD;
-
- /* try setting offboard mission item */
-
- } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
+ if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Executing mission.");
@@ -544,6 +457,7 @@ Mission::set_mission_items()
set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
/* update position setpoint triplet */
+ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -588,6 +502,7 @@ Mission::set_mission_items()
}
/* we have a new position item so set previous position setpoint to current */
+ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous = pos_sp_triplet->current;
/* do takeoff before going to setpoint if needed and not already in takeoff */
@@ -710,8 +625,7 @@ Mission::set_mission_items()
float altitude = _navigator->get_global_position()->alt;
- if (pos_sp_triplet->current.valid
- && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+ if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
altitude = pos_sp_triplet->current.alt;
}
@@ -785,6 +699,7 @@ Mission::set_mission_items()
}
} else {
+ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* handle non-position mission items such as commands */
/* turn towards next waypoint before MC to FW transition */
@@ -842,6 +757,8 @@ Mission::set_mission_items()
/*********************************** set setpoints and check next *********************************************/
+ position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
/* set current position setpoint from mission item (is protected against non-position items) */
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
@@ -987,10 +904,8 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss
mission_item->autocontinue = true;
mission_item->time_inside = 0.0f;
mission_item->yaw = get_bearing_to_next_waypoint(
- _navigator->get_global_position()->lat,
- _navigator->get_global_position()->lon,
- mission_item_next->lat,
- mission_item_next->lon);
+ _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+ mission_item_next->lat, mission_item_next->lon);
mission_item->force_heading = true;
}
@@ -1048,8 +963,7 @@ Mission::heading_sp_update()
point_from_latlon[1] = _navigator->get_global_position()->lon;
/* target location is home */
- if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
- || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
+ if ((_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME || _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME)
// need to be rotary wing for this but not in a transition
// in VTOL mode this will prevent updating yaw during FW flight
// (which would result in a wrong yaw setpoint spike during back transition)
@@ -1196,9 +1110,9 @@ Mission::do_abort_landing()
// loiter at the larger of MIS_LTRMIN_ALT above the landing point
// or 2 * FW_CLMBOUT_DIFF above the current altitude
- float alt_landing = get_absolute_altitude_for_item(_mission_item);
- float min_climbout = _navigator->get_global_position()->alt + 20.0f;
- float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), min_climbout);
+ const float alt_landing = get_absolute_altitude_for_item(_mission_item);
+ const float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(),
+ _navigator->get_global_position()->alt + 20.0f);
// turn current landing waypoint into an indefinite loiter
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
@@ -1209,12 +1123,11 @@ Mission::do_abort_landing()
_mission_item.autocontinue = false;
_mission_item.origin = ORIGIN_ONBOARD;
- struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(_mission_item);
- mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+ mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
_navigator->set_position_setpoint_triplet_updated();
- mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %dm above landing.",
+ mavlink_and_console_log_info(_navigator->get_mavlink_log_pub(), "Holding at %d m above landing.",
(int)(alt_sp - alt_landing));
// reset mission index to start of landing
@@ -1240,18 +1153,18 @@ Mission::do_abort_landing()
}
bool
-Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
- struct mission_item_s *next_position_mission_item, bool *has_next_position_item)
+Mission::prepare_mission_items(mission_item_s *mission_item, mission_item_s *next_position_mission_item,
+ bool *has_next_position_item)
{
bool first_res = false;
int offset = 1;
- if (read_mission_item(onboard, 0, mission_item)) {
+ if (read_mission_item(0, mission_item)) {
first_res = true;
/* trying to find next position mission item */
- while (read_mission_item(onboard, offset, next_position_mission_item)) {
+ while (read_mission_item(offset, next_position_mission_item)) {
if (item_contains_position(*next_position_mission_item)) {
*has_next_position_item = true;
@@ -1266,32 +1179,21 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item
}
bool
-Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item)
+Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
{
- /* select onboard/offboard mission */
- int *mission_index_ptr;
- dm_item_t dm_item;
-
- struct mission_s *mission = (onboard) ? &_onboard_mission : &_offboard_mission;
- int current_index = (onboard) ? _current_onboard_mission_index : _current_offboard_mission_index;
+ /* select offboard mission */
+ struct mission_s *mission = &_offboard_mission;
+ int current_index = _current_offboard_mission_index;
int index_to_read = current_index + offset;
+ int *mission_index_ptr = (offset == 0) ? &_current_offboard_mission_index : &index_to_read;
+ const dm_item_t dm_item = (dm_item_t)_offboard_mission.dataman_id;
+
/* do not work on empty missions */
if (mission->count == 0) {
return false;
}
- if (onboard) {
- /* onboard mission */
- mission_index_ptr = (offset == 0) ? &_current_onboard_mission_index : &index_to_read;
- dm_item = DM_KEY_WAYPOINTS_ONBOARD;
-
- } else {
- /* offboard mission */
- mission_index_ptr = (offset == 0) ? &_current_offboard_mission_index : &index_to_read;
- dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
- }
-
/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
* 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
for (int i = 0; i < 10; i++) {
@@ -1300,8 +1202,7 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
/* mission item index out of bounds - if they are equal, we just reached the end */
if (*mission_index_ptr != (int)mission->count) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission item index out of bound, index: %d, max: %d.",
- *mission_index_ptr,
- (int)mission->count);
+ *mission_index_ptr, mission->count);
}
return false;
@@ -1331,10 +1232,8 @@ Mission::read_mission_item(bool onboard, int offset, struct mission_item_s *miss
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
- if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
- &mission_item_tmp, len) != len) {
- /* not supposed to happen unless the datamanager can't access the
- * dataman */
+ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET, &mission_item_tmp, len) != len) {
+ /* not supposed to happen unless the datamanager can't access the dataman */
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.");
return false;
}
@@ -1376,7 +1275,7 @@ Mission::save_offboard_mission_state()
int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
if (dm_lock_ret != 0) {
- PX4_ERR("lock failed");
+ PX4_ERR("DM_KEY_MISSION_STATE lock failed");
}
/* read current state */
@@ -1387,16 +1286,19 @@ Mission::save_offboard_mission_state()
if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
/* navigator may modify only sequence, write modified state only if it changed */
if (mission_state.current_seq != _current_offboard_mission_index) {
+ mission_state.timestamp = hrt_absolute_time();
+
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state,
sizeof(mission_s)) != sizeof(mission_s)) {
- mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Can't save mission state.");
+ PX4_ERR("Can't save mission state");
}
}
}
} else {
/* invalid data, this must not happen and indicates error in offboard_mission publisher */
+ mission_state.timestamp = hrt_absolute_time();
mission_state.dataman_id = _offboard_mission.dataman_id;
mission_state.count = _offboard_mission.count;
mission_state.current_seq = _current_offboard_mission_index;
@@ -1407,7 +1309,7 @@ Mission::save_offboard_mission_state()
if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state,
sizeof(mission_s)) != sizeof(mission_s)) {
- mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Can't save mission state.");
+ PX4_ERR("Can't save mission state");
}
}
@@ -1424,6 +1326,7 @@ Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
_navigator->get_mission_result()->item_do_jump_changed = true;
_navigator->get_mission_result()->item_changed_index = index;
_navigator->get_mission_result()->item_do_jump_remaining = do_jumps_remaining;
+
_navigator->set_mission_result_updated();
}
@@ -1432,6 +1335,7 @@ Mission::set_mission_item_reached()
{
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->set_mission_result_updated();
+
reset_mission_item_reached();
}
@@ -1440,6 +1344,7 @@ Mission::set_current_offboard_mission_item()
{
_navigator->get_mission_result()->finished = false;
_navigator->get_mission_result()->seq_current = _current_offboard_mission_index;
+
_navigator->set_mission_result_updated();
save_offboard_mission_state();
@@ -1474,13 +1379,13 @@ Mission::reset_offboard_mission(struct mission_s &mission)
dm_lock(DM_KEY_MISSION_STATE);
if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
- if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+ if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
/* set current item to 0 */
mission.current_seq = 0;
/* reset jump counters */
if (mission.count > 0) {
- dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
+ const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
for (unsigned index = 0; index < mission.count; index++) {
struct mission_item_s item;
@@ -1506,7 +1411,8 @@ Mission::reset_offboard_mission(struct mission_s &mission)
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.");
/* initialize mission state in dataman */
- mission.dataman_id = 0;
+ mission.timestamp = hrt_absolute_time();
+ mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
mission.count = 0;
mission.current_seq = 0;
}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 14ea0a7810ba8e6936b75b28f3c57a2f00e91c46..966d40fa4d94e25630497bbf521cfed10665195e 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -97,10 +97,6 @@ public:
uint16_t get_land_start_index() const { return _land_start_index; }
private:
- /**
- * Update onboard mission topic
- */
- void update_onboard_mission();
/**
* Update offboard mission topic
@@ -175,8 +171,8 @@ private:
*
* @return true if current mission item available
*/
- bool prepare_mission_items(bool onboard, struct mission_item_s *mission_item,
- struct mission_item_s *next_position_mission_item, bool *has_next_position_item);
+ bool prepare_mission_items(mission_item_s *mission_item, mission_item_s *next_position_mission_item,
+ bool *has_next_position_item);
/**
* Read current (offset == 0) or a specific (offset > 0) mission item
@@ -184,7 +180,7 @@ private:
*
* @return true if successful
*/
- bool read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item);
+ bool read_mission_item(int offset, mission_item_s *mission_item);
/**
* Save current offboard mission state to dataman
@@ -232,16 +228,13 @@ private:
*/
bool find_offboard_land_start();
- control::BlockParamInt _param_onboard_enabled;
control::BlockParamFloat _param_dist_1wp;
control::BlockParamFloat _param_dist_between_wps;
control::BlockParamInt _param_altmode;
control::BlockParamInt _param_yawmode;
- struct mission_s _onboard_mission {};
struct mission_s _offboard_mission {};
- int32_t _current_onboard_mission_index{-1};
int32_t _current_offboard_mission_index{-1};
// track location of planned mission landing
@@ -252,7 +245,6 @@ private:
enum {
MISSION_TYPE_NONE,
- MISSION_TYPE_ONBOARD,
MISSION_TYPE_OFFBOARD
} _mission_type{MISSION_TYPE_NONE};
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index bdf15fbb52d26788aae44b05fe31cdd6d5df409d..c44b78d0827be642a6daf7f2663726f704f5897a 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -55,9 +55,6 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
bool land_start_req)
{
- const dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
- const size_t nMissionItems = mission.count;
-
bool failed = false;
bool warned = false;
@@ -71,36 +68,36 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
} else {
- failed = failed || !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint);
+ failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
}
const float home_alt = _navigator->get_home_position()->alt;
// check if all mission item commands are supported
- failed = failed || !checkMissionItemValidity(dm_current, nMissionItems);
- failed = failed || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints);
- failed = failed || !checkGeofence(dm_current, nMissionItems, home_alt, home_valid);
- failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);
+ failed = failed || !checkMissionItemValidity(mission);
+ failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
+ failed = failed || !checkGeofence(mission, home_alt, home_valid);
+ failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
// VTOL always respects rotary wing feasibility
if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) {
- failed = failed || !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid);
+ failed = failed || !checkRotarywing(mission, home_alt, home_alt_valid);
} else {
- failed = failed || !checkFixedwing(dm_current, nMissionItems, home_alt, home_alt_valid, land_start_req);
+ failed = failed || !checkFixedwing(mission, home_alt, home_alt_valid, land_start_req);
}
return !failed;
}
bool
-MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid)
+MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt, bool home_alt_valid)
{
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(struct mission_item_s);
- if (dm_read(dm_current, i, &missionitem, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
@@ -131,19 +128,19 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
}
bool
-MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
+MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool home_alt_valid,
bool land_start_req)
{
/* Perform checks and issue feedback to the user for all checks */
- bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_valid);
- bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, land_start_req);
+ bool resTakeoff = checkFixedWingTakeoff(mission, home_alt, home_alt_valid);
+ bool resLanding = checkFixedWingLanding(mission, land_start_req);
/* Mission is only marked as feasible if all checks return true */
return (resTakeoff && resLanding);
}
bool
-MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid)
+MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid)
{
if (_navigator->get_geofence().isHomeRequired() && !home_valid) {
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position");
@@ -152,11 +149,11 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
/* Check if all mission items are inside the geofence (if we have a valid geofence) */
if (_navigator->get_geofence().valid()) {
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(missionitem);
- if (dm_read(dm_current, i, &missionitem, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
@@ -181,22 +178,22 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
}
bool
-MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt,
- bool home_alt_valid, bool throw_error)
+MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid,
+ bool throw_error)
{
/* Check if all waypoints are above the home altitude */
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(struct mission_item_s);
- if (dm_read(dm_current, i, &missionitem, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
_navigator->get_mission_result()->warning = true;
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
/* reject relative alt without home set */
- if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(missionitem)) {
+ if (missionitem.altitude_is_relative && !home_alt_valid && MissionBlock::item_contains_position(missionitem)) {
_navigator->get_mission_result()->warning = true;
@@ -232,14 +229,14 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
}
bool
-MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems)
+MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
{
// do not allow mission if we find unsupported item
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(struct mission_item_s);
- if (dm_read(dm_current, i, &missionitem, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
// not supposed to happen unless the datamanager can't access the SD card, etc.
mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card");
return false;
@@ -311,14 +308,13 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
}
bool
-MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt,
- bool home_alt_valid)
+MissionFeasibilityChecker::checkFixedWingTakeoff(const mission_s &mission, float home_alt, bool home_alt_valid)
{
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem = {};
const ssize_t len = sizeof(struct mission_item_s);
- if (dm_read(dm_current, i, &missionitem, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
@@ -351,7 +347,7 @@ MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nM
}
bool
-MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, bool land_start_req)
+MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
{
/* Go through all mission items and search for a landing waypoint
* if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
@@ -362,11 +358,11 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
size_t do_land_start_index = 0;
size_t landing_approach_index = 0;
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s missionitem;
const ssize_t len = sizeof(missionitem);
- if (dm_read(dm_current, i, &missionitem, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
@@ -389,7 +385,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
if (i > 0) {
landing_approach_index = i - 1;
- if (dm_read(dm_current, landing_approach_index, &missionitem_previous, len) != len) {
+ if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
return false;
}
@@ -468,7 +464,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
}
bool
-MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance)
+MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
{
if (max_distance <= 0.0f) {
/* param not set, check is ok */
@@ -476,11 +472,11 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, si
}
/* find first waypoint (with lat/lon) item in datamanager */
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
- if (!(dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
+ if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
return false;
@@ -524,8 +520,7 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, si
}
bool
-MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems,
- float max_distance)
+MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance)
{
if (max_distance <= 0.0f) {
/* param not set, check is ok */
@@ -536,11 +531,11 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current,
double last_lon = NAN;
/* Go through all waypoints */
- for (size_t i = 0; i < nMissionItems; i++) {
+ for (size_t i = 0; i < mission.count; i++) {
struct mission_item_s mission_item {};
- if (!(dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
+ if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.");
return false;
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index 99fdd55488f6d4f80475849e7bebe99ab1707dd6..65f8e3dd3f9651f39eecd9042009c45bf1d8a46f 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -55,24 +55,22 @@ private:
Navigator *_navigator{nullptr};
/* Checks for all airframes */
- bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid);
+ bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
- bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
- bool throw_error);
+ bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid, bool throw_error);
- bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
+ bool checkMissionItemValidity(const mission_s &mission);
- bool checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance);
- bool checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems, float max_distance);
+ bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance);
+ bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance);
/* Checks specific to fixedwing airframes */
- bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid, bool land_start_req);
-
- bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid);
- bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, bool land_start_req);
+ bool checkFixedwing(const mission_s &mission, float home_alt, bool home_alt_valid, bool land_start_req);
+ bool checkFixedWingTakeoff(const mission_s &mission, float home_alt, bool home_alt_valid);
+ bool checkFixedWingLanding(const mission_s &mission, bool land_start_req);
/* Checks specific to rotarywing airframes */
- bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid);
+ bool checkRotarywing(const mission_s &mission, float home_alt, bool home_alt_valid);
public:
MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index de201fb61fffbc8a800df25ea278afb36db786dc..e477b1011cd4f25fe6f94b288d175dab4ac84e65 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -72,17 +72,6 @@ PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
*/
PARAM_DEFINE_FLOAT(MIS_LTRMIN_ALT, -1.0f);
-/**
- * Persistent onboard mission storage
- *
- * When enabled, missions that have been uploaded by the GCS are stored
- * and reloaded after reboot persistently.
- *
- * @boolean
- * @group Mission
- */
-PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
-
/**
* Maximal horizontal distance from home to first waypoint
*
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index daca10bae653e77ec8770bbc79a029098c8d7ac0..ce15ca431a08563426b10c486b7e4aa5aa566fc9 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -154,7 +154,6 @@ public:
bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos); }
- int get_onboard_mission_sub() { return _onboard_mission_sub; }
int get_offboard_mission_sub() { return _offboard_mission_sub; }
Geofence &get_geofence() { return _geofence; }
@@ -272,7 +271,6 @@ private:
int _land_detected_sub{-1}; /**< vehicle land detected subscription */
int _local_pos_sub{-1}; /**< local position subscription */
int _offboard_mission_sub{-1}; /**< offboard mission subscription */
- int _onboard_mission_sub{-1}; /**< onboard mission subscription */
int _param_update_sub{-1}; /**< param update subscription */
int _sensor_combined_sub{-1}; /**< sensor combined subscription */
int _traffic_sub{-1}; /**< traffic subscription */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a862dd30eb83fbdb2d4491109ca12a8f3d8bcdd0..c63af2248045c8b6367a12d3dba7d1ae87e52187 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -248,8 +248,7 @@ Navigator::task_main()
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
- _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
- _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+ _offboard_mission_sub = orb_subscribe(ORB_ID(mission));
_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_traffic_sub = orb_subscribe(ORB_ID(transponder_report));
@@ -740,7 +739,6 @@ Navigator::task_main()
orb_unsubscribe(_vstatus_sub);
orb_unsubscribe(_land_detected_sub);
orb_unsubscribe(_home_pos_sub);
- orb_unsubscribe(_onboard_mission_sub);
orb_unsubscribe(_offboard_mission_sub);
orb_unsubscribe(_param_update_sub);
orb_unsubscribe(_vehicle_command_sub);