mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-09 22:08:56 +08:00
fix(mavlink): Allowing custom modes to request offboard setpoints via the external mode registration (#26949)
* forwarding offboard setpoints in modes using the external mode registration * requesting offboard setpoint in RAPTOR * adding old message versions * bumping vehicle status * adding message translations * updating reference to versioned old message on previous translations
This commit is contained in:
Jonas Eschmann
GitHub
@@ -0,0 +1,24 @@
|
||||
# Request to register an external component
|
||||
|
||||
uint32 MESSAGE_VERSION = 1
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint64 request_id # ID, set this to a random value
|
||||
char[25] name # either the requested mode name, or component name
|
||||
|
||||
uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change.
|
||||
|
||||
uint16 px4_ros2_api_version # Set to LATEST_PX4_ROS2_API_VERSION
|
||||
|
||||
# Components to be registered
|
||||
bool register_arming_check
|
||||
bool register_mode # registering a mode also requires arming_check to be set
|
||||
bool register_mode_executor # registering an executor also requires a mode to be registered (which is the owned mode by the executor)
|
||||
|
||||
bool enable_replace_internal_mode # set to true if an internal mode should be replaced
|
||||
uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_*
|
||||
bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor)
|
||||
bool not_user_selectable # mode cannot be selected by the user
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 2
|
||||
@@ -0,0 +1,123 @@
|
||||
# Encodes the system state of the vehicle published by commander
|
||||
|
||||
uint32 MESSAGE_VERSION = 3
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
uint64 armed_time # Arming timestamp (microseconds)
|
||||
uint64 takeoff_time # Takeoff timestamp (microseconds)
|
||||
|
||||
uint8 arming_state
|
||||
uint8 ARMING_STATE_DISARMED = 1
|
||||
uint8 ARMING_STATE_ARMED = 2
|
||||
|
||||
uint8 latest_arming_reason
|
||||
uint8 latest_disarming_reason
|
||||
uint8 ARM_DISARM_REASON_STICK_GESTURE = 1
|
||||
uint8 ARM_DISARM_REASON_RC_SWITCH = 2
|
||||
uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3
|
||||
uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4
|
||||
uint8 ARM_DISARM_REASON_MISSION_START = 5
|
||||
uint8 ARM_DISARM_REASON_LANDING = 6
|
||||
uint8 ARM_DISARM_REASON_PREFLIGHT_INACTION = 7
|
||||
uint8 ARM_DISARM_REASON_KILL_SWITCH = 8
|
||||
uint8 ARM_DISARM_REASON_RC_BUTTON = 13
|
||||
uint8 ARM_DISARM_REASON_FAILSAFE = 14
|
||||
|
||||
uint64 nav_state_timestamp # time when current nav_state activated
|
||||
|
||||
uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation)
|
||||
|
||||
uint8 nav_state # Currently active mode
|
||||
uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode
|
||||
uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode
|
||||
uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode
|
||||
uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode
|
||||
uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode
|
||||
uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode
|
||||
uint8 NAVIGATION_STATE_POSITION_SLOW = 6
|
||||
uint8 NAVIGATION_STATE_FREE5 = 7
|
||||
uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode
|
||||
uint8 NAVIGATION_STATE_FREE3 = 9
|
||||
uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode
|
||||
uint8 NAVIGATION_STATE_FREE2 = 11
|
||||
uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control)
|
||||
uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode
|
||||
uint8 NAVIGATION_STATE_OFFBOARD = 14
|
||||
uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode
|
||||
uint8 NAVIGATION_STATE_FREE1 = 16
|
||||
uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff
|
||||
uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land
|
||||
uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow
|
||||
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target
|
||||
uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle
|
||||
uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter
|
||||
uint8 NAVIGATION_STATE_EXTERNAL1 = 23
|
||||
uint8 NAVIGATION_STATE_EXTERNAL2 = 24
|
||||
uint8 NAVIGATION_STATE_EXTERNAL3 = 25
|
||||
uint8 NAVIGATION_STATE_EXTERNAL4 = 26
|
||||
uint8 NAVIGATION_STATE_EXTERNAL5 = 27
|
||||
uint8 NAVIGATION_STATE_EXTERNAL6 = 28
|
||||
uint8 NAVIGATION_STATE_EXTERNAL7 = 29
|
||||
uint8 NAVIGATION_STATE_EXTERNAL8 = 30
|
||||
uint8 NAVIGATION_STATE_MAX = 31
|
||||
|
||||
uint8 executor_in_charge # Current mode executor in charge (0=Autopilot)
|
||||
uint8 nav_state_display # User-visible nav state sent via MAVLink (executor state if active, otherwise nav_state)
|
||||
|
||||
uint32 valid_nav_states_mask # Bitmask for all valid nav_state values
|
||||
uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select
|
||||
|
||||
uint8 hil_state
|
||||
uint8 HIL_STATE_OFF = 0
|
||||
uint8 HIL_STATE_ON = 1
|
||||
|
||||
# Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method)
|
||||
uint8 vehicle_type
|
||||
uint8 VEHICLE_TYPE_UNSPECIFIED = 0
|
||||
uint8 VEHICLE_TYPE_ROTARY_WING = 1
|
||||
uint8 VEHICLE_TYPE_FIXED_WING = 2
|
||||
uint8 VEHICLE_TYPE_ROVER = 3
|
||||
|
||||
uint8 FAILSAFE_DEFER_STATE_DISABLED = 0
|
||||
uint8 FAILSAFE_DEFER_STATE_ENABLED = 1
|
||||
uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe
|
||||
|
||||
bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
|
||||
bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control
|
||||
uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
|
||||
|
||||
# Link loss
|
||||
bool gcs_connection_lost # datalink to GCS lost
|
||||
uint8 gcs_connection_lost_counter # counts unique GCS connection lost events
|
||||
bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
|
||||
|
||||
# VTOL flags
|
||||
bool is_vtol # True if the system is VTOL capable
|
||||
bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW
|
||||
bool in_transition_mode # True if VTOL is doing a transition
|
||||
bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
|
||||
|
||||
# MAVLink identification
|
||||
uint8 system_type # system type, contains mavlink MAV_TYPE
|
||||
uint8 system_id # system id, contains MAVLink's system ID field
|
||||
uint8 component_id # subsystem / component id, contains MAVLink's component ID field
|
||||
|
||||
bool safety_button_available # Set to true if a safety button is connected
|
||||
bool safety_off # Set to true if safety is off
|
||||
|
||||
bool power_input_valid # set if input power is valid
|
||||
bool usb_connected # set to true (never cleared) once telemetry received from usb link
|
||||
|
||||
bool open_drone_id_system_present
|
||||
bool open_drone_id_system_healthy
|
||||
|
||||
bool parachute_system_present
|
||||
bool parachute_system_healthy
|
||||
|
||||
bool traffic_avoidance_system_present
|
||||
|
||||
bool rc_calibration_in_progress
|
||||
bool calibration_enabled
|
||||
|
||||
bool pre_flight_checks_pass # true if all checks necessary to arm pass
|
||||
@@ -16,9 +16,11 @@
|
||||
#include "translation_home_position_v1.h"
|
||||
#include "translation_register_ext_component_reply_v1.h"
|
||||
#include "translation_register_ext_component_request_v1.h"
|
||||
#include "translation_register_ext_component_request_v2.h"
|
||||
#include "translation_vehicle_attitude_setpoint_v1.h"
|
||||
#include "translation_vehicle_command_ack_v1.h"
|
||||
#include "translation_vehicle_local_position_v1.h"
|
||||
#include "translation_vehicle_status_v1.h"
|
||||
#include "translation_vehicle_status_v2.h"
|
||||
#include "translation_vehicle_status_v3.h"
|
||||
#include "translation_vehicle_status_v4.h"
|
||||
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
// Translate RegisterExtComponentRequest v0 <--> v1
|
||||
#include <px4_msgs_old/msg/register_ext_component_request_v0.hpp>
|
||||
#include <px4_msgs/msg/register_ext_component_request.hpp>
|
||||
#include <px4_msgs_old/msg/register_ext_component_request_v1.hpp>
|
||||
|
||||
class RegisterExtComponentRequestV1Translation {
|
||||
public:
|
||||
using MessageOlder = px4_msgs_old::msg::RegisterExtComponentRequestV0;
|
||||
static_assert(MessageOlder::MESSAGE_VERSION == 0);
|
||||
|
||||
using MessageNewer = px4_msgs::msg::RegisterExtComponentRequest;
|
||||
using MessageNewer = px4_msgs_old::msg::RegisterExtComponentRequestV1;
|
||||
static_assert(MessageNewer::MESSAGE_VERSION == 1);
|
||||
|
||||
static constexpr const char* kTopic = "fmu/in/register_ext_component_request";
|
||||
|
||||
@@ -0,0 +1,51 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2026 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
// Translate RegisterExtComponentRequest v1 <--> v2
|
||||
#include <px4_msgs_old/msg/register_ext_component_request_v1.hpp>
|
||||
#include <px4_msgs/msg/register_ext_component_request.hpp>
|
||||
|
||||
class RegisterExtComponentRequestV2Translation {
|
||||
public:
|
||||
using MessageOlder = px4_msgs_old::msg::RegisterExtComponentRequestV1;
|
||||
static_assert(MessageOlder::MESSAGE_VERSION == 1);
|
||||
|
||||
using MessageNewer = px4_msgs::msg::RegisterExtComponentRequest;
|
||||
static_assert(MessageNewer::MESSAGE_VERSION == 2);
|
||||
|
||||
static constexpr const char* kTopic = "fmu/in/register_ext_component_request";
|
||||
|
||||
static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
|
||||
msg_newer.timestamp = msg_older.timestamp;
|
||||
msg_newer.request_id = msg_older.request_id;
|
||||
msg_newer.name = msg_older.name;
|
||||
msg_newer.px4_ros2_api_version = msg_older.px4_ros2_api_version;
|
||||
msg_newer.register_arming_check = msg_older.register_arming_check;
|
||||
msg_newer.register_mode = msg_older.register_mode;
|
||||
msg_newer.register_mode_executor = msg_older.register_mode_executor;
|
||||
msg_newer.enable_replace_internal_mode = msg_older.enable_replace_internal_mode;
|
||||
msg_newer.replace_internal_mode = msg_older.replace_internal_mode;
|
||||
msg_newer.activate_mode_immediately = msg_older.activate_mode_immediately;
|
||||
msg_newer.not_user_selectable = msg_older.not_user_selectable;
|
||||
msg_newer.request_offboard_setpoints = false;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
|
||||
msg_older.timestamp = msg_newer.timestamp;
|
||||
msg_older.request_id = msg_newer.request_id;
|
||||
msg_older.name = msg_newer.name;
|
||||
msg_older.px4_ros2_api_version = msg_newer.px4_ros2_api_version;
|
||||
msg_older.register_arming_check = msg_newer.register_arming_check;
|
||||
msg_older.register_mode = msg_newer.register_mode;
|
||||
msg_older.register_mode_executor = msg_newer.register_mode_executor;
|
||||
msg_older.enable_replace_internal_mode = msg_newer.enable_replace_internal_mode;
|
||||
msg_older.replace_internal_mode = msg_newer.replace_internal_mode;
|
||||
msg_older.activate_mode_immediately = msg_newer.activate_mode_immediately;
|
||||
msg_older.not_user_selectable = msg_newer.not_user_selectable;
|
||||
}
|
||||
};
|
||||
|
||||
REGISTER_TOPIC_TRANSLATION_DIRECT(RegisterExtComponentRequestV2Translation);
|
||||
@@ -6,14 +6,14 @@
|
||||
|
||||
// Translate VehicleStatus v2 <--> v3
|
||||
#include <px4_msgs_old/msg/vehicle_status_v2.hpp>
|
||||
#include <px4_msgs/msg/vehicle_status.hpp>
|
||||
#include <px4_msgs_old/msg/vehicle_status_v3.hpp>
|
||||
|
||||
class VehicleStatusV3Translation {
|
||||
public:
|
||||
using MessageOlder = px4_msgs_old::msg::VehicleStatusV2;
|
||||
static_assert(MessageOlder::MESSAGE_VERSION == 2);
|
||||
|
||||
using MessageNewer = px4_msgs::msg::VehicleStatus;
|
||||
using MessageNewer = px4_msgs_old::msg::VehicleStatusV3;
|
||||
static_assert(MessageNewer::MESSAGE_VERSION == 3);
|
||||
|
||||
static constexpr const char* kTopic = "fmu/out/vehicle_status";
|
||||
|
||||
@@ -0,0 +1,109 @@
|
||||
/****************************************************************************
|
||||
* Copyright (c) 2026 PX4 Development Team.
|
||||
* SPDX-License-Identifier: BSD-3-Clause
|
||||
****************************************************************************/
|
||||
#pragma once
|
||||
|
||||
// Translate VehicleStatus v3 <--> v4
|
||||
#include <px4_msgs_old/msg/vehicle_status_v3.hpp>
|
||||
#include <px4_msgs/msg/vehicle_status.hpp>
|
||||
|
||||
class VehicleStatusV4Translation {
|
||||
public:
|
||||
using MessageOlder = px4_msgs_old::msg::VehicleStatusV3;
|
||||
static_assert(MessageOlder::MESSAGE_VERSION == 3);
|
||||
|
||||
using MessageNewer = px4_msgs::msg::VehicleStatus;
|
||||
static_assert(MessageNewer::MESSAGE_VERSION == 4);
|
||||
|
||||
static constexpr const char* kTopic = "fmu/out/vehicle_status";
|
||||
|
||||
static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
|
||||
msg_newer.timestamp = msg_older.timestamp;
|
||||
msg_newer.armed_time = msg_older.armed_time;
|
||||
msg_newer.takeoff_time = msg_older.takeoff_time;
|
||||
msg_newer.arming_state = msg_older.arming_state;
|
||||
msg_newer.latest_arming_reason = msg_older.latest_arming_reason;
|
||||
msg_newer.latest_disarming_reason = msg_older.latest_disarming_reason;
|
||||
msg_newer.nav_state_timestamp = msg_older.nav_state_timestamp;
|
||||
msg_newer.nav_state_user_intention = msg_older.nav_state_user_intention;
|
||||
msg_newer.nav_state = msg_older.nav_state;
|
||||
msg_newer.executor_in_charge = msg_older.executor_in_charge;
|
||||
msg_newer.nav_state_display = msg_older.nav_state_display;
|
||||
msg_newer.accepts_offboard_setpoints = false;
|
||||
msg_newer.valid_nav_states_mask = msg_older.valid_nav_states_mask;
|
||||
msg_newer.can_set_nav_states_mask = msg_older.can_set_nav_states_mask;
|
||||
msg_newer.hil_state = msg_older.hil_state;
|
||||
msg_newer.vehicle_type = msg_older.vehicle_type;
|
||||
msg_newer.failsafe = msg_older.failsafe;
|
||||
msg_newer.failsafe_and_user_took_over = msg_older.failsafe_and_user_took_over;
|
||||
msg_newer.failsafe_defer_state = msg_older.failsafe_defer_state;
|
||||
msg_newer.gcs_connection_lost = msg_older.gcs_connection_lost;
|
||||
msg_newer.gcs_connection_lost_counter = msg_older.gcs_connection_lost_counter;
|
||||
msg_newer.high_latency_data_link_lost = msg_older.high_latency_data_link_lost;
|
||||
msg_newer.is_vtol = msg_older.is_vtol;
|
||||
msg_newer.is_vtol_tailsitter = msg_older.is_vtol_tailsitter;
|
||||
msg_newer.in_transition_mode = msg_older.in_transition_mode;
|
||||
msg_newer.in_transition_to_fw = msg_older.in_transition_to_fw;
|
||||
msg_newer.system_type = msg_older.system_type;
|
||||
msg_newer.system_id = msg_older.system_id;
|
||||
msg_newer.component_id = msg_older.component_id;
|
||||
msg_newer.safety_button_available = msg_older.safety_button_available;
|
||||
msg_newer.safety_off = msg_older.safety_off;
|
||||
msg_newer.power_input_valid = msg_older.power_input_valid;
|
||||
msg_newer.usb_connected = msg_older.usb_connected;
|
||||
msg_newer.open_drone_id_system_present = msg_older.open_drone_id_system_present;
|
||||
msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy;
|
||||
msg_newer.parachute_system_present = msg_older.parachute_system_present;
|
||||
msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy;
|
||||
msg_newer.traffic_avoidance_system_present = msg_older.traffic_avoidance_system_present;
|
||||
msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress;
|
||||
msg_newer.calibration_enabled = msg_older.calibration_enabled;
|
||||
msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass;
|
||||
}
|
||||
|
||||
static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
|
||||
msg_older.timestamp = msg_newer.timestamp;
|
||||
msg_older.armed_time = msg_newer.armed_time;
|
||||
msg_older.takeoff_time = msg_newer.takeoff_time;
|
||||
msg_older.arming_state = msg_newer.arming_state;
|
||||
msg_older.latest_arming_reason = msg_newer.latest_arming_reason;
|
||||
msg_older.latest_disarming_reason = msg_newer.latest_disarming_reason;
|
||||
msg_older.nav_state_timestamp = msg_newer.nav_state_timestamp;
|
||||
msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention;
|
||||
msg_older.nav_state = msg_newer.nav_state;
|
||||
msg_older.executor_in_charge = msg_newer.executor_in_charge;
|
||||
msg_older.nav_state_display = msg_newer.nav_state_display;
|
||||
msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask;
|
||||
msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask;
|
||||
msg_older.hil_state = msg_newer.hil_state;
|
||||
msg_older.vehicle_type = msg_newer.vehicle_type;
|
||||
msg_older.failsafe = msg_newer.failsafe;
|
||||
msg_older.failsafe_and_user_took_over = msg_newer.failsafe_and_user_took_over;
|
||||
msg_older.failsafe_defer_state = msg_newer.failsafe_defer_state;
|
||||
msg_older.gcs_connection_lost = msg_newer.gcs_connection_lost;
|
||||
msg_older.gcs_connection_lost_counter = msg_newer.gcs_connection_lost_counter;
|
||||
msg_older.high_latency_data_link_lost = msg_newer.high_latency_data_link_lost;
|
||||
msg_older.is_vtol = msg_newer.is_vtol;
|
||||
msg_older.is_vtol_tailsitter = msg_newer.is_vtol_tailsitter;
|
||||
msg_older.in_transition_mode = msg_newer.in_transition_mode;
|
||||
msg_older.in_transition_to_fw = msg_newer.in_transition_to_fw;
|
||||
msg_older.system_type = msg_newer.system_type;
|
||||
msg_older.system_id = msg_newer.system_id;
|
||||
msg_older.component_id = msg_newer.component_id;
|
||||
msg_older.safety_button_available = msg_newer.safety_button_available;
|
||||
msg_older.safety_off = msg_newer.safety_off;
|
||||
msg_older.power_input_valid = msg_newer.power_input_valid;
|
||||
msg_older.usb_connected = msg_newer.usb_connected;
|
||||
msg_older.open_drone_id_system_present = msg_newer.open_drone_id_system_present;
|
||||
msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy;
|
||||
msg_older.parachute_system_present = msg_newer.parachute_system_present;
|
||||
msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy;
|
||||
msg_older.traffic_avoidance_system_present = msg_newer.traffic_avoidance_system_present;
|
||||
msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress;
|
||||
msg_older.calibration_enabled = msg_newer.calibration_enabled;
|
||||
msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass;
|
||||
}
|
||||
};
|
||||
|
||||
REGISTER_TOPIC_TRANSLATION_DIRECT(VehicleStatusV4Translation);
|
||||
@@ -1,6 +1,6 @@
|
||||
# Request to register an external component
|
||||
|
||||
uint32 MESSAGE_VERSION = 1
|
||||
uint32 MESSAGE_VERSION = 2
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
@@ -20,5 +20,6 @@ bool enable_replace_internal_mode # set to true if an internal mode should be r
|
||||
uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_*
|
||||
bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor)
|
||||
bool not_user_selectable # mode cannot be selected by the user
|
||||
bool request_offboard_setpoints # set to true if the registered mode wants to receive offboard trajectory setpoints via MAVLink
|
||||
|
||||
uint8 ORB_QUEUE_LENGTH = 2
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
# Encodes the system state of the vehicle published by commander
|
||||
|
||||
uint32 MESSAGE_VERSION = 3
|
||||
uint32 MESSAGE_VERSION = 4
|
||||
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
@@ -65,6 +65,8 @@ uint8 NAVIGATION_STATE_MAX = 31
|
||||
uint8 executor_in_charge # Current mode executor in charge (0=Autopilot)
|
||||
uint8 nav_state_display # User-visible nav state sent via MAVLink (executor state if active, otherwise nav_state)
|
||||
|
||||
bool accepts_offboard_setpoints # True if the current mode accepts offboard trajectory setpoints via MAVLink
|
||||
|
||||
uint32 valid_nav_states_mask # Bitmask for all valid nav_state values
|
||||
uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select
|
||||
|
||||
|
||||
@@ -2026,6 +2026,7 @@ void Commander::run()
|
||||
|
||||
// vehicle_status publish (after prearm/preflight updates above)
|
||||
_mode_management.getModeStatus(_vehicle_status.valid_nav_states_mask, _vehicle_status.can_set_nav_states_mask);
|
||||
_vehicle_status.accepts_offboard_setpoints = _mode_management.currentModeAcceptsOffboardSetpoints(_vehicle_status.nav_state);
|
||||
_vehicle_status.timestamp = hrt_absolute_time();
|
||||
_vehicle_status_pub.publish(_vehicle_status);
|
||||
|
||||
|
||||
@@ -295,6 +295,8 @@ void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
|
||||
mode.replaces_nav_state = request.replace_internal_mode;
|
||||
}
|
||||
|
||||
mode.request_offboard_setpoints = request.request_offboard_setpoints;
|
||||
|
||||
nav_mode_id = _modes.addExternalMode(mode);
|
||||
reply.mode_id = nav_mode_id;
|
||||
}
|
||||
@@ -671,4 +673,19 @@ void ModeManagement::getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can
|
||||
}
|
||||
}
|
||||
|
||||
bool ModeManagement::currentModeAcceptsOffboardSetpoints(uint8_t nav_state) const
|
||||
{
|
||||
// OFFBOARD mode always accepts offboard setpoints
|
||||
if (nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// Check if it's an external mode that requests offboard setpoints
|
||||
if (_modes.valid(nav_state)) {
|
||||
return _modes.mode(nav_state).request_offboard_setpoints;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
#endif /* CONSTRAINED_FLASH */
|
||||
|
||||
@@ -105,6 +105,7 @@ public:
|
||||
bool unresponsive_reported{false};
|
||||
int arming_check_registration_id{-1};
|
||||
int mode_executor_registration_id{-1};
|
||||
bool request_offboard_setpoints{false};
|
||||
config_overrides_s overrides{};
|
||||
vehicle_control_mode_s config_control_setpoint{};
|
||||
};
|
||||
@@ -169,6 +170,8 @@ public:
|
||||
|
||||
void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const;
|
||||
|
||||
bool currentModeAcceptsOffboardSetpoints(uint8_t nav_state) const;
|
||||
|
||||
void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out);
|
||||
|
||||
private:
|
||||
@@ -232,6 +235,11 @@ public:
|
||||
can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION);
|
||||
}
|
||||
|
||||
bool currentModeAcceptsOffboardSetpoints(uint8_t nav_state) const
|
||||
{
|
||||
return nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
|
||||
}
|
||||
|
||||
void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { }
|
||||
|
||||
private:
|
||||
|
||||
@@ -1169,8 +1169,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
|
||||
// only publish setpoint once in OFFBOARD
|
||||
if (vehicle_status.accepts_offboard_setpoints) {
|
||||
// only publish setpoint once in mode that accepts offboard setpoints
|
||||
setpoint.timestamp = hrt_absolute_time();
|
||||
_trajectory_setpoint_pub.publish(setpoint);
|
||||
}
|
||||
|
||||
@@ -260,6 +260,7 @@ bool Raptor::init()
|
||||
register_ext_component_request.register_mode = true;
|
||||
register_ext_component_request.enable_replace_internal_mode = _param_mc_raptor_offboard.get();
|
||||
register_ext_component_request.replace_internal_mode = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
|
||||
register_ext_component_request.request_offboard_setpoints = true;
|
||||
_register_ext_component_request_pub.publish(register_ext_component_request);
|
||||
|
||||
int32_t imu_gyro_ratemax = _param_imu_gyro_ratemax.get();
|
||||
|
||||
Reference in New Issue
Block a user