|
|
|
@@ -50,27 +50,75 @@
|
|
|
|
|
#include <px4_platform_common/events.h>
|
|
|
|
|
#include <mathlib/mathlib.h>
|
|
|
|
|
#include <matrix/math.hpp>
|
|
|
|
|
#include <mavlink.h>
|
|
|
|
|
#include <navigator/navigation.h>
|
|
|
|
|
#include <uORB/topics/mission.h>
|
|
|
|
|
#include <uORB/topics/mission_result.h>
|
|
|
|
|
#include <crc32.h>
|
|
|
|
|
|
|
|
|
|
using matrix::wrap_2pi;
|
|
|
|
|
|
|
|
|
|
dm_item_t MavlinkMissionManager::_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
|
|
|
|
|
bool MavlinkMissionManager::_dataman_init = false;
|
|
|
|
|
uint16_t MavlinkMissionManager::_count[3] = { 0, 0, 0 };
|
|
|
|
|
uint32_t MavlinkMissionManager::_crc32[3] = { 0, 0, 0 };
|
|
|
|
|
int32_t MavlinkMissionManager::_current_seq = 0;
|
|
|
|
|
bool MavlinkMissionManager::_transfer_in_progress = false;
|
|
|
|
|
constexpr uint16_t MavlinkMissionManager::MAX_COUNT[];
|
|
|
|
|
uint16_t MavlinkMissionManager::_geofence_update_counter = 0;
|
|
|
|
|
uint16_t MavlinkMissionManager::_safepoint_update_counter = 0;
|
|
|
|
|
|
|
|
|
|
typedef struct __attribute__((packed)) CrcMissionItem {
|
|
|
|
|
uint8_t frame;
|
|
|
|
|
uint16_t command;
|
|
|
|
|
uint8_t autocontinue;
|
|
|
|
|
float params[7];
|
|
|
|
|
} CrcMissionItem_t;
|
|
|
|
|
|
|
|
|
|
static constexpr int MISSION_ITEM_CRC_SIZE = sizeof(CrcMissionItem_t);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \
|
|
|
|
|
((_msg.target_component == mavlink_system.compid) || \
|
|
|
|
|
(_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \
|
|
|
|
|
(_msg.target_component == MAV_COMP_ID_ALL)))
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
static uint32_t crc32_for_mission_item(const mavlink_mission_item_t &mission_item, uint32_t prev_crc32)
|
|
|
|
|
{
|
|
|
|
|
union {
|
|
|
|
|
CrcMissionItem_t item;
|
|
|
|
|
uint8_t raw[MISSION_ITEM_CRC_SIZE];
|
|
|
|
|
} u;
|
|
|
|
|
|
|
|
|
|
#if MAVLINK_NEED_BYTE_SWAP
|
|
|
|
|
_mav_put_uint8_t(u.raw, 0, mission_item.frame);
|
|
|
|
|
_mav_put_uint16_t(u.raw, 1, mission_item.command);
|
|
|
|
|
_mav_put_uint8_t(u.raw, 3, mission_item.autocontinue);
|
|
|
|
|
_mav_put_float(u.raw, 4, mission_item.param1);
|
|
|
|
|
_mav_put_float(u.raw, 8, mission_item.param2);
|
|
|
|
|
_mav_put_float(u.raw, 12, mission_item.param3);
|
|
|
|
|
_mav_put_float(u.raw, 16, mission_item.param4);
|
|
|
|
|
_mav_put_float(u.raw, 20, mission_item.x);
|
|
|
|
|
_mav_put_float(u.raw, 24, mission_item.y);
|
|
|
|
|
_mav_put_float(u.raw, 28, mission_item.z);
|
|
|
|
|
#else
|
|
|
|
|
u.item.frame = mission_item.frame;
|
|
|
|
|
u.item.command = mission_item.command;
|
|
|
|
|
u.item.autocontinue = mission_item.autocontinue;
|
|
|
|
|
u.item.params[0] = mission_item.param1;
|
|
|
|
|
u.item.params[1] = mission_item.param2;
|
|
|
|
|
u.item.params[2] = mission_item.param3;
|
|
|
|
|
u.item.params[3] = mission_item.param4;
|
|
|
|
|
u.item.params[4] = mission_item.x;
|
|
|
|
|
u.item.params[5] = mission_item.y;
|
|
|
|
|
u.item.params[6] = mission_item.z;
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
return crc32part(u.raw, sizeof(u), prev_crc32);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) :
|
|
|
|
|
_mavlink(mavlink)
|
|
|
|
|
{
|
|
|
|
@@ -101,6 +149,7 @@ MavlinkMissionManager::init_offboard_mission()
|
|
|
|
|
if (ret > 0) {
|
|
|
|
|
_dataman_id = (dm_item_t)mission_state.dataman_id;
|
|
|
|
|
_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_MISSION] = mission_state.checksum;
|
|
|
|
|
_current_seq = mission_state.current_seq;
|
|
|
|
|
|
|
|
|
|
} else if (ret < 0) {
|
|
|
|
@@ -124,6 +173,7 @@ MavlinkMissionManager::load_geofence_stats()
|
|
|
|
|
|
|
|
|
|
if (ret == sizeof(mission_stats_entry_s)) {
|
|
|
|
|
_count[MAV_MISSION_TYPE_FENCE] = stats.num_items;
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_FENCE] = stats.checksum;
|
|
|
|
|
_geofence_update_counter = stats.update_counter;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -139,6 +189,7 @@ MavlinkMissionManager::load_safepoint_stats()
|
|
|
|
|
|
|
|
|
|
if (ret == sizeof(mission_stats_entry_s)) {
|
|
|
|
|
_count[MAV_MISSION_TYPE_RALLY] = stats.num_items;
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_RALLY] = stats.checksum;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
@@ -148,7 +199,7 @@ MavlinkMissionManager::load_safepoint_stats()
|
|
|
|
|
* Publish mission topic to notify navigator about changes.
|
|
|
|
|
*/
|
|
|
|
|
int
|
|
|
|
|
MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
|
|
|
|
|
MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, uint32_t crc32)
|
|
|
|
|
{
|
|
|
|
|
// We want to make sure the whole struct is initialized including padding before getting written by dataman.
|
|
|
|
|
mission_s mission{};
|
|
|
|
@@ -156,6 +207,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
|
|
|
|
|
mission.dataman_id = dataman_id;
|
|
|
|
|
mission.count = count;
|
|
|
|
|
mission.current_seq = seq;
|
|
|
|
|
mission.checksum = crc32;
|
|
|
|
|
|
|
|
|
|
/* update mission state in dataman */
|
|
|
|
|
|
|
|
|
@@ -177,6 +229,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
|
|
|
|
|
/* update active mission state */
|
|
|
|
|
_dataman_id = dataman_id;
|
|
|
|
|
_count[MAV_MISSION_TYPE_MISSION] = count;
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_MISSION] = crc32;
|
|
|
|
|
_current_seq = seq;
|
|
|
|
|
_my_dataman_id = _dataman_id;
|
|
|
|
|
|
|
|
|
@@ -199,17 +252,19 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
|
MavlinkMissionManager::update_geofence_count(unsigned count)
|
|
|
|
|
MavlinkMissionManager::update_geofence_count(unsigned count, uint32_t crc32)
|
|
|
|
|
{
|
|
|
|
|
mission_stats_entry_s stats;
|
|
|
|
|
stats.num_items = count;
|
|
|
|
|
stats.update_counter = ++_geofence_update_counter; // this makes sure navigator will reload the fence data
|
|
|
|
|
stats.checksum = crc32;
|
|
|
|
|
|
|
|
|
|
/* update stats in dataman */
|
|
|
|
|
int res = dm_write(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
|
|
|
|
|
|
|
|
|
|
if (res == sizeof(mission_stats_entry_s)) {
|
|
|
|
|
_count[MAV_MISSION_TYPE_FENCE] = count;
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_FENCE] = crc32;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
|
|
|
|
@@ -226,17 +281,19 @@ MavlinkMissionManager::update_geofence_count(unsigned count)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int
|
|
|
|
|
MavlinkMissionManager::update_safepoint_count(unsigned count)
|
|
|
|
|
MavlinkMissionManager::update_safepoint_count(unsigned count, uint32_t crc32)
|
|
|
|
|
{
|
|
|
|
|
mission_stats_entry_s stats;
|
|
|
|
|
stats.num_items = count;
|
|
|
|
|
stats.update_counter = ++_safepoint_update_counter;
|
|
|
|
|
stats.checksum = crc32;
|
|
|
|
|
|
|
|
|
|
/* update stats in dataman */
|
|
|
|
|
int res = dm_write(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
|
|
|
|
|
|
|
|
|
|
if (res == sizeof(mission_stats_entry_s)) {
|
|
|
|
|
_count[MAV_MISSION_TYPE_RALLY] = count;
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_RALLY] = crc32;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
|
|
|
|
@@ -277,6 +334,57 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
|
|
|
|
|
PX4_DEBUG("WPM: Send MISSION_CURRENT seq %d", seq);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
|
* Combines two independently computed CRC32 checksums to a single checksum
|
|
|
|
|
* that is the same as if the checksum was computed over the concatenation
|
|
|
|
|
* of both blocks.
|
|
|
|
|
* Reference: https://stackoverflow.com/questions/23122312/crc-calculation-of-a-mostly-static-data-stream
|
|
|
|
|
*/
|
|
|
|
|
uint32_t crc32combine(uint32_t crc_a, uint32_t crc_b, size_t len_b)
|
|
|
|
|
{
|
|
|
|
|
size_t i;
|
|
|
|
|
uint8_t zero = 0;
|
|
|
|
|
|
|
|
|
|
for (i = 0; i < len_b; i++) {
|
|
|
|
|
crc_a = crc32part(&zero, 1, crc_a);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return (crc_a ^ crc_b);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
#if !defined(CONSTRAINED_FLASH)
|
|
|
|
|
bool MavlinkMissionManager::send_mission_checksum(MAV_MISSION_TYPE mission_type)
|
|
|
|
|
{
|
|
|
|
|
if (mission_type != MAV_MISSION_TYPE_MISSION &&
|
|
|
|
|
mission_type != MAV_MISSION_TYPE_FENCE &&
|
|
|
|
|
mission_type != MAV_MISSION_TYPE_RALLY &&
|
|
|
|
|
mission_type != MAV_MISSION_TYPE_ALL
|
|
|
|
|
) {
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_time_last_sent = hrt_absolute_time();
|
|
|
|
|
mavlink_mission_checksum_t wpc{};
|
|
|
|
|
wpc.mission_type = mission_type;
|
|
|
|
|
|
|
|
|
|
if (mission_type == MAV_MISSION_TYPE_ALL) {
|
|
|
|
|
// combine all individual checksums
|
|
|
|
|
uint32_t checksum = _crc32[MAV_MISSION_TYPE_MISSION];
|
|
|
|
|
checksum = crc32combine(checksum, _crc32[MAV_MISSION_TYPE_FENCE],
|
|
|
|
|
_count[MAV_MISSION_TYPE_FENCE] * MISSION_ITEM_CRC_SIZE);
|
|
|
|
|
checksum = crc32combine(checksum, _crc32[MAV_MISSION_TYPE_RALLY],
|
|
|
|
|
_count[MAV_MISSION_TYPE_RALLY] * MISSION_ITEM_CRC_SIZE);
|
|
|
|
|
wpc.checksum = checksum;
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
wpc.checksum = _crc32[mission_type];
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
mavlink_msg_mission_checksum_send_struct(_mavlink->get_channel(), &wpc);
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
void
|
|
|
|
|
MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type)
|
|
|
|
|
{
|
|
|
|
@@ -478,6 +586,8 @@ MavlinkMissionManager::send()
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
hrt_abstime t_now{hrt_absolute_time()};
|
|
|
|
|
|
|
|
|
|
mission_result_s mission_result{};
|
|
|
|
|
|
|
|
|
|
if (_mission_result_sub.update(&mission_result)) {
|
|
|
|
@@ -524,7 +634,7 @@ MavlinkMissionManager::send()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else if (_slow_rate_limiter.check(hrt_absolute_time())) {
|
|
|
|
|
} else if (_slow_rate_limiter.check(t_now)) {
|
|
|
|
|
if ((_count[MAV_MISSION_TYPE_MISSION] > 0) && (_current_seq >= 0)) {
|
|
|
|
|
|
|
|
|
|
send_mission_current(_current_seq);
|
|
|
|
@@ -537,6 +647,13 @@ MavlinkMissionManager::send()
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Send mission checksum at a low rate */
|
|
|
|
|
if (_checksum_rate_limiter.check(t_now)) {
|
|
|
|
|
send_mission_checksum(MAV_MISSION_TYPE_MISSION);
|
|
|
|
|
send_mission_checksum(MAV_MISSION_TYPE_FENCE);
|
|
|
|
|
send_mission_checksum(MAV_MISSION_TYPE_RALLY);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
/* check for timed-out operations */
|
|
|
|
|
if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0)
|
|
|
|
|
&& hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) {
|
|
|
|
@@ -679,7 +796,8 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg)
|
|
|
|
|
_time_last_recv = hrt_absolute_time();
|
|
|
|
|
|
|
|
|
|
if (wpc.seq < _count[MAV_MISSION_TYPE_MISSION]) {
|
|
|
|
|
if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq) == PX4_OK) {
|
|
|
|
|
if (update_active_mission(_dataman_id, _count[MAV_MISSION_TYPE_MISSION], wpc.seq,
|
|
|
|
|
_crc32[MAV_MISSION_TYPE_MISSION]) == PX4_OK) {
|
|
|
|
|
PX4_DEBUG("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
@@ -901,6 +1019,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|
|
|
|
|
|
|
|
|
_transfer_in_progress = true;
|
|
|
|
|
_mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
|
|
|
|
|
_transfer_current_crc32 = 0;
|
|
|
|
|
|
|
|
|
|
if (wpc.count > current_max_item_count()) {
|
|
|
|
|
PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count());
|
|
|
|
@@ -919,20 +1038,20 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
|
|
|
|
|
/* alternate dataman ID anyway to let navigator know about changes */
|
|
|
|
|
|
|
|
|
|
if (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0) {
|
|
|
|
|
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0);
|
|
|
|
|
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_1, 0, 0, 0);
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
|
|
|
|
|
update_active_mission(DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_FENCE:
|
|
|
|
|
update_geofence_count(0);
|
|
|
|
|
update_geofence_count(0, 0);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_RALLY:
|
|
|
|
|
update_safepoint_count(0);
|
|
|
|
|
update_safepoint_count(0, 0);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
default:
|
|
|
|
@@ -1113,6 +1232,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
_transfer_current_crc32 = crc32_for_mission_item(wp, _transfer_current_crc32);
|
|
|
|
|
|
|
|
|
|
bool write_failed = false;
|
|
|
|
|
bool check_failed = false;
|
|
|
|
|
|
|
|
|
@@ -1157,7 +1278,7 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|
|
|
|
if (mission_item.vertex_count < 3) { // feasibility check
|
|
|
|
|
PX4_ERR("Fence: too few vertices");
|
|
|
|
|
check_failed = true;
|
|
|
|
|
update_geofence_count(0);
|
|
|
|
|
update_geofence_count(0, 0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
@@ -1226,15 +1347,15 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|
|
|
|
|
|
|
|
|
switch (_mission_type) {
|
|
|
|
|
case MAV_MISSION_TYPE_MISSION:
|
|
|
|
|
ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq);
|
|
|
|
|
ret = update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq, _transfer_current_crc32);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_FENCE:
|
|
|
|
|
ret = update_geofence_count(_transfer_count);
|
|
|
|
|
ret = update_geofence_count(_transfer_count, _transfer_current_crc32);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_RALLY:
|
|
|
|
|
ret = update_safepoint_count(_transfer_count);
|
|
|
|
|
ret = update_safepoint_count(_transfer_count, _transfer_current_crc32);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
default:
|
|
|
|
@@ -1248,6 +1369,9 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
|
|
|
|
|
|
|
|
|
|
if (ret == PX4_OK) {
|
|
|
|
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED);
|
|
|
|
|
#if !defined(CONSTRAINED_FLASH)
|
|
|
|
|
send_mission_checksum(_mission_type);
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
|
} else {
|
|
|
|
|
send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR);
|
|
|
|
@@ -1281,22 +1405,22 @@ 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 == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
|
|
|
|
|
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
|
|
|
|
|
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_FENCE:
|
|
|
|
|
ret = update_geofence_count(0);
|
|
|
|
|
ret = update_geofence_count(0, 0);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_RALLY:
|
|
|
|
|
ret = update_safepoint_count(0);
|
|
|
|
|
ret = update_safepoint_count(0, 0);
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_MISSION_TYPE_ALL:
|
|
|
|
|
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;
|
|
|
|
|
DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, 0);
|
|
|
|
|
ret = update_geofence_count(0, 0) || ret;
|
|
|
|
|
ret = update_safepoint_count(0, 0) || ret;
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
default:
|
|
|
|
@@ -1400,8 +1524,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_TAKEOFF:
|
|
|
|
|
mission_item->nav_cmd = NAV_CMD_TAKEOFF;
|
|
|
|
|
mission_item->params[0] = mavlink_mission_item->param1; // Pitch
|
|
|
|
|
mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
|
|
|
|
|
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_LOITER_TO_ALT:
|
|
|
|
@@ -1435,8 +1559,14 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_VTOL_TAKEOFF:
|
|
|
|
|
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
|
|
|
|
|
mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_VTOL_LAND:
|
|
|
|
|
mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
|
|
|
|
|
mission_item->params[0] = mavlink_mission_item->param1; // Land options
|
|
|
|
|
mission_item->params[2] = mavlink_mission_item->param3; // Approach altitude
|
|
|
|
|
mission_item->yaw = wrap_2pi(math::radians(mavlink_mission_item->param4));
|
|
|
|
|
break;
|
|
|
|
|
|
|
|
|
|