fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

mavlink: implement extended MISSION_CURRENT

Browse Source 
The message MISSION_CURRENT got extensions regarding mission state in
https://github.com/mavlink/mavlink/pull/1869

This is an attempt to actually populate those fields.
This commit is contained in:
Julian Oes 2025-06-13 13:38:43 +12:00
parent e0663cd6ad
commit 0180ad3a63
2 changed files with 67 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -278,9 +278,14 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
void
MavlinkMissionManager::send_mission_current(uint16_t seq)
{
// Update mission state before sending
update_mission_state();
mavlink_mission_current_t wpc{};
wpc.seq = seq;
wpc.total = _count[MAV_MISSION_TYPE_MISSION] > 0 ? _count[MAV_MISSION_TYPE_MISSION] : UINT16_MAX;
wpc.mission_state = static_cast<uint8_t>(_mission_state);
wpc.mission_mode = static_cast<uint8_t>(_mission_mode);
wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION];
wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE];
wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY];
@ -911,6 +916,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
_transfer_partner_compid = msg->compid;
_mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
_transfer_current_crc32 = 0;
_last_reached = -1; // Reset last reached waypoint when new mission starts
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());
@ -1907,3 +1913,51 @@ uint32_t MavlinkMissionManager::crc32_for_mission_item(const mavlink_mission_ite
return crc32part(u.raw, sizeof(u), prev_crc32);
}
void
MavlinkMissionManager::update_mission_state()
{
// Get vehicle status
vehicle_status_s vehicle_status;
if (!_vehicle_status_sub.update(&vehicle_status)) {
return;
}
// Get mission result
mission_result_s mission_result;
if (!_mission_result_sub.update(&mission_result)) {
return;
}
// Update mission mode
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
_mission_mode = MISSION_MODE_ACTIVE;
} else {
_mission_mode = MISSION_MODE_SUSPENDED;
}
// Update mission state
if (_count[MAV_MISSION_TYPE_MISSION] == 0) {
_mission_state = MISSION_STATE_NO_MISSION;
} else if (mission_result.finished) {
// Mission is complete if the navigator says it's finished
_mission_state = MISSION_STATE_COMPLETE;
} else if (_mission_mode == MISSION_MODE_ACTIVE
&& vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
_mission_state = MISSION_STATE_ACTIVE;
} else if (_mission_mode == MISSION_MODE_SUSPENDED && _last_reached >= 0) {
// Only PAUSED if we were actually in the middle of a mission
_mission_state = MISSION_STATE_PAUSED;
} else {
_mission_state = MISSION_STATE_NOT_STARTED;
}
}

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

@ -49,6 +49,7 @@
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/vehicle_status.h>
#include "mavlink_bridge_header.h"
#include "mavlink_rate_limiter.h"
@ -60,6 +61,13 @@ enum MAVLINK_WPM_STATES {
MAVLINK_WPM_STATE_ENUM_END
};
// Mission mode states
enum MISSION_MODE {
MISSION_MODE_UNKNOWN = 0,
MISSION_MODE_ACTIVE = 1,
MISSION_MODE_SUSPENDED = 2
};
enum MAVLINK_WPM_CODES {
MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
@ -94,9 +102,13 @@ public:
private:
enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE}; ///< Current state
enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION}; ///< mission type of current transmission (only one at a time possible)
enum MISSION_STATE _mission_state {MISSION_STATE_UNKNOWN}; ///< Current mission state machine state
enum MISSION_MODE _mission_mode {MISSION_MODE_UNKNOWN}; ///< Current mission mode
DatamanClient _dataman_client{};
void update_mission_state();
uint64_t _time_last_recv{0};
uint64_t _time_last_sent{0};
@ -138,6 +150,7 @@ private:
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
uORB::SubscriptionData<mission_s> _mission_sub{ORB_ID(mission)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription
uORB::Publication<mission_s> _offboard_mission_pub{ORB_ID(mission)};