forked from Archive/PX4-Autopilot
Mission merge offboard + onboard and simplify
This commit is contained in:
Daniel Agar
Lorenz Meier
parent
956935141e
commit
916d6a15fd
|
|
@ -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
|
||||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -1414,33 +1414,28 @@ Commander::run()
|
|||
|
||||
/* command ack */
|
||||
orb_advert_t command_ack_pub = nullptr;
|
||||
orb_advert_t commander_state_pub = nullptr;
|
||||
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;
|
||||
|
||||
orb_advert_t commander_state_pub = nullptr;
|
||||
|
||||
vehicle_status_flags_s vehicle_status_flags = {};
|
||||
orb_advert_t vehicle_status_flags_pub = nullptr;
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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};
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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) {}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
*
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue