forked from Archive/PX4-Autopilot
Compare commits
14 Commits
main
...
external_m
Author | SHA1 | Date |
---|---|---|
Frederik Markus | 6fef20988e | |
frederik | 4e8554f0a6 | |
Beat Küng | 8e0a2e38fe | |
Yannick Fuhrer | c7e11f1774 | |
Beat Küng | c0b9ecdc82 | |
Beat Küng | 0b37155ed6 | |
Beat Küng | 5b64e46e03 | |
Beat Küng | 3d6456dc5f | |
Matthias Grob | 3df333378a | |
Beat Küng | 8eefe05767 | |
Beat Küng | f9b34fe9d7 | |
Beat Küng | c57ee3fc24 | |
Beat Küng | ff9abf7001 | |
Beat Küng | 9cb8245851 |
|
@ -38,6 +38,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
|
||||||
|
|
||||||
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
|
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
|
||||||
|
|
||||||
|
|
||||||
# set local coordinate frame reference
|
# set local coordinate frame reference
|
||||||
if [ -n "${PX4_HOME_LAT}" ]; then
|
if [ -n "${PX4_HOME_LAT}" ]; then
|
||||||
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
|
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
|
||||||
|
@ -59,6 +60,9 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
|
||||||
. ../gz_env.sh
|
. ../gz_env.sh
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
# Only start up Gazebo if STANDALONE set to false
|
||||||
|
if [ "$STANDALONE" != '1' ]; then
|
||||||
|
|
||||||
# "gz sim" only avaiilable in Garden and later
|
# "gz sim" only avaiilable in Garden and later
|
||||||
GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
|
GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
|
||||||
if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
|
if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
|
||||||
|
@ -90,6 +94,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
|
||||||
echo "INFO [init] gazebo already running world: ${gz_world}"
|
echo "INFO [init] gazebo already running world: ${gz_world}"
|
||||||
PX4_GZ_WORLD=${gz_world}
|
PX4_GZ_WORLD=${gz_world}
|
||||||
fi
|
fi
|
||||||
|
fi
|
||||||
|
|
||||||
# start gz_bridge
|
# start gz_bridge
|
||||||
if [ -n "${PX4_GZ_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
|
if [ -n "${PX4_GZ_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
|
||||||
|
|
|
@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
|
||||||
return spec_temp.parsed_fields()
|
return spec_temp.parsed_fields()
|
||||||
|
|
||||||
|
|
||||||
|
def get_message_fields_str_for_message_hash(msg_fields, search_path):
|
||||||
|
"""
|
||||||
|
Get all fields (including for nested types) in the form of:
|
||||||
|
'''
|
||||||
|
uint64 timestamp
|
||||||
|
uint8 esc_count
|
||||||
|
uint8 esc_online_flags
|
||||||
|
EscReport[8] esc
|
||||||
|
uint64 timestamp
|
||||||
|
uint32 esc_errorcount
|
||||||
|
int32 esc_rpm
|
||||||
|
float32 esc_voltage
|
||||||
|
uint16 failures
|
||||||
|
int8 esc_power
|
||||||
|
'''
|
||||||
|
"""
|
||||||
|
all_fields_str = ''
|
||||||
|
for field in msg_fields:
|
||||||
|
if field.is_header:
|
||||||
|
continue
|
||||||
|
|
||||||
|
type_name = field.type
|
||||||
|
# detect embedded types
|
||||||
|
sl_pos = type_name.find('/')
|
||||||
|
if sl_pos >= 0:
|
||||||
|
type_name = type_name[sl_pos + 1:]
|
||||||
|
|
||||||
|
all_fields_str += type_name + ' ' + field.name + '\n'
|
||||||
|
|
||||||
|
if sl_pos >= 0: # nested type, add all nested fields
|
||||||
|
children_fields = get_children_fields(field.base_type, search_path)
|
||||||
|
all_fields_str += get_message_fields_str_for_message_hash(children_fields, search_path)
|
||||||
|
|
||||||
|
return all_fields_str
|
||||||
|
|
||||||
|
|
||||||
|
def hash_32_fnv1a(data: str):
|
||||||
|
hash_val = 0x811c9dc5
|
||||||
|
prime = 0x1000193
|
||||||
|
for i in range(len(data)):
|
||||||
|
value = ord(data[i])
|
||||||
|
hash_val = hash_val ^ value
|
||||||
|
hash_val *= prime
|
||||||
|
hash_val &= 0xffffffff
|
||||||
|
return hash_val
|
||||||
|
|
||||||
|
|
||||||
|
def get_message_hash(msg_fields, search_path):
|
||||||
|
"""
|
||||||
|
Get a 32 bit message hash over all fields
|
||||||
|
"""
|
||||||
|
all_fields_str = get_message_fields_str_for_message_hash(msg_fields, search_path)
|
||||||
|
return hash_32_fnv1a(all_fields_str)
|
||||||
|
|
||||||
|
|
||||||
def add_padding_bytes(fields, search_path):
|
def add_padding_bytes(fields, search_path):
|
||||||
"""
|
"""
|
||||||
Add padding fields before the embedded types, at the end and calculate the
|
Add padding fields before the embedded types, at the end and calculate the
|
||||||
|
|
|
@ -58,6 +58,7 @@ from px_generate_uorb_topic_helper import * # this is in Tools/
|
||||||
|
|
||||||
uorb_struct = '%s_s'%name_snake_case
|
uorb_struct = '%s_s'%name_snake_case
|
||||||
|
|
||||||
|
message_hash = get_message_hash(spec.parsed_fields(), search_path)
|
||||||
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
|
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
|
||||||
struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
|
struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
|
||||||
}@
|
}@
|
||||||
|
@ -74,7 +75,7 @@ struct_size, padding_end_size = add_padding_bytes(sorted_fields, search_path)
|
||||||
|
|
||||||
@[for topic in topics]@
|
@[for topic in topics]@
|
||||||
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
|
static_assert(static_cast<orb_id_size_t>(ORB_ID::@topic) == @(all_topics.index(topic)), "ORB_ID index mismatch");
|
||||||
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), static_cast<orb_id_size_t>(ORB_ID::@topic));
|
ORB_DEFINE(@topic, struct @uorb_struct, @(struct_size-padding_end_size), @(message_hash)u, static_cast<orb_id_size_t>(ORB_ID::@topic));
|
||||||
@[end for]
|
@[end for]
|
||||||
|
|
||||||
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
||||||
|
|
|
@ -250,6 +250,22 @@ class SourceParser(object):
|
||||||
event.group = "arming_check"
|
event.group = "arming_check"
|
||||||
event.prepend_arguments([('navigation_mode_group_t', 'modes'),
|
event.prepend_arguments([('navigation_mode_group_t', 'modes'),
|
||||||
('uint8_t', 'health_component_index')])
|
('uint8_t', 'health_component_index')])
|
||||||
|
elif call in ['reporter.healthFailureExt', 'reporter.armingCheckFailureExt']: # from ROS2
|
||||||
|
assert len(args_split) == num_args + 3, \
|
||||||
|
"Unexpected Number of arguments for: {:}, {:}".format(args_split, num_args)
|
||||||
|
m = self.re_event_id.search(args_split[0])
|
||||||
|
if m:
|
||||||
|
_, event_name = m.group(1, 2)
|
||||||
|
else:
|
||||||
|
raise Exception("Could not extract event ID from {:}".format(args_split[0]))
|
||||||
|
event.name = event_name
|
||||||
|
event.message = args_split[2][1:-1]
|
||||||
|
if 'health' in call:
|
||||||
|
event.group = "health"
|
||||||
|
else:
|
||||||
|
event.group = "arming_check"
|
||||||
|
event.prepend_arguments([('navigation_mode_group_t', 'modes'),
|
||||||
|
('uint8_t', 'health_component_index')])
|
||||||
else:
|
else:
|
||||||
raise Exception("unknown event method call: {}, args: {}".format(call, args))
|
raise Exception("unknown event method call: {}, args: {}".format(call, args))
|
||||||
|
|
||||||
|
|
|
@ -69,7 +69,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/body.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/body.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -184,7 +184,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/iris_prop_ccw.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -229,7 +229,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/left_aileron.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_aileron.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -385,7 +385,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/right_aileron.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_aileron.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -412,7 +412,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/left_flap.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_flap.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -439,7 +439,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/right_flap.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_flap.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -466,7 +466,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/elevators.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/elevators.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -493,7 +493,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.1 0.1 0.1</scale>
|
<scale>0.1 0.1 0.1</scale>
|
||||||
<uri>model://rc_cessna/meshes/rudder.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/rudder.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
|
|
@ -43,7 +43,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.001 0.001 0.001</scale>
|
<scale>0.001 0.001 0.001</scale>
|
||||||
<uri>model://standard_vtol/meshes/x8_wing.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_wing.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -183,7 +183,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -246,7 +246,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -309,7 +309,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -372,7 +372,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -436,7 +436,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.8 0.8 0.8</scale>
|
<scale>0.8 0.8 0.8</scale>
|
||||||
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -483,7 +483,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.001 0.001 0.001</scale>
|
<scale>0.001 0.001 0.001</scale>
|
||||||
<uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_left.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -510,7 +510,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.001 0.001 0.001</scale>
|
<scale>0.001 0.001 0.001</scale>
|
||||||
<uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_right.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
|
|
@ -23,7 +23,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/NXP-HGD-CF.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/NXP-HGD-CF.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -32,7 +32,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Base.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -41,7 +41,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Base.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -50,7 +50,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Base.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -59,7 +59,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Base.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -77,7 +77,7 @@
|
||||||
<specular>1.0 1.0 1.0</specular>
|
<specular>1.0 1.0 1.0</specular>
|
||||||
<pbr>
|
<pbr>
|
||||||
<metal>
|
<metal>
|
||||||
<albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
|
<albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
|
||||||
</metal>
|
</metal>
|
||||||
</pbr>
|
</pbr>
|
||||||
</material>
|
</material>
|
||||||
|
@ -96,7 +96,7 @@
|
||||||
<specular>1.0 1.0 1.0</specular>
|
<specular>1.0 1.0 1.0</specular>
|
||||||
<pbr>
|
<pbr>
|
||||||
<metal>
|
<metal>
|
||||||
<albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
|
<albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
|
||||||
</metal>
|
</metal>
|
||||||
</pbr>
|
</pbr>
|
||||||
</material>
|
</material>
|
||||||
|
@ -115,7 +115,7 @@
|
||||||
<specular>1.0 1.0 1.0</specular>
|
<specular>1.0 1.0 1.0</specular>
|
||||||
<pbr>
|
<pbr>
|
||||||
<metal>
|
<metal>
|
||||||
<albedo_map>model://x500/materials/textures/rd.png</albedo_map>
|
<albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/rd.png</albedo_map>
|
||||||
</metal>
|
</metal>
|
||||||
</pbr>
|
</pbr>
|
||||||
</material>
|
</material>
|
||||||
|
@ -250,7 +250,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
||||||
<uri>model://x500/meshes/1345_prop_ccw.stl</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -265,7 +265,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Bell.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -322,7 +322,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
||||||
<uri>model://x500/meshes/1345_prop_ccw.stl</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -337,7 +337,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Bell.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -394,7 +394,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
||||||
<uri>model://x500/meshes/1345_prop_cw.stl</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -409,7 +409,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Bell.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
@ -466,7 +466,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
|
||||||
<uri>model://x500/meshes/1345_prop_cw.stl</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
<material>
|
<material>
|
||||||
|
@ -481,7 +481,7 @@
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
<scale>1 1 1</scale>
|
<scale>1 1 1</scale>
|
||||||
<uri>model://x500/meshes/5010Bell.dae</uri>
|
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
|
||||||
</mesh>
|
</mesh>
|
||||||
</geometry>
|
</geometry>
|
||||||
</visual>
|
</visual>
|
||||||
|
|
|
@ -111,3 +111,4 @@ CONFIG_SYSTEMCMDS_UORB=y
|
||||||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||||
CONFIG_SYSTEMCMDS_VER=y
|
CONFIG_SYSTEMCMDS_VER=y
|
||||||
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
|
||||||
|
CONFIG_MAVLINK_DIALECT="development"
|
||||||
|
|
|
@ -0,0 +1,34 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
uint8 request_id
|
||||||
|
uint8 registration_id
|
||||||
|
|
||||||
|
uint8 HEALTH_COMPONENT_INDEX_NONE = 0
|
||||||
|
uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19
|
||||||
|
|
||||||
|
uint8 health_component_index # HEALTH_COMPONENT_INDEX_*
|
||||||
|
bool health_component_is_present
|
||||||
|
bool health_component_warning
|
||||||
|
bool health_component_error
|
||||||
|
|
||||||
|
bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run
|
||||||
|
|
||||||
|
uint8 num_events
|
||||||
|
|
||||||
|
Event[5] events
|
||||||
|
|
||||||
|
# Mode requirements
|
||||||
|
bool mode_req_angular_velocity
|
||||||
|
bool mode_req_attitude
|
||||||
|
bool mode_req_local_alt
|
||||||
|
bool mode_req_local_position
|
||||||
|
bool mode_req_local_position_relaxed
|
||||||
|
bool mode_req_global_position
|
||||||
|
bool mode_req_mission
|
||||||
|
bool mode_req_home_position
|
||||||
|
bool mode_req_prevent_arming
|
||||||
|
bool mode_req_manual_control
|
||||||
|
|
||||||
|
|
||||||
|
uint8 ORB_QUEUE_LENGTH = 4
|
||||||
|
|
|
@ -0,0 +1,7 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
# broadcast message to request all registered arming checks to be reported
|
||||||
|
|
||||||
|
uint8 request_id
|
||||||
|
|
||||||
|
|
|
@ -49,6 +49,8 @@ set(msg_files
|
||||||
Airspeed.msg
|
Airspeed.msg
|
||||||
AirspeedValidated.msg
|
AirspeedValidated.msg
|
||||||
AirspeedWind.msg
|
AirspeedWind.msg
|
||||||
|
ArmingCheckReply.msg
|
||||||
|
ArmingCheckRequest.msg
|
||||||
AutotuneAttitudeControlStatus.msg
|
AutotuneAttitudeControlStatus.msg
|
||||||
BatteryStatus.msg
|
BatteryStatus.msg
|
||||||
ButtonEvent.msg
|
ButtonEvent.msg
|
||||||
|
@ -58,6 +60,7 @@ set(msg_files
|
||||||
CellularStatus.msg
|
CellularStatus.msg
|
||||||
CollisionConstraints.msg
|
CollisionConstraints.msg
|
||||||
CollisionReport.msg
|
CollisionReport.msg
|
||||||
|
ConfigOverrides.msg
|
||||||
ControlAllocatorStatus.msg
|
ControlAllocatorStatus.msg
|
||||||
Cpuload.msg
|
Cpuload.msg
|
||||||
DatamanRequest.msg
|
DatamanRequest.msg
|
||||||
|
@ -130,6 +133,8 @@ set(msg_files
|
||||||
ManualControlSwitches.msg
|
ManualControlSwitches.msg
|
||||||
MavlinkLog.msg
|
MavlinkLog.msg
|
||||||
MavlinkTunnel.msg
|
MavlinkTunnel.msg
|
||||||
|
MessageFormatRequest.msg
|
||||||
|
MessageFormatResponse.msg
|
||||||
Mission.msg
|
Mission.msg
|
||||||
MissionResult.msg
|
MissionResult.msg
|
||||||
MountOrientation.msg
|
MountOrientation.msg
|
||||||
|
@ -161,6 +166,8 @@ set(msg_files
|
||||||
RateCtrlStatus.msg
|
RateCtrlStatus.msg
|
||||||
RcChannels.msg
|
RcChannels.msg
|
||||||
RcParameterMap.msg
|
RcParameterMap.msg
|
||||||
|
RegisterExtComponentReply.msg
|
||||||
|
RegisterExtComponentRequest.msg
|
||||||
Rpm.msg
|
Rpm.msg
|
||||||
RtlTimeEstimate.msg
|
RtlTimeEstimate.msg
|
||||||
SatelliteInfo.msg
|
SatelliteInfo.msg
|
||||||
|
@ -198,6 +205,7 @@ set(msg_files
|
||||||
UavcanParameterValue.msg
|
UavcanParameterValue.msg
|
||||||
UlogStream.msg
|
UlogStream.msg
|
||||||
UlogStreamAck.msg
|
UlogStreamAck.msg
|
||||||
|
UnregisterExtComponent.msg
|
||||||
VehicleAcceleration.msg
|
VehicleAcceleration.msg
|
||||||
VehicleAirData.msg
|
VehicleAirData.msg
|
||||||
VehicleAngularAccelerationSetpoint.msg
|
VehicleAngularAccelerationSetpoint.msg
|
||||||
|
|
|
@ -0,0 +1,19 @@
|
||||||
|
# Configurable overrides by (external) modes or mode executors
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured)
|
||||||
|
|
||||||
|
bool defer_failsafes # Defer all failsafes that can be deferred (until the flag is cleared)
|
||||||
|
int16 defer_failsafes_timeout_s # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout
|
||||||
|
|
||||||
|
|
||||||
|
int8 SOURCE_TYPE_MODE = 0
|
||||||
|
int8 SOURCE_TYPE_MODE_EXECUTOR = 1
|
||||||
|
int8 source_type
|
||||||
|
|
||||||
|
uint8 source_id # ID depending on source_type
|
||||||
|
|
||||||
|
uint8 ORB_QUEUE_LENGTH = 4
|
||||||
|
|
||||||
|
# TOPICS config_overrides config_overrides_request
|
||||||
|
|
|
@ -0,0 +1,10 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
# Request to PX4 to get the hash of a message, to check for message compatibility
|
||||||
|
|
||||||
|
uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes.
|
||||||
|
|
||||||
|
uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
|
||||||
|
|
||||||
|
char[50] topic_name # E.g. /fmu/in/vehicle_command
|
||||||
|
|
|
@ -0,0 +1,11 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
# Response from PX4 with the format of a message
|
||||||
|
|
||||||
|
uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp
|
||||||
|
|
||||||
|
char[50] topic_name # E.g. /fmu/in/vehicle_command
|
||||||
|
|
||||||
|
bool success
|
||||||
|
uint32 message_hash # hash over all message fields
|
||||||
|
|
|
@ -0,0 +1,14 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
uint64 request_id # ID from the request
|
||||||
|
char[25] name # name from the request
|
||||||
|
|
||||||
|
uint16 px4_ros2_api_version
|
||||||
|
|
||||||
|
bool success
|
||||||
|
int8 arming_check_id # arming check registration ID (-1 if invalid)
|
||||||
|
int8 mode_id # assigned mode ID (-1 if invalid)
|
||||||
|
int8 mode_executor_id # assigned mode executor ID (-1 if invalid)
|
||||||
|
|
||||||
|
uint8 ORB_QUEUE_LENGTH = 2
|
||||||
|
|
|
@ -0,0 +1,21 @@
|
||||||
|
# Request to register an external component
|
||||||
|
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)
|
||||||
|
|
||||||
|
|
||||||
|
uint8 ORB_QUEUE_LENGTH = 2
|
|
@ -0,0 +1,10 @@
|
||||||
|
uint64 timestamp # time since system start (microseconds)
|
||||||
|
|
||||||
|
char[25] name # either the mode name, or component name
|
||||||
|
|
||||||
|
int8 arming_check_id # arming check registration ID (-1 if not registered)
|
||||||
|
int8 mode_id # assigned mode ID (-1 if not registered)
|
||||||
|
int8 mode_executor_id # assigned mode executor ID (-1 if not registered)
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -71,6 +71,7 @@ uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 ==
|
||||||
uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
|
uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
|
||||||
uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
|
uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
|
||||||
uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
|
uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
|
||||||
|
uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVLink mode |MAV_STANDARD_MODE|
|
||||||
uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal
|
uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal
|
||||||
|
|
||||||
uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)|
|
uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)|
|
||||||
|
@ -103,6 +104,7 @@ uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch.
|
||||||
# PX4 vehicle commands (beyond 16 bit mavlink commands)
|
# PX4 vehicle commands (beyond 16 bit mavlink commands)
|
||||||
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX)
|
uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX)
|
||||||
uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude|
|
uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude|
|
||||||
|
uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty|
|
||||||
|
|
||||||
uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
|
uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
|
||||||
uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
|
uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
|
||||||
|
@ -174,8 +176,10 @@ uint32 command # Command ID
|
||||||
uint8 target_system # System which should execute the command
|
uint8 target_system # System which should execute the command
|
||||||
uint8 target_component # Component which should execute the command, 0 for all components
|
uint8 target_component # Component which should execute the command, 0 for all components
|
||||||
uint8 source_system # System sending the command
|
uint8 source_system # System sending the command
|
||||||
uint8 source_component # Component sending the command
|
uint16 source_component # Component / mode executor sending the command
|
||||||
uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
|
uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
|
||||||
bool from_external
|
bool from_external
|
||||||
|
|
||||||
# TOPICS vehicle_command gimbal_v1_command
|
uint16 COMPONENT_MODE_EXECUTOR_START = 1000
|
||||||
|
|
||||||
|
# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor
|
||||||
|
|
|
@ -28,6 +28,6 @@ uint8 result # Command result
|
||||||
uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
|
uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
|
||||||
int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
|
int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
|
||||||
uint8 target_system
|
uint8 target_system
|
||||||
uint8 target_component
|
uint16 target_component # Target component / mode executor
|
||||||
|
|
||||||
bool from_external # Indicates if the command came from an external source
|
bool from_external # Indicates if the command came from an external source
|
||||||
|
|
|
@ -15,3 +15,8 @@ bool flag_control_altitude_enabled # true if altitude is controlled
|
||||||
bool flag_control_climb_rate_enabled # true if climb rate is controlled
|
bool flag_control_climb_rate_enabled # true if climb rate is controlled
|
||||||
bool flag_control_termination_enabled # true if flighttermination is enabled
|
bool flag_control_termination_enabled # true if flighttermination is enabled
|
||||||
bool flag_control_allocation_enabled # true if control allocation is enabled
|
bool flag_control_allocation_enabled # true if control allocation is enabled
|
||||||
|
|
||||||
|
# TODO: use dedicated topic for external requests
|
||||||
|
uint8 source_id # Mode ID (nav_state)
|
||||||
|
|
||||||
|
# TOPICS vehicle_control_mode config_control_setpoints
|
||||||
|
|
|
@ -52,7 +52,20 @@ uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow
|
||||||
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target
|
uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target
|
||||||
uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle
|
uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle
|
||||||
uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter
|
uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter
|
||||||
uint8 NAVIGATION_STATE_MAX = 23
|
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)
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
# Bitmask of detected failures
|
# Bitmask of detected failures
|
||||||
uint16 failure_detector_status
|
uint16 failure_detector_status
|
||||||
|
@ -78,8 +91,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
|
||||||
uint8 VEHICLE_TYPE_ROVER = 3
|
uint8 VEHICLE_TYPE_ROVER = 3
|
||||||
uint8 VEHICLE_TYPE_AIRSHIP = 4
|
uint8 VEHICLE_TYPE_AIRSHIP = 4
|
||||||
|
|
||||||
|
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 # 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
|
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
|
# Link loss
|
||||||
bool gcs_connection_lost # datalink to GCS lost
|
bool gcs_connection_lost # datalink to GCS lost
|
||||||
|
|
|
@ -51,6 +51,7 @@ struct orb_metadata {
|
||||||
const char *o_name; /**< unique object name */
|
const char *o_name; /**< unique object name */
|
||||||
const uint16_t o_size; /**< object size */
|
const uint16_t o_size; /**< object size */
|
||||||
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
|
const uint16_t o_size_no_padding; /**< object size w/o padding at the end (for logger) */
|
||||||
|
uint32_t message_hash; /**< Hash over all fields for message compatibility checks */
|
||||||
orb_id_size_t o_id; /**< ORB_ID enum */
|
orb_id_size_t o_id; /**< ORB_ID enum */
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -99,13 +100,15 @@ typedef const struct orb_metadata *orb_id_t;
|
||||||
* @param _name The name of the topic.
|
* @param _name The name of the topic.
|
||||||
* @param _struct The structure the topic provides.
|
* @param _struct The structure the topic provides.
|
||||||
* @param _size_no_padding Struct size w/o padding at the end
|
* @param _size_no_padding Struct size w/o padding at the end
|
||||||
|
* @param _message_hash 32 bit message hash over all fields
|
||||||
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
|
* @param _orb_id_enum ORB ID enum e.g.: ORB_ID::vehicle_status
|
||||||
*/
|
*/
|
||||||
#define ORB_DEFINE(_name, _struct, _size_no_padding, _orb_id_enum) \
|
#define ORB_DEFINE(_name, _struct, _size_no_padding, _message_hash, _orb_id_enum) \
|
||||||
const struct orb_metadata __orb_##_name = { \
|
const struct orb_metadata __orb_##_name = { \
|
||||||
#_name, \
|
#_name, \
|
||||||
sizeof(_struct), \
|
sizeof(_struct), \
|
||||||
_size_no_padding, \
|
_size_no_padding, \
|
||||||
|
_message_hash, \
|
||||||
_orb_id_enum \
|
_orb_id_enum \
|
||||||
}; struct hack
|
}; struct hack
|
||||||
|
|
||||||
|
|
|
@ -110,6 +110,38 @@
|
||||||
"4194304": {
|
"4194304": {
|
||||||
"name": "vtol_takeoff",
|
"name": "vtol_takeoff",
|
||||||
"description": "VTOL Takeoff"
|
"description": "VTOL Takeoff"
|
||||||
|
},
|
||||||
|
"8388608": {
|
||||||
|
"name": "external1",
|
||||||
|
"description": "External 1"
|
||||||
|
},
|
||||||
|
"16777216": {
|
||||||
|
"name": "external2",
|
||||||
|
"description": "External 2"
|
||||||
|
},
|
||||||
|
"33554432": {
|
||||||
|
"name": "external3",
|
||||||
|
"description": "External 3"
|
||||||
|
},
|
||||||
|
"67108864": {
|
||||||
|
"name": "external4",
|
||||||
|
"description": "External 4"
|
||||||
|
},
|
||||||
|
"134217728": {
|
||||||
|
"name": "external5",
|
||||||
|
"description": "External 5"
|
||||||
|
},
|
||||||
|
"268435456": {
|
||||||
|
"name": "external6",
|
||||||
|
"description": "External 6"
|
||||||
|
},
|
||||||
|
"536870912": {
|
||||||
|
"name": "external7",
|
||||||
|
"description": "External 7"
|
||||||
|
},
|
||||||
|
"1073741824": {
|
||||||
|
"name": "external8",
|
||||||
|
"description": "External 8"
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
|
@ -532,6 +564,38 @@
|
||||||
"name": "auto_vtol_takeoff",
|
"name": "auto_vtol_takeoff",
|
||||||
"description": "Vtol Takeoff"
|
"description": "Vtol Takeoff"
|
||||||
},
|
},
|
||||||
|
"16": {
|
||||||
|
"name": "external1",
|
||||||
|
"description": "External 1"
|
||||||
|
},
|
||||||
|
"17": {
|
||||||
|
"name": "external2",
|
||||||
|
"description": "External 2"
|
||||||
|
},
|
||||||
|
"18": {
|
||||||
|
"name": "external3",
|
||||||
|
"description": "External 3"
|
||||||
|
},
|
||||||
|
"19": {
|
||||||
|
"name": "external4",
|
||||||
|
"description": "External 4"
|
||||||
|
},
|
||||||
|
"20": {
|
||||||
|
"name": "external5",
|
||||||
|
"description": "External 5"
|
||||||
|
},
|
||||||
|
"21": {
|
||||||
|
"name": "external6",
|
||||||
|
"description": "External 6"
|
||||||
|
},
|
||||||
|
"22": {
|
||||||
|
"name": "external7",
|
||||||
|
"description": "External 7"
|
||||||
|
},
|
||||||
|
"23": {
|
||||||
|
"name": "external8",
|
||||||
|
"description": "External 8"
|
||||||
|
},
|
||||||
"255": {
|
"255": {
|
||||||
"name": "unknown",
|
"name": "unknown",
|
||||||
"description": "[Unknown]"
|
"description": "[Unknown]"
|
||||||
|
@ -705,7 +769,15 @@
|
||||||
"19": [134479872],
|
"19": [134479872],
|
||||||
"20": [151257088],
|
"20": [151257088],
|
||||||
"21": [16973824],
|
"21": [16973824],
|
||||||
"22": [168034304]
|
"22": [168034304],
|
||||||
|
"23": [184811520],
|
||||||
|
"24": [201588736],
|
||||||
|
"25": [218365952],
|
||||||
|
"26": [235143168],
|
||||||
|
"27": [251920384],
|
||||||
|
"28": [268697600],
|
||||||
|
"29": [285474816],
|
||||||
|
"30": [302252032]
|
||||||
}
|
}
|
||||||
},
|
},
|
||||||
"supported_protocols": [ "health_and_arming_check" ]
|
"supported_protocols": [ "health_and_arming_check" ]
|
||||||
|
|
|
@ -38,7 +38,7 @@
|
||||||
#include <uORB/topics/vehicle_command.h>
|
#include <uORB/topics/vehicle_command.h>
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
|
* Functions: Peripheral_via_Actuator_Set1 ... Peripheral_via_Actuator_Set6
|
||||||
*/
|
*/
|
||||||
class FunctionActuatorSet : public FunctionProviderBase
|
class FunctionActuatorSet : public FunctionProviderBase
|
||||||
{
|
{
|
||||||
|
@ -72,7 +72,7 @@ public:
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }
|
float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Peripheral_via_Actuator_Set1]; }
|
||||||
|
|
||||||
private:
|
private:
|
||||||
static constexpr int max_num_actuators = 6;
|
static constexpr int max_num_actuators = 6;
|
||||||
|
|
|
@ -57,7 +57,7 @@ static const FunctionProvider all_function_providers[] = {
|
||||||
{OutputFunction::Constant_Max, &FunctionConstantMax::allocate},
|
{OutputFunction::Constant_Max, &FunctionConstantMax::allocate},
|
||||||
{OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate},
|
{OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::allocate},
|
||||||
{OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate},
|
{OutputFunction::Servo1, OutputFunction::ServoMax, &FunctionServos::allocate},
|
||||||
{OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate},
|
{OutputFunction::Peripheral_via_Actuator_Set1, OutputFunction::Peripheral_via_Actuator_Set6, &FunctionActuatorSet::allocate},
|
||||||
{OutputFunction::Landing_Gear, &FunctionLandingGear::allocate},
|
{OutputFunction::Landing_Gear, &FunctionLandingGear::allocate},
|
||||||
{OutputFunction::Landing_Gear_Wheel, &FunctionLandingGearWheel::allocate},
|
{OutputFunction::Landing_Gear_Wheel, &FunctionLandingGearWheel::allocate},
|
||||||
{OutputFunction::Parachute, &FunctionParachute::allocate},
|
{OutputFunction::Parachute, &FunctionParachute::allocate},
|
||||||
|
|
|
@ -13,7 +13,7 @@ functions:
|
||||||
Servo:
|
Servo:
|
||||||
start: 201
|
start: 201
|
||||||
count: 8
|
count: 8
|
||||||
Offboard_Actuator_Set:
|
Peripheral_via_Actuator_Set:
|
||||||
start: 301
|
start: 301
|
||||||
count: 6
|
count: 6
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1,97 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
namespace mode_util
|
||||||
|
{
|
||||||
|
|
||||||
|
// This matches the definition from MAVLink MAV_STANDARD_MODE
|
||||||
|
enum class StandardMode : uint8_t {
|
||||||
|
NON_STANDARD = 0,
|
||||||
|
POSITION_HOLD = 1,
|
||||||
|
ORBIT = 2,
|
||||||
|
CRUISE = 3,
|
||||||
|
ALTITUDE_HOLD = 4,
|
||||||
|
RETURN_HOME = 5,
|
||||||
|
SAFE_RECOVERY = 6,
|
||||||
|
MISSION = 7,
|
||||||
|
LAND = 8,
|
||||||
|
TAKEOFF = 9,
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @return Get MAVLink standard mode from nav_state
|
||||||
|
*/
|
||||||
|
static inline StandardMode getStandardModeFromNavState(uint8_t nav_state)
|
||||||
|
{
|
||||||
|
switch (nav_state) {
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: return StandardMode::RETURN_HOME;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: return StandardMode::MISSION;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND: return StandardMode::LAND;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: return StandardMode::TAKEOFF;
|
||||||
|
// Note: all other standard modes do not directly map, or are vehicle-type specific
|
||||||
|
}
|
||||||
|
|
||||||
|
return StandardMode::NON_STANDARD;
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @return Get nav_state from a standard mode, or vehicle_status_s::NAVIGATION_STATE_MAX if not supported
|
||||||
|
*/
|
||||||
|
static inline uint8_t getNavStateFromStandardMode(StandardMode mode)
|
||||||
|
{
|
||||||
|
switch (mode) {
|
||||||
|
case StandardMode::RETURN_HOME: return vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
|
||||||
|
|
||||||
|
case StandardMode::MISSION: return vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
|
||||||
|
|
||||||
|
case StandardMode::LAND: return vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
|
||||||
|
|
||||||
|
case StandardMode::TAKEOFF: return vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
|
||||||
|
|
||||||
|
default: break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return vehicle_status_s::NAVIGATION_STATE_MAX;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
} // namespace mode_util
|
|
@ -0,0 +1,133 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
namespace mode_util
|
||||||
|
{
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @return Bitmask with all valid modes
|
||||||
|
*/
|
||||||
|
static inline uint32_t getValidNavStates()
|
||||||
|
{
|
||||||
|
return (1u << vehicle_status_s::NAVIGATION_STATE_MANUAL) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_ALTCTL) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_POSCTL) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_ACRO) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_OFFBOARD) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_STAB) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_ORBIT) |
|
||||||
|
(1u << vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF);
|
||||||
|
|
||||||
|
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update");
|
||||||
|
}
|
||||||
|
|
||||||
|
const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
|
||||||
|
"Manual",
|
||||||
|
"Altitude",
|
||||||
|
"Position",
|
||||||
|
"Mission",
|
||||||
|
"Hold",
|
||||||
|
"Return",
|
||||||
|
"6: unallocated",
|
||||||
|
"7: unallocated",
|
||||||
|
"8: unallocated",
|
||||||
|
"9: unallocated",
|
||||||
|
"Acro",
|
||||||
|
"11: UNUSED",
|
||||||
|
"Descend",
|
||||||
|
"Termination",
|
||||||
|
"Offboard",
|
||||||
|
"Stabilized",
|
||||||
|
"16: UNUSED2",
|
||||||
|
"Takeoff",
|
||||||
|
"Land",
|
||||||
|
"Follow Target",
|
||||||
|
"Precision Landing",
|
||||||
|
"Orbit",
|
||||||
|
"VTOL Takeoff",
|
||||||
|
"External 1",
|
||||||
|
"External 2",
|
||||||
|
"External 3",
|
||||||
|
"External 4",
|
||||||
|
"External 5",
|
||||||
|
"External 6",
|
||||||
|
"External 7",
|
||||||
|
"External 8",
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @return returns true for advanced modes
|
||||||
|
*/
|
||||||
|
static inline bool isAdvanced(uint8_t nav_state)
|
||||||
|
{
|
||||||
|
switch (nav_state) {
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_ALTCTL: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_POSCTL: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return false;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return false;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace mode_util
|
|
@ -1,6 +1,6 @@
|
||||||
############################################################################
|
############################################################################
|
||||||
#
|
#
|
||||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
# Copyright (c) 2015-2023 PX4 Development Team. All rights reserved.
|
||||||
#
|
#
|
||||||
# Redistribution and use in source and binary forms, with or without
|
# Redistribution and use in source and binary forms, with or without
|
||||||
# modification, are permitted provided that the following conditions
|
# modification, are permitted provided that the following conditions
|
||||||
|
@ -31,11 +31,12 @@
|
||||||
#
|
#
|
||||||
############################################################################
|
############################################################################
|
||||||
|
|
||||||
|
add_subdirectory(Arming)
|
||||||
|
add_subdirectory(failsafe)
|
||||||
add_subdirectory(failure_detector)
|
add_subdirectory(failure_detector)
|
||||||
add_subdirectory(HealthAndArmingChecks)
|
add_subdirectory(HealthAndArmingChecks)
|
||||||
add_subdirectory(failsafe)
|
|
||||||
add_subdirectory(Arming)
|
|
||||||
add_subdirectory(ModeUtil)
|
add_subdirectory(ModeUtil)
|
||||||
|
add_subdirectory(MulticopterThrowLaunch)
|
||||||
|
|
||||||
px4_add_module(
|
px4_add_module(
|
||||||
MODULE modules__commander
|
MODULE modules__commander
|
||||||
|
@ -52,24 +53,30 @@ px4_add_module(
|
||||||
factory_calibration_storage.cpp
|
factory_calibration_storage.cpp
|
||||||
gyro_calibration.cpp
|
gyro_calibration.cpp
|
||||||
HomePosition.cpp
|
HomePosition.cpp
|
||||||
|
ModeManagement.cpp
|
||||||
UserModeIntention.cpp
|
UserModeIntention.cpp
|
||||||
level_calibration.cpp
|
level_calibration.cpp
|
||||||
lm_fit.cpp
|
lm_fit.cpp
|
||||||
mag_calibration.cpp
|
mag_calibration.cpp
|
||||||
rc_calibration.cpp
|
rc_calibration.cpp
|
||||||
Safety.cpp
|
Safety.cpp
|
||||||
|
UserModeIntention.cpp
|
||||||
worker_thread.cpp
|
worker_thread.cpp
|
||||||
|
MODULE_CONFIG
|
||||||
|
module.yaml
|
||||||
DEPENDS
|
DEPENDS
|
||||||
|
ArmAuthorization
|
||||||
circuit_breaker
|
circuit_breaker
|
||||||
|
failsafe
|
||||||
failure_detector
|
failure_detector
|
||||||
geo
|
geo
|
||||||
health_and_arming_checks
|
health_and_arming_checks
|
||||||
hysteresis
|
hysteresis
|
||||||
ArmAuthorization
|
mode_util
|
||||||
|
MulticopterThrowLaunch
|
||||||
sensor_calibration
|
sensor_calibration
|
||||||
world_magnetic_model
|
world_magnetic_model
|
||||||
mode_util
|
|
||||||
failsafe
|
|
||||||
)
|
)
|
||||||
|
|
||||||
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
|
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)
|
||||||
|
px4_add_functional_gtest(SRC ModeManagementTest.cpp LINKLIBS modules__commander)
|
||||||
|
|
|
@ -48,6 +48,8 @@
|
||||||
#include "px4_custom_mode.h"
|
#include "px4_custom_mode.h"
|
||||||
#include "ModeUtil/control_mode.hpp"
|
#include "ModeUtil/control_mode.hpp"
|
||||||
#include "ModeUtil/conversions.hpp"
|
#include "ModeUtil/conversions.hpp"
|
||||||
|
#include <lib/modes/ui.hpp>
|
||||||
|
#include <lib/modes/standard_modes.hpp>
|
||||||
|
|
||||||
/* PX4 headers */
|
/* PX4 headers */
|
||||||
#include <drivers/drv_hrt.h>
|
#include <drivers/drv_hrt.h>
|
||||||
|
@ -407,6 +409,10 @@ int Commander::custom_command(int argc, char *argv[])
|
||||||
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
|
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
|
||||||
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
|
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND);
|
||||||
|
|
||||||
|
} else if (!strcmp(argv[1], "ext1")) {
|
||||||
|
send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL1);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
PX4_ERR("argument %s unsupported.", argv[1]);
|
PX4_ERR("argument %s unsupported.", argv[1]);
|
||||||
}
|
}
|
||||||
|
@ -475,8 +481,9 @@ int Commander::print_status()
|
||||||
{
|
{
|
||||||
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
|
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
|
||||||
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
|
PX4_INFO("navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state]);
|
||||||
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_user_mode_intention.get()]);
|
PX4_INFO("user intended navigation mode: %s", mode_util::nav_state_names[_vehicle_status.nav_state_user_intention]);
|
||||||
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
|
PX4_INFO("in failsafe: %s", _failsafe.inFailsafe() ? "yes" : "no");
|
||||||
|
_mode_management.printStatus();
|
||||||
perf_print_counter(_loop_perf);
|
perf_print_counter(_loop_perf);
|
||||||
perf_print_counter(_preflight_check_perf);
|
perf_print_counter(_preflight_check_perf);
|
||||||
return 0;
|
return 0;
|
||||||
|
@ -727,7 +734,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
|
||||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -807,6 +814,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case PX4_CUSTOM_SUB_MODE_EXTERNAL1...PX4_CUSTOM_SUB_MODE_EXTERNAL8:
|
||||||
|
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + (custom_sub_mode - PX4_CUSTOM_SUB_MODE_EXTERNAL1);
|
||||||
|
break;
|
||||||
|
|
||||||
default:
|
default:
|
||||||
main_ret = TRANSITION_DENIED;
|
main_ret = TRANSITION_DENIED;
|
||||||
mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
|
mavlink_log_critical(&_mavlink_log_pub, "Unsupported auto mode\t");
|
||||||
|
@ -848,7 +859,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
}
|
}
|
||||||
|
|
||||||
if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
|
if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
|
||||||
if (_user_mode_intention.change(desired_nav_state)) {
|
if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
|
||||||
main_ret = TRANSITION_CHANGED;
|
main_ret = TRANSITION_CHANGED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -869,6 +880,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case vehicle_command_s::VEHICLE_CMD_SET_NAV_STATE: { // Used from ROS
|
||||||
|
uint8_t desired_nav_state = (uint8_t)(cmd.param1 + 0.5f);
|
||||||
|
|
||||||
|
if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
|
||||||
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
case vehicle_command_s::VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
||||||
|
|
||||||
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
|
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
|
||||||
|
@ -969,7 +992,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
|
case vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH: {
|
||||||
/* switch to RTL which ends the mission */
|
/* switch to RTL which ends the mission */
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, getSourceFromCommand(cmd))) {
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
|
mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
|
||||||
events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
|
events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
|
||||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
@ -983,7 +1006,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
|
case vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF: {
|
||||||
/* ok, home set, use it to take off */
|
/* ok, home set, use it to take off */
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF, getSourceFromCommand(cmd))) {
|
||||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -997,7 +1020,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
|
#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
|
||||||
|
|
||||||
/* ok, home set, use it to take off */
|
/* ok, home set, use it to take off */
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, getSourceFromCommand(cmd))) {
|
||||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1011,7 +1034,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
|
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
|
mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
|
||||||
events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
|
events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
|
||||||
"Landing at current position");
|
"Landing at current position");
|
||||||
|
@ -1025,7 +1048,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
|
case vehicle_command_s::VEHICLE_CMD_NAV_PRECLAND: {
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND, getSourceFromCommand(cmd))) {
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
|
mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
|
||||||
events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
|
events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
|
||||||
"Landing using precision landing");
|
"Landing using precision landing");
|
||||||
|
@ -1049,7 +1072,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
|
if (PX4_ISFINITE(cmd.param1) && (cmd.param1 >= -1) && (cmd.param1 < _mission_result_sub.get().seq_total)) {
|
||||||
|
|
||||||
// switch to AUTO_MISSION and ARM
|
// switch to AUTO_MISSION and ARM
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, getSourceFromCommand(cmd))
|
||||||
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
|
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
|
||||||
|
|
||||||
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
|
||||||
|
@ -1088,7 +1111,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
|
|
||||||
} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||||
// for fixed wings the behavior of orbit is the same as loiter
|
// for fixed wings the behavior of orbit is the same as loiter
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, getSourceFromCommand(cmd))) {
|
||||||
main_ret = TRANSITION_CHANGED;
|
main_ret = TRANSITION_CHANGED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1097,7 +1120,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
// Switch to orbit state and let the orbit task handle the command further
|
// Switch to orbit state and let the orbit task handle the command further
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_ORBIT, getSourceFromCommand(cmd))) {
|
||||||
main_ret = TRANSITION_CHANGED;
|
main_ret = TRANSITION_CHANGED;
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
|
@ -1382,6 +1405,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
case vehicle_command_s::VEHICLE_CMD_DO_SET_STANDARD_MODE: {
|
||||||
|
mode_util::StandardMode standard_mode = (mode_util::StandardMode) roundf(cmd.param1);
|
||||||
|
uint8_t nav_state = mode_util::getNavStateFromStandardMode(standard_mode);
|
||||||
|
|
||||||
|
if (nav_state == vehicle_status_s::NAVIGATION_STATE_MAX) {
|
||||||
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (_user_mode_intention.change(nav_state, getSourceFromCommand(cmd))) {
|
||||||
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
|
case vehicle_command_s::VEHICLE_CMD_RUN_PREARM_CHECKS:
|
||||||
_health_and_arming_checks.update(true);
|
_health_and_arming_checks.update(true);
|
||||||
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
|
||||||
|
@ -1445,6 +1486,43 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
ModeChangeSource Commander::getSourceFromCommand(const vehicle_command_s &cmd)
|
||||||
|
{
|
||||||
|
return cmd.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START ? ModeChangeSource::ModeExecutor :
|
||||||
|
ModeChangeSource::User;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Commander::handleCommandsFromModeExecutors()
|
||||||
|
{
|
||||||
|
if (_vehicle_command_mode_executor_sub.updated()) {
|
||||||
|
const unsigned last_generation = _vehicle_command_mode_executor_sub.get_last_generation();
|
||||||
|
vehicle_command_s cmd;
|
||||||
|
|
||||||
|
if (_vehicle_command_mode_executor_sub.copy(&cmd)) {
|
||||||
|
if (_vehicle_command_mode_executor_sub.get_last_generation() != last_generation + 1) {
|
||||||
|
PX4_ERR("vehicle_command from executor lost, generation %u -> %u", last_generation,
|
||||||
|
_vehicle_command_mode_executor_sub.get_last_generation());
|
||||||
|
}
|
||||||
|
|
||||||
|
// For commands from mode executors, we check if it is in charge and then publish it on the official
|
||||||
|
// command topic
|
||||||
|
const int mode_executor_in_charge = _mode_management.modeExecutorInCharge();
|
||||||
|
|
||||||
|
// source_system is set to the mode executor
|
||||||
|
if (cmd.source_component == vehicle_command_s::COMPONENT_MODE_EXECUTOR_START + mode_executor_in_charge) {
|
||||||
|
cmd.source_system = _vehicle_status.system_id;
|
||||||
|
cmd.timestamp = hrt_absolute_time();
|
||||||
|
_vehicle_command_pub.publish(cmd);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
cmd.source_system = _vehicle_status.system_id;
|
||||||
|
answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
|
||||||
|
PX4_WARN("Got cmd from executor %i not in charge (in charge: %i)", cmd.source_system, mode_executor_in_charge);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
|
unsigned Commander::handleCommandActuatorTest(const vehicle_command_s &cmd)
|
||||||
{
|
{
|
||||||
if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
|
if (isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
|
||||||
|
@ -1569,7 +1647,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
|
||||||
|
|
||||||
case action_request_s::ACTION_SWITCH_MODE:
|
case action_request_s::ACTION_SWITCH_MODE:
|
||||||
|
|
||||||
if (!_user_mode_intention.change(action_request.mode, true)) {
|
if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) {
|
||||||
printRejectMode(action_request.mode);
|
printRejectMode(action_request.mode);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1717,6 +1795,8 @@ void Commander::run()
|
||||||
_status_changed = true;
|
_status_changed = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
modeManagementUpdate();
|
||||||
|
|
||||||
const hrt_abstime now = hrt_absolute_time();
|
const hrt_abstime now = hrt_absolute_time();
|
||||||
|
|
||||||
const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
|
const bool nav_state_or_failsafe_changed = handleModeIntentionAndFailsafe();
|
||||||
|
@ -1739,6 +1819,8 @@ void Commander::run()
|
||||||
}
|
}
|
||||||
|
|
||||||
// handle commands last, as the system needs to be updated to handle them
|
// handle commands last, as the system needs to be updated to handle them
|
||||||
|
handleCommandsFromModeExecutors();
|
||||||
|
|
||||||
if (_vehicle_command_sub.updated()) {
|
if (_vehicle_command_sub.updated()) {
|
||||||
// got command
|
// got command
|
||||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||||
|
@ -1799,6 +1881,7 @@ void Commander::run()
|
||||||
updateControlMode();
|
updateControlMode();
|
||||||
|
|
||||||
// vehicle_status publish (after prearm/preflight updates above)
|
// 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.timestamp = hrt_absolute_time();
|
_vehicle_status.timestamp = hrt_absolute_time();
|
||||||
_vehicle_status_pub.publish(_vehicle_status);
|
_vehicle_status_pub.publish(_vehicle_status);
|
||||||
|
|
||||||
|
@ -1880,7 +1963,8 @@ void Commander::checkForMissionUpdate()
|
||||||
|
|
||||||
if (isArmed() && !_vehicle_land_detected.landed
|
if (isArmed() && !_vehicle_land_detected.landed
|
||||||
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
|
&& (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
|
||||||
&& mission_result.finished) {
|
&& mission_result.finished
|
||||||
|
&& _mode_management.modeExecutorInCharge() == ModeExecutors::AUTOPILOT_EXECUTOR_ID) {
|
||||||
|
|
||||||
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|
||||||
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
|
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
|
||||||
|
@ -2131,8 +2215,10 @@ void Commander::handleAutoDisarm()
|
||||||
|
|
||||||
const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
|
const bool landed_amid_mission = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)
|
||||||
&& !_mission_result_sub.get().finished;
|
&& !_mission_result_sub.get().finished;
|
||||||
|
const bool auto_disarm_land_enabled = _param_com_disarm_land.get() > 0 && !landed_amid_mission
|
||||||
|
&& !_config_overrides.disable_auto_disarm;
|
||||||
|
|
||||||
if (_param_com_disarm_land.get() > 0 && _have_taken_off_since_arming && !landed_amid_mission) {
|
if (auto_disarm_land_enabled && _have_taken_off_since_arming) {
|
||||||
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
|
_auto_disarm_landed.set_hysteresis_time_from(false, _param_com_disarm_land.get() * 1_s);
|
||||||
_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
|
_auto_disarm_landed.set_state_and_update(_vehicle_land_detected.landed, hrt_absolute_time());
|
||||||
|
|
||||||
|
@ -2181,6 +2267,7 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||||
{
|
{
|
||||||
const uint8_t prev_nav_state = _vehicle_status.nav_state;
|
const uint8_t prev_nav_state = _vehicle_status.nav_state;
|
||||||
const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
|
const FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
|
||||||
|
const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state;
|
||||||
|
|
||||||
FailsafeBase::State state{};
|
FailsafeBase::State state{};
|
||||||
state.armed = isArmed();
|
state.armed = isArmed();
|
||||||
|
@ -2200,13 +2287,16 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||||
|
|
||||||
// Force intended mode if changed by the failsafe state machine
|
// Force intended mode if changed by the failsafe state machine
|
||||||
if (state.user_intended_mode != updated_user_intented_mode) {
|
if (state.user_intended_mode != updated_user_intented_mode) {
|
||||||
_user_mode_intention.change(updated_user_intented_mode, false, true);
|
_user_mode_intention.change(updated_user_intented_mode, ModeChangeSource::User, false, true);
|
||||||
_user_mode_intention.getHadModeChangeAndClear();
|
_user_mode_intention.getHadModeChangeAndClear();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Handle failsafe action
|
// Handle failsafe action
|
||||||
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_user_mode_intention.get(),
|
||||||
_vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get());
|
false);
|
||||||
|
_vehicle_status.nav_state = _mode_management.getNavStateReplacementIfValid(FailsafeBase::modeFromAction(
|
||||||
|
_failsafe.selectedAction(), _user_mode_intention.get()));
|
||||||
|
_vehicle_status.executor_in_charge = _mode_management.modeExecutorInCharge(); // Set this in sync with nav_state
|
||||||
|
|
||||||
switch (_failsafe.selectedAction()) {
|
switch (_failsafe.selectedAction()) {
|
||||||
case FailsafeBase::Action::Disarm:
|
case FailsafeBase::Action::Disarm:
|
||||||
|
@ -2228,7 +2318,24 @@ bool Commander::handleModeIntentionAndFailsafe()
|
||||||
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
_vehicle_status.nav_state_timestamp = hrt_absolute_time();
|
||||||
}
|
}
|
||||||
|
|
||||||
return prev_nav_state != _vehicle_status.nav_state || prev_failsafe_action != _failsafe.selectedAction();
|
_mode_management.updateActiveConfigOverrides(_vehicle_status.nav_state, _config_overrides);
|
||||||
|
|
||||||
|
// Apply failsafe deferring & get the current state
|
||||||
|
_failsafe.deferFailsafes(_config_overrides.defer_failsafes, _config_overrides.defer_failsafes_timeout_s);
|
||||||
|
|
||||||
|
if (_failsafe.failsafeDeferred()) {
|
||||||
|
_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_WOULD_FAILSAFE;
|
||||||
|
|
||||||
|
} else if (_failsafe.getDeferFailsafes()) {
|
||||||
|
_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_ENABLED;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_vehicle_status.failsafe_defer_state = vehicle_status_s::FAILSAFE_DEFER_STATE_DISABLED;
|
||||||
|
}
|
||||||
|
|
||||||
|
return prev_nav_state != _vehicle_status.nav_state ||
|
||||||
|
prev_failsafe_action != _failsafe.selectedAction() ||
|
||||||
|
prev_failsafe_defer_state != _vehicle_status.failsafe_defer_state;
|
||||||
}
|
}
|
||||||
|
|
||||||
void Commander::checkAndInformReadyForTakeoff()
|
void Commander::checkAndInformReadyForTakeoff()
|
||||||
|
@ -2248,6 +2355,21 @@ void Commander::checkAndInformReadyForTakeoff()
|
||||||
#endif // CONFIG_ARCH_BOARD_PX4_SITL
|
#endif // CONFIG_ARCH_BOARD_PX4_SITL
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void Commander::modeManagementUpdate()
|
||||||
|
{
|
||||||
|
ModeManagement::UpdateRequest mode_management_update{};
|
||||||
|
_mode_management.update(isArmed(), _vehicle_status.nav_state_user_intention,
|
||||||
|
_failsafe.selectedAction() > FailsafeBase::Action::Warn, mode_management_update);
|
||||||
|
|
||||||
|
if (!isArmed() && mode_management_update.change_user_intended_nav_state) {
|
||||||
|
_user_mode_intention.change(mode_management_update.user_intended_nav_state);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mode_management_update.control_setpoint_update) {
|
||||||
|
_status_changed = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||||
{
|
{
|
||||||
switch (blink_msg_state()) {
|
switch (blink_msg_state()) {
|
||||||
|
@ -2393,8 +2515,19 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||||
void Commander::updateControlMode()
|
void Commander::updateControlMode()
|
||||||
{
|
{
|
||||||
_vehicle_control_mode = {};
|
_vehicle_control_mode = {};
|
||||||
mode_util::getVehicleControlMode(isArmed(), _vehicle_status.nav_state,
|
|
||||||
|
mode_util::getVehicleControlMode(_vehicle_status.nav_state,
|
||||||
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
|
_vehicle_status.vehicle_type, _offboard_control_mode_sub.get(), _vehicle_control_mode);
|
||||||
|
_mode_management.updateControlMode(_vehicle_status.nav_state, _vehicle_control_mode);
|
||||||
|
|
||||||
|
_vehicle_control_mode.flag_armed = isArmed();
|
||||||
|
_vehicle_control_mode.flag_multicopter_position_control_enabled =
|
||||||
|
(_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
|
||||||
|
&& (_vehicle_control_mode.flag_control_altitude_enabled
|
||||||
|
|| _vehicle_control_mode.flag_control_climb_rate_enabled
|
||||||
|
|| _vehicle_control_mode.flag_control_position_enabled
|
||||||
|
|| _vehicle_control_mode.flag_control_velocity_enabled
|
||||||
|
|| _vehicle_control_mode.flag_control_acceleration_enabled);
|
||||||
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
_vehicle_control_mode.timestamp = hrt_absolute_time();
|
||||||
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
|
_vehicle_control_mode_pub.publish(_vehicle_control_mode);
|
||||||
}
|
}
|
||||||
|
@ -2744,7 +2877,7 @@ void Commander::manualControlCheck()
|
||||||
if (override_enabled) {
|
if (override_enabled) {
|
||||||
// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
|
// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
|
||||||
if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
|
if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
|
||||||
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, true)) {
|
if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
|
||||||
tune_positive(true);
|
tune_positive(true);
|
||||||
mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
|
mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
|
||||||
events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
|
events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
|
||||||
|
@ -2761,7 +2894,7 @@ void Commander::manualControlCheck()
|
||||||
|
|
||||||
// if there's never been a mode change force position control as initial state
|
// if there's never been a mode change force position control as initial state
|
||||||
if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) {
|
if (!_user_mode_intention.everHadModeChange() && (is_mavlink || !_mode_switch_mapped)) {
|
||||||
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, false, true);
|
_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, false, true);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -2823,7 +2956,7 @@ The commander module contains the state machine for mode switching and failsafe
|
||||||
PRINT_MODULE_USAGE_COMMAND("land");
|
PRINT_MODULE_USAGE_COMMAND("land");
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
|
PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition");
|
||||||
PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
|
PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode");
|
||||||
PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland",
|
PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland|ext1",
|
||||||
"Flight mode", false);
|
"Flight mode", false);
|
||||||
PRINT_MODULE_USAGE_COMMAND("pair");
|
PRINT_MODULE_USAGE_COMMAND("pair");
|
||||||
PRINT_MODULE_USAGE_COMMAND("lockdown");
|
PRINT_MODULE_USAGE_COMMAND("lockdown");
|
||||||
|
|
|
@ -34,13 +34,13 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
/* Helper classes */
|
/* Helper classes */
|
||||||
#include "failure_detector/FailureDetector.hpp"
|
|
||||||
#include "failsafe/failsafe.h"
|
#include "failsafe/failsafe.h"
|
||||||
#include "Safety.hpp"
|
#include "failure_detector/FailureDetector.hpp"
|
||||||
#include "worker_thread.hpp"
|
|
||||||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||||
#include "HomePosition.hpp"
|
#include "HomePosition.hpp"
|
||||||
|
#include "ModeManagement.hpp"
|
||||||
#include "UserModeIntention.hpp"
|
#include "UserModeIntention.hpp"
|
||||||
|
#include "worker_thread.hpp"
|
||||||
|
|
||||||
#include <lib/controllib/blocks.hpp>
|
#include <lib/controllib/blocks.hpp>
|
||||||
#include <lib/hysteresis/hysteresis.h>
|
#include <lib/hysteresis/hysteresis.h>
|
||||||
|
@ -80,7 +80,6 @@
|
||||||
#include <uORB/topics/vehicle_command.h>
|
#include <uORB/topics/vehicle_command.h>
|
||||||
#include <uORB/topics/vehicle_global_position.h>
|
#include <uORB/topics/vehicle_global_position.h>
|
||||||
#include <uORB/topics/vehicle_land_detected.h>
|
#include <uORB/topics/vehicle_land_detected.h>
|
||||||
#include <uORB/topics/vehicle_local_position.h>
|
|
||||||
#include <uORB/topics/vtol_vehicle_status.h>
|
#include <uORB/topics/vtol_vehicle_status.h>
|
||||||
|
|
||||||
using math::constrain;
|
using math::constrain;
|
||||||
|
@ -124,6 +123,7 @@ public:
|
||||||
|
|
||||||
private:
|
private:
|
||||||
bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); }
|
bool isArmed() const { return (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); }
|
||||||
|
static ModeChangeSource getSourceFromCommand(const vehicle_command_s &cmd);
|
||||||
|
|
||||||
void answer_command(const vehicle_command_s &cmd, uint8_t result);
|
void answer_command(const vehicle_command_s &cmd, uint8_t result);
|
||||||
|
|
||||||
|
@ -174,6 +174,10 @@ private:
|
||||||
|
|
||||||
void safetyButtonUpdate();
|
void safetyButtonUpdate();
|
||||||
|
|
||||||
|
bool isThrowLaunchInProgress() const;
|
||||||
|
|
||||||
|
void throwLaunchUpdate();
|
||||||
|
|
||||||
void vtolStatusUpdate();
|
void vtolStatusUpdate();
|
||||||
|
|
||||||
void updateTunes();
|
void updateTunes();
|
||||||
|
@ -190,6 +194,10 @@ private:
|
||||||
|
|
||||||
void checkAndInformReadyForTakeoff();
|
void checkAndInformReadyForTakeoff();
|
||||||
|
|
||||||
|
void handleCommandsFromModeExecutors();
|
||||||
|
|
||||||
|
void modeManagementUpdate();
|
||||||
|
|
||||||
enum class PrearmedMode {
|
enum class PrearmedMode {
|
||||||
DISABLED = 0,
|
DISABLED = 0,
|
||||||
SAFETY_BUTTON = 1,
|
SAFETY_BUTTON = 1,
|
||||||
|
@ -210,12 +218,19 @@ private:
|
||||||
FailsafeBase &_failsafe{_failsafe_instance};
|
FailsafeBase &_failsafe{_failsafe_instance};
|
||||||
FailureDetector _failure_detector{this};
|
FailureDetector _failure_detector{this};
|
||||||
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
|
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
|
||||||
|
MulticopterThrowLaunch _multicopter_throw_launch{this};
|
||||||
Safety _safety{};
|
Safety _safety{};
|
||||||
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
|
|
||||||
WorkerThread _worker_thread{};
|
WorkerThread _worker_thread{};
|
||||||
|
ModeManagement _mode_management{
|
||||||
|
#ifndef CONSTRAINED_FLASH
|
||||||
|
_health_and_arming_checks.externalChecks()
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
UserModeIntention _user_mode_intention {this, _vehicle_status, _health_and_arming_checks, &_mode_management};
|
||||||
|
|
||||||
const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
|
const failsafe_flags_s &_failsafe_flags{_health_and_arming_checks.failsafeFlags()};
|
||||||
HomePosition _home_position{_failsafe_flags};
|
HomePosition _home_position{_failsafe_flags};
|
||||||
|
config_overrides_s _config_overrides{};
|
||||||
|
|
||||||
|
|
||||||
Hysteresis _auto_disarm_landed{false};
|
Hysteresis _auto_disarm_landed{false};
|
||||||
|
@ -276,6 +291,7 @@ private:
|
||||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||||
|
uORB::Subscription _vehicle_command_mode_executor_sub{ORB_ID(vehicle_command_mode_executor)};
|
||||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||||
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||||
|
|
||||||
|
@ -295,6 +311,7 @@ private:
|
||||||
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
|
uORB::Publication<actuator_test_s> _actuator_test_pub{ORB_ID(actuator_test)};
|
||||||
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
|
uORB::Publication<failure_detector_status_s> _failure_detector_status_pub{ORB_ID(failure_detector_status)};
|
||||||
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
|
||||||
|
uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
|
||||||
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
|
uORB::Publication<vehicle_control_mode_s> _vehicle_control_mode_pub{ORB_ID(vehicle_control_mode)};
|
||||||
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
|
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
|
||||||
|
|
||||||
|
|
|
@ -65,6 +65,7 @@ px4_add_library(health_and_arming_checks
|
||||||
checks/vtolCheck.cpp
|
checks/vtolCheck.cpp
|
||||||
checks/windCheck.cpp
|
checks/windCheck.cpp
|
||||||
|
|
||||||
|
checks/externalChecks.cpp
|
||||||
)
|
)
|
||||||
add_dependencies(health_and_arming_checks mode_util)
|
add_dependencies(health_and_arming_checks mode_util)
|
||||||
|
|
||||||
|
|
|
@ -87,6 +87,30 @@ Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const eve
|
||||||
return header;
|
return header;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
bool Report::addExternalEvent(const event_s &event, NavModes modes)
|
||||||
|
{
|
||||||
|
unsigned args_size = sizeof(event.arguments);
|
||||||
|
|
||||||
|
// trim 0's
|
||||||
|
while (args_size > 0 && event.arguments[args_size - 1] == '\0') {
|
||||||
|
--args_size;
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned total_size = sizeof(EventBufferHeader) + args_size;
|
||||||
|
|
||||||
|
if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
|
||||||
|
_buffer_overflowed = true;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
events::LogLevels log_levels{events::externalLogLevel(event.log_levels), events::internalLogLevel((event.log_levels))};
|
||||||
|
memcpy(_event_buffer + _next_buffer_idx + sizeof(EventBufferHeader), &event.arguments, args_size);
|
||||||
|
addEventToBuffer(event.id, log_levels, (uint32_t)modes, args_size);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
NavModes Report::reportedModes(NavModes required_modes)
|
NavModes Report::reportedModes(NavModes required_modes)
|
||||||
{
|
{
|
||||||
// Make sure optional checks are still shown in the UI
|
// Make sure optional checks are still shown in the UI
|
||||||
|
|
|
@ -249,8 +249,8 @@ public:
|
||||||
void clearArmingBits(NavModes modes);
|
void clearArmingBits(NavModes modes);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
|
* Clear can_run bits for certain modes. This will prevent mode switching.
|
||||||
* mode is being run.
|
* For failsafe use the mode requirements instead, which then will clear the can_run bits.
|
||||||
* @param modes affected modes
|
* @param modes affected modes
|
||||||
*/
|
*/
|
||||||
void clearCanRunBits(NavModes modes);
|
void clearCanRunBits(NavModes modes);
|
||||||
|
@ -259,6 +259,8 @@ public:
|
||||||
const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
|
const ArmingCheckResults &armingCheckResults() const { return _results[_current_result].arming_checks; }
|
||||||
|
|
||||||
bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
|
bool modePreventsArming(uint8_t nav_state) const { return _failsafe_flags.mode_req_prevent_arming & (1u << nav_state); }
|
||||||
|
|
||||||
|
bool addExternalEvent(const event_s &event, NavModes modes);
|
||||||
private:
|
private:
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -307,6 +309,7 @@ private:
|
||||||
NavModes getModeGroup(uint8_t nav_state) const;
|
NavModes getModeGroup(uint8_t nav_state) const;
|
||||||
|
|
||||||
friend class HealthAndArmingChecks;
|
friend class HealthAndArmingChecks;
|
||||||
|
friend class ExternalChecks;
|
||||||
FRIEND_TEST(ReporterTest, basic_no_checks);
|
FRIEND_TEST(ReporterTest, basic_no_checks);
|
||||||
FRIEND_TEST(ReporterTest, basic_fail_all_modes);
|
FRIEND_TEST(ReporterTest, basic_fail_all_modes);
|
||||||
FRIEND_TEST(ReporterTest, arming_checks_mode_category);
|
FRIEND_TEST(ReporterTest, arming_checks_mode_category);
|
||||||
|
|
|
@ -69,6 +69,7 @@
|
||||||
#include "checks/vtolCheck.hpp"
|
#include "checks/vtolCheck.hpp"
|
||||||
#include "checks/offboardCheck.hpp"
|
#include "checks/offboardCheck.hpp"
|
||||||
#include "checks/openDroneIDCheck.hpp"
|
#include "checks/openDroneIDCheck.hpp"
|
||||||
|
#include "checks/externalChecks.hpp"
|
||||||
|
|
||||||
class HealthAndArmingChecks : public ModuleParams
|
class HealthAndArmingChecks : public ModuleParams
|
||||||
{
|
{
|
||||||
|
@ -101,6 +102,10 @@ public:
|
||||||
|
|
||||||
const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
|
const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
|
||||||
|
|
||||||
|
#ifndef CONSTRAINED_FLASH
|
||||||
|
ExternalChecks &externalChecks() { return _external_checks; }
|
||||||
|
#endif
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
void updateParams() override;
|
void updateParams() override;
|
||||||
private:
|
private:
|
||||||
|
@ -143,8 +148,14 @@ private:
|
||||||
RcAndDataLinkChecks _rc_and_data_link_checks;
|
RcAndDataLinkChecks _rc_and_data_link_checks;
|
||||||
VtolChecks _vtol_checks;
|
VtolChecks _vtol_checks;
|
||||||
OffboardChecks _offboard_checks;
|
OffboardChecks _offboard_checks;
|
||||||
|
#ifndef CONSTRAINED_FLASH
|
||||||
|
ExternalChecks _external_checks;
|
||||||
|
#endif
|
||||||
|
|
||||||
HealthAndArmingCheckBase *_checks[31] = {
|
HealthAndArmingCheckBase *_checks[40] = {
|
||||||
|
#ifndef CONSTRAINED_FLASH
|
||||||
|
&_external_checks,
|
||||||
|
#endif
|
||||||
&_accelerometer_checks,
|
&_accelerometer_checks,
|
||||||
&_airspeed_checks,
|
&_airspeed_checks,
|
||||||
&_arm_permission_checks,
|
&_arm_permission_checks,
|
||||||
|
@ -161,7 +172,7 @@ private:
|
||||||
&_home_position_checks,
|
&_home_position_checks,
|
||||||
&_mission_checks,
|
&_mission_checks,
|
||||||
&_offboard_checks, // must be after _estimator_checks
|
&_offboard_checks, // must be after _estimator_checks
|
||||||
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
|
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks, _external_checks
|
||||||
&_open_drone_id_checks,
|
&_open_drone_id_checks,
|
||||||
&_parachute_checks,
|
&_parachute_checks,
|
||||||
&_power_checks,
|
&_power_checks,
|
||||||
|
|
|
@ -0,0 +1,335 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include "externalChecks.hpp"
|
||||||
|
|
||||||
|
static void setOrClearRequirementBits(bool requirement_set, int8_t nav_state, int8_t replaces_nav_state, uint32_t &bits)
|
||||||
|
{
|
||||||
|
if (requirement_set) {
|
||||||
|
bits |= 1u << nav_state;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (replaces_nav_state != -1) {
|
||||||
|
if (requirement_set) {
|
||||||
|
bits |= 1u << replaces_nav_state;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
bits &= ~(1u << replaces_nav_state);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int ExternalChecks::addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state)
|
||||||
|
{
|
||||||
|
int free_registration_index = -1;
|
||||||
|
|
||||||
|
for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
|
||||||
|
if (!registrationValid(i)) {
|
||||||
|
free_registration_index = i;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (free_registration_index != -1) {
|
||||||
|
_active_registrations_mask |= 1 << free_registration_index;
|
||||||
|
_registrations[free_registration_index].nav_mode_id = nav_mode_id;
|
||||||
|
_registrations[free_registration_index].replaces_nav_state = replaces_nav_state;
|
||||||
|
_registrations[free_registration_index].num_no_response = 0;
|
||||||
|
_registrations[free_registration_index].unresponsive = false;
|
||||||
|
_registrations[free_registration_index].total_num_unresponsive = 0;
|
||||||
|
|
||||||
|
if (!_registrations[free_registration_index].reply) {
|
||||||
|
_registrations[free_registration_index].reply = new arming_check_reply_s();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return free_registration_index;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ExternalChecks::removeRegistration(int registration_id, int8_t nav_mode_id)
|
||||||
|
{
|
||||||
|
if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (registrationValid(registration_id)) {
|
||||||
|
if (_registrations[registration_id].nav_mode_id == nav_mode_id) {
|
||||||
|
_active_registrations_mask &= ~(1u << registration_id);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_ERR("trying to remove inactive external check");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ExternalChecks::isUnresponsive(int registration_id)
|
||||||
|
{
|
||||||
|
if (registration_id < 0 || registration_id >= MAX_NUM_REGISTRATIONS) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (registrationValid(registration_id)) {
|
||||||
|
return _registrations[registration_id].unresponsive;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
|
||||||
|
{
|
||||||
|
checkNonRegisteredModes(context, reporter);
|
||||||
|
|
||||||
|
if (_active_registrations_mask == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
NavModes unresponsive_modes{NavModes::None};
|
||||||
|
|
||||||
|
for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
|
||||||
|
if (!registrationValid(reg_idx) || !_registrations[reg_idx].reply) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
arming_check_reply_s &reply = *_registrations[reg_idx].reply;
|
||||||
|
|
||||||
|
int8_t nav_mode_id = _registrations[reply.registration_id].nav_mode_id;
|
||||||
|
|
||||||
|
if (_registrations[reply.registration_id].unresponsive) {
|
||||||
|
|
||||||
|
if (nav_mode_id != -1) {
|
||||||
|
unresponsive_modes = unresponsive_modes | reporter.getModeGroup(nav_mode_id);
|
||||||
|
setOrClearRequirementBits(true, nav_mode_id, -1, reporter.failsafeFlags().mode_req_other);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
NavModes modes;
|
||||||
|
|
||||||
|
// We distinguish between two cases:
|
||||||
|
// - external navigation mode: in that case we set the single arming can_run bit for the mode
|
||||||
|
// - generic external arming check: set all arming bits
|
||||||
|
if (nav_mode_id == -1) {
|
||||||
|
modes = NavModes::All;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
modes = reporter.getModeGroup(nav_mode_id);
|
||||||
|
|
||||||
|
int8_t replaces_nav_state = _registrations[reply.registration_id].replaces_nav_state;
|
||||||
|
|
||||||
|
if (replaces_nav_state != -1) {
|
||||||
|
modes = modes | reporter.getModeGroup(replaces_nav_state);
|
||||||
|
// Also clear the arming bits for the replaced mode, as the user intention is always set to the
|
||||||
|
// replaced mode.
|
||||||
|
// We only have to clear the bits, as for the internal/replaced mode, the bits are not cleared yet.
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!reply.can_arm_and_run) {
|
||||||
|
setOrClearRequirementBits(true, nav_mode_id, replaces_nav_state, reporter.failsafeFlags().mode_req_other);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Mode requirements
|
||||||
|
// A replacement mode will also replace the mode requirements of the internal/replaced mode
|
||||||
|
setOrClearRequirementBits(reply.mode_req_angular_velocity, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_angular_velocity);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_attitude, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_attitude);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_local_alt, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_local_alt);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_local_position, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_local_position);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_local_position_relaxed, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_local_position_relaxed);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_global_position);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_mission);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_home_position);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_prevent_arming, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_prevent_arming);
|
||||||
|
setOrClearRequirementBits(reply.mode_req_manual_control, nav_mode_id, replaces_nav_state,
|
||||||
|
reporter.failsafeFlags().mode_req_manual_control);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!reply.can_arm_and_run) {
|
||||||
|
reporter.clearArmingBits(modes);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (reply.health_component_index > 0) {
|
||||||
|
reporter.setHealth((health_component_t)(1ull << reply.health_component_index),
|
||||||
|
reply.health_component_is_present, reply.health_component_warning,
|
||||||
|
reply.health_component_error);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i = 0; i < reply.num_events; ++i) {
|
||||||
|
// set the modes, which is the first argument
|
||||||
|
memcpy(reply.events[i].arguments, &modes, sizeof(modes));
|
||||||
|
|
||||||
|
reporter.addExternalEvent(reply.events[i], modes);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (unresponsive_modes != NavModes::None) {
|
||||||
|
/* EVENT
|
||||||
|
* @description
|
||||||
|
* The application running the mode might have crashed or the CPU load is too high.
|
||||||
|
*/
|
||||||
|
reporter.armingCheckFailure(unresponsive_modes, health_component_t::system,
|
||||||
|
events::ID("check_external_modes_unresponsive"),
|
||||||
|
events::Log::Critical, "Mode is unresponsive");
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void ExternalChecks::update()
|
||||||
|
{
|
||||||
|
if (_active_registrations_mask == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const hrt_abstime now = hrt_absolute_time();
|
||||||
|
|
||||||
|
// Check for incoming replies
|
||||||
|
arming_check_reply_s reply;
|
||||||
|
int max_num_updates = arming_check_reply_s::ORB_QUEUE_LENGTH;
|
||||||
|
|
||||||
|
while (_arming_check_reply_sub.update(&reply) && --max_num_updates >= 0) {
|
||||||
|
if (reply.registration_id < MAX_NUM_REGISTRATIONS && registrationValid(reply.registration_id)
|
||||||
|
&& _current_request_id == reply.request_id) {
|
||||||
|
_reply_received_mask |= 1u << reply.registration_id;
|
||||||
|
_registrations[reply.registration_id].num_no_response = 0;
|
||||||
|
|
||||||
|
// Prevent toggling between unresponsive & responsive state
|
||||||
|
if (_registrations[reply.registration_id].total_num_unresponsive <= 3) {
|
||||||
|
_registrations[reply.registration_id].unresponsive = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_registrations[reply.registration_id].reply) {
|
||||||
|
*_registrations[reply.registration_id].reply = reply;
|
||||||
|
}
|
||||||
|
|
||||||
|
// PX4_DEBUG("Registration id=%i: %i events", reply.registration_id, reply.num_events);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_last_update > 0) {
|
||||||
|
if (_reply_received_mask == _active_registrations_mask) { // Got all responses
|
||||||
|
// Nothing to do
|
||||||
|
} else if (now > _last_update + REQUEST_TIMEOUT && !_had_timeout) { // Timeout
|
||||||
|
_had_timeout = true;
|
||||||
|
unsigned no_reply = _active_registrations_mask & ~_reply_received_mask;
|
||||||
|
|
||||||
|
for (int i = 0; i < MAX_NUM_REGISTRATIONS; ++i) {
|
||||||
|
if ((1u << i) & no_reply) {
|
||||||
|
if (!_registrations[i].unresponsive && ++_registrations[i].num_no_response >= NUM_NO_REPLY_UNTIL_UNRESPONSIVE) {
|
||||||
|
// Clear immediately if not a mode
|
||||||
|
if (_registrations[i].nav_mode_id == -1) {
|
||||||
|
removeRegistration(i, -1);
|
||||||
|
PX4_WARN("No response from %i, removing", i);
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_registrations[i].unresponsive = true;
|
||||||
|
|
||||||
|
if (_registrations[i].total_num_unresponsive < 100) {
|
||||||
|
++_registrations[i].total_num_unresponsive;
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_WARN("No response from %i, flagging unresponsive", i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Start a new request?
|
||||||
|
if (now > _last_update + UPDATE_INTERVAL) {
|
||||||
|
_reply_received_mask = 0;
|
||||||
|
_last_update = now;
|
||||||
|
_had_timeout = false;
|
||||||
|
|
||||||
|
// Request the state from all registered components
|
||||||
|
arming_check_request_s request{};
|
||||||
|
request.request_id = ++_current_request_id;
|
||||||
|
request.timestamp = hrt_absolute_time();
|
||||||
|
_arming_check_request_pub.publish(request);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ExternalChecks::setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state)
|
||||||
|
{
|
||||||
|
_first_external_nav_state = first_external_nav_state;
|
||||||
|
_last_external_nav_state = last_external_nav_state;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ExternalChecks::checkNonRegisteredModes(const Context &context, Report &reporter) const
|
||||||
|
{
|
||||||
|
// Clear the arming bits for all non-registered external modes.
|
||||||
|
// But only report if one of them is selected, so we don't need to generate the extra event in most cases.
|
||||||
|
bool report_mode_not_available = false;
|
||||||
|
|
||||||
|
for (uint8_t external_nav_state = _first_external_nav_state; external_nav_state <= _last_external_nav_state;
|
||||||
|
++external_nav_state) {
|
||||||
|
bool found = false;
|
||||||
|
|
||||||
|
for (int reg_idx = 0; reg_idx < MAX_NUM_REGISTRATIONS; ++reg_idx) {
|
||||||
|
if (registrationValid(reg_idx) && _registrations[reg_idx].nav_mode_id == external_nav_state) {
|
||||||
|
found = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!found) {
|
||||||
|
if (external_nav_state == context.status().nav_state_user_intention) {
|
||||||
|
report_mode_not_available = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
reporter.clearArmingBits(reporter.getModeGroup(external_nav_state));
|
||||||
|
setOrClearRequirementBits(true, external_nav_state, -1, reporter.failsafeFlags().mode_req_other);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (report_mode_not_available) {
|
||||||
|
/* EVENT
|
||||||
|
* @description
|
||||||
|
* The application running the mode is not started.
|
||||||
|
*/
|
||||||
|
reporter.armingCheckFailure(reporter.getModeGroup(context.status().nav_state_user_intention),
|
||||||
|
health_component_t::system,
|
||||||
|
events::ID("check_external_modes_unavailable"),
|
||||||
|
events::Log::Error, "Mode is not registered");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -0,0 +1,108 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "../Common.hpp"
|
||||||
|
#include <uORB/topics/arming_check_request.h>
|
||||||
|
#include <uORB/topics/arming_check_reply.h>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/Publication.hpp>
|
||||||
|
|
||||||
|
static_assert((1ull << arming_check_reply_s::HEALTH_COMPONENT_INDEX_AVOIDANCE) == (uint64_t)
|
||||||
|
health_component_t::avoidance, "enum definition missmatch");
|
||||||
|
|
||||||
|
class ExternalChecks : public HealthAndArmingCheckBase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static constexpr int MAX_NUM_REGISTRATIONS = 8;
|
||||||
|
|
||||||
|
ExternalChecks() = default;
|
||||||
|
~ExternalChecks() = default;
|
||||||
|
|
||||||
|
void setExternalNavStates(uint8_t first_external_nav_state, uint8_t last_external_nav_state);
|
||||||
|
|
||||||
|
void checkAndReport(const Context &context, Report &reporter) override;
|
||||||
|
|
||||||
|
bool hasFreeRegistrations() const { return _active_registrations_mask != (1u << MAX_NUM_REGISTRATIONS) - 1; }
|
||||||
|
/**
|
||||||
|
* Add registration
|
||||||
|
* @param nav_mode_id associated mode, -1 if none
|
||||||
|
* @param replaces_nav_state replaced mode, -1 if none
|
||||||
|
* @return registration id, or -1
|
||||||
|
*/
|
||||||
|
int addRegistration(int8_t nav_mode_id, int8_t replaces_nav_state);
|
||||||
|
bool removeRegistration(int registration_id, int8_t nav_mode_id);
|
||||||
|
void update();
|
||||||
|
|
||||||
|
bool isUnresponsive(int registration_id);
|
||||||
|
|
||||||
|
private:
|
||||||
|
static constexpr hrt_abstime REQUEST_TIMEOUT = 50_ms;
|
||||||
|
static constexpr hrt_abstime UPDATE_INTERVAL = 300_ms;
|
||||||
|
static_assert(REQUEST_TIMEOUT < UPDATE_INTERVAL, "keep timeout < update interval");
|
||||||
|
static constexpr int NUM_NO_REPLY_UNTIL_UNRESPONSIVE = 3; ///< Mode timeout = this value * UPDATE_INTERVAL
|
||||||
|
|
||||||
|
void checkNonRegisteredModes(const Context &context, Report &reporter) const;
|
||||||
|
|
||||||
|
bool registrationValid(int reg_idx) const { return ((1u << reg_idx) & _active_registrations_mask) != 0; }
|
||||||
|
|
||||||
|
struct Registration {
|
||||||
|
~Registration() { delete reply; }
|
||||||
|
|
||||||
|
int8_t nav_mode_id{-1}; ///< associated mode, -1 if none
|
||||||
|
int8_t replaces_nav_state{-1};
|
||||||
|
|
||||||
|
uint8_t num_no_response{0};
|
||||||
|
bool unresponsive{false};
|
||||||
|
uint8_t total_num_unresponsive{0};
|
||||||
|
arming_check_reply_s *reply{nullptr};
|
||||||
|
};
|
||||||
|
|
||||||
|
unsigned _active_registrations_mask{0};
|
||||||
|
Registration _registrations[MAX_NUM_REGISTRATIONS] {};
|
||||||
|
|
||||||
|
uint8_t _first_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
|
||||||
|
uint8_t _last_external_nav_state = vehicle_status_s::NAVIGATION_STATE_MAX;
|
||||||
|
|
||||||
|
// Current requests (async updates)
|
||||||
|
hrt_abstime _last_update{0};
|
||||||
|
unsigned _reply_received_mask{0};
|
||||||
|
bool _had_timeout{false};
|
||||||
|
|
||||||
|
uint8_t _current_request_id{0};
|
||||||
|
|
||||||
|
uORB::Subscription _arming_check_reply_sub{ORB_ID(arming_check_reply)};
|
||||||
|
|
||||||
|
uORB::Publication<arming_check_request_s> _arming_check_request_pub{ORB_ID(arming_check_request)};
|
||||||
|
};
|
|
@ -0,0 +1,639 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef CONSTRAINED_FLASH
|
||||||
|
|
||||||
|
#include "ModeManagement.hpp"
|
||||||
|
|
||||||
|
#include <px4_platform_common/events.h>
|
||||||
|
|
||||||
|
bool ModeExecutors::hasFreeExecutors() const
|
||||||
|
{
|
||||||
|
for (int i = 0; i < MAX_NUM; ++i) {
|
||||||
|
if (!_mode_executors[i].valid) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
int ModeExecutors::addExecutor(const ModeExecutors::ModeExecutor &executor)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < MAX_NUM; ++i) {
|
||||||
|
if (!_mode_executors[i].valid) {
|
||||||
|
_mode_executors[i] = executor;
|
||||||
|
_mode_executors[i].valid = true;
|
||||||
|
return i + FIRST_EXECUTOR_ID;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_ERR("logic error");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeExecutors::removeExecutor(int id)
|
||||||
|
{
|
||||||
|
if (valid(id)) {
|
||||||
|
_mode_executors[id - FIRST_EXECUTOR_ID].valid = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeExecutors::printStatus(int executor_in_charge) const
|
||||||
|
{
|
||||||
|
for (int i = 0; i < MAX_NUM; ++i) {
|
||||||
|
if (_mode_executors[i].valid) {
|
||||||
|
int executor_id = i + FIRST_EXECUTOR_ID;
|
||||||
|
PX4_INFO("Mode Executor %i: owned nav_state: %i, in charge: %s", executor_id, _mode_executors[i].owned_nav_state,
|
||||||
|
executor_id == executor_in_charge ? "yes" : "no");
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Modes::hasFreeExternalModes() const
|
||||||
|
{
|
||||||
|
for (int i = 0; i < MAX_NUM; ++i) {
|
||||||
|
if (!_modes[i].valid) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t Modes::addExternalMode(const Modes::Mode &mode)
|
||||||
|
{
|
||||||
|
int32_t mode_name_hash = (int32_t)events::util::hash_32_fnv1a_const(mode.name);
|
||||||
|
|
||||||
|
if (mode_name_hash == 0) { // 0 is reserved for unused indexes
|
||||||
|
mode_name_hash = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Try to find the index with matching hash (if mode was already registered before),
|
||||||
|
// so that the same mode always gets the same index (required for RC switch mode assignment)
|
||||||
|
int first_unused_idx = -1;
|
||||||
|
int first_invalid_idx = -1;
|
||||||
|
int matching_idx = -1;
|
||||||
|
|
||||||
|
for (int i = 0; i < MAX_NUM; ++i) {
|
||||||
|
char hash_param_name[20];
|
||||||
|
snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", i);
|
||||||
|
const param_t handle = param_find(hash_param_name);
|
||||||
|
int32_t current_hash{};
|
||||||
|
|
||||||
|
if (handle != PARAM_INVALID && param_get(handle, ¤t_hash) == 0) {
|
||||||
|
if (!_modes[i].valid && current_hash == 0 && first_unused_idx == -1) {
|
||||||
|
first_unused_idx = i;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (current_hash == mode_name_hash) {
|
||||||
|
matching_idx = i;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!_modes[i].valid && first_invalid_idx == -1) {
|
||||||
|
first_invalid_idx = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool need_to_update_param = false;
|
||||||
|
int new_mode_idx = -1;
|
||||||
|
|
||||||
|
if (matching_idx != -1) {
|
||||||
|
// If we found a match, try to use it but check for hash collisions or duplicate mode name
|
||||||
|
if (_modes[matching_idx].valid) {
|
||||||
|
// This can happen when restarting modes while armed
|
||||||
|
PX4_WARN("Mode '%s' already registered (as '%s')", mode.name, _modes[matching_idx].name);
|
||||||
|
|
||||||
|
if (first_unused_idx != -1) {
|
||||||
|
new_mode_idx = first_unused_idx;
|
||||||
|
// Do not update the hash
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// Need to overwrite a hash. Reset it as we can't store duplicate hashes anyway
|
||||||
|
new_mode_idx = first_invalid_idx;
|
||||||
|
need_to_update_param = true;
|
||||||
|
mode_name_hash = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
new_mode_idx = matching_idx;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else if (first_unused_idx != -1) {
|
||||||
|
// Mode registers the first time and there's still unused indexes
|
||||||
|
need_to_update_param = true;
|
||||||
|
new_mode_idx = first_unused_idx;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// Mode registers the first time but all indexes are used so we need to overwrite one
|
||||||
|
need_to_update_param = true;
|
||||||
|
new_mode_idx = first_invalid_idx;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (new_mode_idx != -1 && !_modes[new_mode_idx].valid) {
|
||||||
|
if (need_to_update_param) {
|
||||||
|
char hash_param_name[20];
|
||||||
|
snprintf(hash_param_name, sizeof(hash_param_name), "COM_MODE%d_HASH", new_mode_idx);
|
||||||
|
const param_t handle = param_find(hash_param_name);
|
||||||
|
|
||||||
|
if (handle != PARAM_INVALID) {
|
||||||
|
param_set_no_notification(handle, &mode_name_hash);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_modes[new_mode_idx] = mode;
|
||||||
|
_modes[new_mode_idx].valid = true;
|
||||||
|
return new_mode_idx + FIRST_EXTERNAL_NAV_STATE;
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_ERR("logic error");
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Modes::removeExternalMode(uint8_t nav_state, const char *name)
|
||||||
|
{
|
||||||
|
if (valid(nav_state) && strncmp(name, _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].name, sizeof(Mode::name)) == 0) {
|
||||||
|
_modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid = false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
PX4_ERR("trying to remove invalid mode %s", name);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Modes::printStatus() const
|
||||||
|
{
|
||||||
|
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||||
|
if (valid(i)) {
|
||||||
|
const Modes::Mode &cur_mode = mode(i);
|
||||||
|
PX4_INFO("External Mode %i: nav_state: %i, name: %s", i - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1, i,
|
||||||
|
cur_mode.name);
|
||||||
|
|
||||||
|
if (cur_mode.replaces_nav_state != Mode::REPLACES_NAV_STATE_NONE
|
||||||
|
&& cur_mode.replaces_nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
|
||||||
|
PX4_INFO(" Replaces mode: %s", mode_util::nav_state_names[cur_mode.replaces_nav_state]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ModeManagement::ModeManagement(ExternalChecks &external_checks)
|
||||||
|
: _external_checks(external_checks)
|
||||||
|
{
|
||||||
|
_external_checks.setExternalNavStates(Modes::FIRST_EXTERNAL_NAV_STATE, Modes::LAST_EXTERNAL_NAV_STATE);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::checkNewRegistrations(UpdateRequest &update_request)
|
||||||
|
{
|
||||||
|
register_ext_component_request_s request;
|
||||||
|
int max_updates = 5;
|
||||||
|
|
||||||
|
while (!update_request.change_user_intended_nav_state && _register_ext_component_request_sub.update(&request)
|
||||||
|
&& --max_updates >= 0) {
|
||||||
|
request.name[sizeof(request.name) - 1] = '\0';
|
||||||
|
PX4_DEBUG("got registration request: %s %llu, arming: %i mode: %i executor: %i", request.name, request.request_id,
|
||||||
|
request.register_arming_check, request.register_mode, request.register_mode_executor);
|
||||||
|
register_ext_component_reply_s reply{};
|
||||||
|
reply.mode_executor_id = -1;
|
||||||
|
reply.mode_id = -1;
|
||||||
|
reply.arming_check_id = -1;
|
||||||
|
static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
|
||||||
|
memcpy(reply.name, request.name, sizeof(request.name));
|
||||||
|
reply.request_id = request.request_id;
|
||||||
|
reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
|
||||||
|
|
||||||
|
// validate
|
||||||
|
bool request_valid = true;
|
||||||
|
|
||||||
|
if (request.register_mode_executor && !request.register_mode) {
|
||||||
|
request_valid = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request.register_mode && !request.register_arming_check) {
|
||||||
|
request_valid = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
reply.success = false;
|
||||||
|
|
||||||
|
if (request_valid) {
|
||||||
|
// check free space
|
||||||
|
reply.success = true;
|
||||||
|
|
||||||
|
if (request.register_arming_check && !_external_checks.hasFreeRegistrations()) {
|
||||||
|
PX4_WARN("No free slots for arming checks");
|
||||||
|
reply.success = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request.register_mode) {
|
||||||
|
if (!_modes.hasFreeExternalModes()) {
|
||||||
|
PX4_WARN("No free slots for modes");
|
||||||
|
reply.success = false;
|
||||||
|
|
||||||
|
} else if (request.enable_replace_internal_mode) {
|
||||||
|
// Check if another one already replaces the same mode
|
||||||
|
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||||
|
if (_modes.valid(i)) {
|
||||||
|
const Modes::Mode &cur_mode = _modes.mode(i);
|
||||||
|
|
||||||
|
if (cur_mode.replaces_nav_state == request.replace_internal_mode) {
|
||||||
|
// TODO: we could add priorities and allow the highest priority to do the replacement
|
||||||
|
PX4_ERR("Trying to replace an already replaced mode (%i)", request.replace_internal_mode);
|
||||||
|
reply.success = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request.register_mode_executor && !_mode_executors.hasFreeExecutors()) {
|
||||||
|
PX4_WARN("No free slots for executors");
|
||||||
|
reply.success = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// register component(s)
|
||||||
|
if (reply.success) {
|
||||||
|
int nav_mode_id = -1;
|
||||||
|
|
||||||
|
if (request.register_mode) {
|
||||||
|
Modes::Mode mode{};
|
||||||
|
strncpy(mode.name, request.name, sizeof(mode.name));
|
||||||
|
|
||||||
|
if (request.enable_replace_internal_mode) {
|
||||||
|
mode.replaces_nav_state = request.replace_internal_mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
nav_mode_id = _modes.addExternalMode(mode);
|
||||||
|
reply.mode_id = nav_mode_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request.register_mode_executor) {
|
||||||
|
ModeExecutors::ModeExecutor executor{};
|
||||||
|
executor.owned_nav_state = nav_mode_id;
|
||||||
|
int registration_id = _mode_executors.addExecutor(executor);
|
||||||
|
|
||||||
|
if (nav_mode_id != -1) {
|
||||||
|
_modes.mode(nav_mode_id).mode_executor_registration_id = registration_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
reply.mode_executor_id = registration_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request.register_arming_check) {
|
||||||
|
int8_t replace_nav_state = request.enable_replace_internal_mode ? request.replace_internal_mode : -1;
|
||||||
|
int registration_id = _external_checks.addRegistration(nav_mode_id, replace_nav_state);
|
||||||
|
|
||||||
|
if (nav_mode_id != -1) {
|
||||||
|
_modes.mode(nav_mode_id).arming_check_registration_id = registration_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
reply.arming_check_id = registration_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Activate the mode?
|
||||||
|
if (request.register_mode_executor && request.activate_mode_immediately && nav_mode_id != -1) {
|
||||||
|
update_request.change_user_intended_nav_state = true;
|
||||||
|
update_request.user_intended_nav_state = nav_mode_id;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
reply.timestamp = hrt_absolute_time();
|
||||||
|
_register_ext_component_reply_pub.publish(reply);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request)
|
||||||
|
{
|
||||||
|
unregister_ext_component_s request;
|
||||||
|
int max_updates = 5;
|
||||||
|
|
||||||
|
while (!update_request.change_user_intended_nav_state && _unregister_ext_component_sub.update(&request)
|
||||||
|
&& --max_updates >= 0) {
|
||||||
|
request.name[sizeof(request.name) - 1] = '\0';
|
||||||
|
PX4_DEBUG("got unregistration request: %s arming: %i mode: %i executor: %i", request.name,
|
||||||
|
(int)request.arming_check_id, (int)request.mode_id, (int)request.mode_executor_id);
|
||||||
|
|
||||||
|
if (request.arming_check_id != -1) {
|
||||||
|
_external_checks.removeRegistration(request.arming_check_id, request.mode_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (request.mode_id != -1) {
|
||||||
|
if (_modes.removeExternalMode(request.mode_id, request.name)) {
|
||||||
|
removeModeExecutor(request.mode_executor_id);
|
||||||
|
// else: if the mode was already removed (due to a timeout), the executor was also removed already
|
||||||
|
}
|
||||||
|
|
||||||
|
// If the removed mode is currently active, switch to Hold
|
||||||
|
if (user_intended_nav_state == request.mode_id) {
|
||||||
|
update_request.change_user_intended_nav_state = true;
|
||||||
|
update_request.user_intended_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active,
|
||||||
|
UpdateRequest &update_request)
|
||||||
|
{
|
||||||
|
_failsafe_action_active = failsafe_action_active;
|
||||||
|
_external_checks.update();
|
||||||
|
|
||||||
|
bool allow_update_while_armed = false;
|
||||||
|
#if defined(CONFIG_ARCH_BOARD_PX4_SITL)
|
||||||
|
// For simulation, allow registering modes while armed for developer convenience
|
||||||
|
allow_update_while_armed = true;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
if (armed && !allow_update_while_armed) {
|
||||||
|
// Reject registration requests
|
||||||
|
register_ext_component_request_s request;
|
||||||
|
|
||||||
|
if (_register_ext_component_request_sub.update(&request)) {
|
||||||
|
PX4_ERR("Not accepting registration requests while armed");
|
||||||
|
register_ext_component_reply_s reply{};
|
||||||
|
reply.success = false;
|
||||||
|
static_assert(sizeof(request.name) == sizeof(reply.name), "size mismatch");
|
||||||
|
memcpy(reply.name, request.name, sizeof(request.name));
|
||||||
|
reply.request_id = request.request_id;
|
||||||
|
reply.px4_ros2_api_version = register_ext_component_request_s::LATEST_PX4_ROS2_API_VERSION;
|
||||||
|
reply.timestamp = hrt_absolute_time();
|
||||||
|
_register_ext_component_reply_pub.publish(reply);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// Check for unresponsive modes
|
||||||
|
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||||
|
if (_modes.valid(i)) {
|
||||||
|
const Modes::Mode &mode = _modes.mode(i);
|
||||||
|
|
||||||
|
// Remove only if not currently selected
|
||||||
|
if (user_intended_nav_state != i && _external_checks.isUnresponsive(mode.arming_check_registration_id)) {
|
||||||
|
PX4_DEBUG("Removing unresponsive mode %i", i);
|
||||||
|
_external_checks.removeRegistration(mode.arming_check_registration_id, i);
|
||||||
|
removeModeExecutor(mode.mode_executor_registration_id);
|
||||||
|
_modes.removeExternalMode(i, mode.name);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// As we're disarmed we can use the user intended mode, as no failsafe will be active
|
||||||
|
checkNewRegistrations(update_request);
|
||||||
|
checkUnregistrations(user_intended_nav_state, update_request);
|
||||||
|
}
|
||||||
|
|
||||||
|
update_request.control_setpoint_update = checkConfigControlSetpointUpdates();
|
||||||
|
checkConfigOverrides();
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state)
|
||||||
|
{
|
||||||
|
// Update mode executor in charge
|
||||||
|
int mode_executor_for_intended_nav_state = -1;
|
||||||
|
|
||||||
|
if (_modes.valid(user_intended_nav_state)) {
|
||||||
|
mode_executor_for_intended_nav_state = _modes.mode(user_intended_nav_state).mode_executor_registration_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (mode_executor_for_intended_nav_state == -1) {
|
||||||
|
// Not an owned mode: check source
|
||||||
|
if (source == ModeChangeSource::User) {
|
||||||
|
// Give control to the pilot
|
||||||
|
_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// Switched into an owned mode: put executor in charge
|
||||||
|
_mode_executor_in_charge = mode_executor_for_intended_nav_state;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t ModeManagement::getNavStateReplacementIfValid(uint8_t nav_state, bool report_error)
|
||||||
|
{
|
||||||
|
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||||
|
if (_modes.valid(i)) {
|
||||||
|
Modes::Mode &mode = _modes.mode(i);
|
||||||
|
|
||||||
|
if (mode.replaces_nav_state == nav_state) {
|
||||||
|
if (_external_checks.isUnresponsive(mode.arming_check_registration_id)) {
|
||||||
|
if (!mode.unresponsive_reported && report_error) {
|
||||||
|
mode.unresponsive_reported = true;
|
||||||
|
events::send(events::ID("commander_mode_fallback_internal"), events::Log::Critical,
|
||||||
|
"External mode is unresponsive, falling back to internal");
|
||||||
|
}
|
||||||
|
|
||||||
|
return nav_state;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
return i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return nav_state;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t ModeManagement::getReplacedModeIfAny(uint8_t nav_state)
|
||||||
|
{
|
||||||
|
if (_modes.valid(nav_state)) {
|
||||||
|
const Modes::Mode &mode = _modes.mode(nav_state);
|
||||||
|
|
||||||
|
if (mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
|
||||||
|
return mode.replaces_nav_state;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return nav_state;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::removeModeExecutor(int mode_executor_id)
|
||||||
|
{
|
||||||
|
if (mode_executor_id == -1) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_mode_executor_in_charge == mode_executor_id) {
|
||||||
|
_mode_executor_in_charge = ModeExecutors::AUTOPILOT_EXECUTOR_ID;
|
||||||
|
}
|
||||||
|
|
||||||
|
_mode_executors.removeExecutor(mode_executor_id);
|
||||||
|
}
|
||||||
|
|
||||||
|
int ModeManagement::modeExecutorInCharge() const
|
||||||
|
{
|
||||||
|
if (_failsafe_action_active) {
|
||||||
|
return ModeExecutors::AUTOPILOT_EXECUTOR_ID;
|
||||||
|
}
|
||||||
|
|
||||||
|
return _mode_executor_in_charge;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ModeManagement::updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const
|
||||||
|
{
|
||||||
|
bool ret = false;
|
||||||
|
|
||||||
|
if (nav_state >= Modes::FIRST_EXTERNAL_NAV_STATE && nav_state <= Modes::LAST_EXTERNAL_NAV_STATE) {
|
||||||
|
if (_modes.valid(nav_state)) {
|
||||||
|
control_mode = _modes.mode(nav_state).config_control_setpoint;
|
||||||
|
ret = true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
Modes::Mode::setControlModeDefaults(control_mode);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::printStatus() const
|
||||||
|
{
|
||||||
|
_modes.printStatus();
|
||||||
|
_mode_executors.printStatus(modeExecutorInCharge());
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out)
|
||||||
|
{
|
||||||
|
config_overrides_s current_overrides;
|
||||||
|
|
||||||
|
if (_modes.valid(nav_state)) {
|
||||||
|
current_overrides = _modes.mode(nav_state).overrides;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
current_overrides = {};
|
||||||
|
}
|
||||||
|
|
||||||
|
// Apply the overrides from executors on top (executors take precedence)
|
||||||
|
const int executor_in_charge = modeExecutorInCharge();
|
||||||
|
|
||||||
|
if (_mode_executors.valid(executor_in_charge)) {
|
||||||
|
const config_overrides_s &executor_overrides = _mode_executors.executor(executor_in_charge).overrides;
|
||||||
|
|
||||||
|
if (executor_overrides.disable_auto_disarm) {
|
||||||
|
current_overrides.disable_auto_disarm = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (executor_overrides.defer_failsafes) {
|
||||||
|
current_overrides.defer_failsafes = true;
|
||||||
|
current_overrides.defer_failsafes_timeout_s = executor_overrides.defer_failsafes_timeout_s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Publish if changed or at low rate
|
||||||
|
current_overrides.timestamp = overrides_in_out.timestamp;
|
||||||
|
|
||||||
|
if (memcmp(&overrides_in_out, ¤t_overrides, sizeof(current_overrides)) != 0
|
||||||
|
|| hrt_elapsed_time(¤t_overrides.timestamp) > 500_ms) {
|
||||||
|
current_overrides.timestamp = hrt_absolute_time();
|
||||||
|
_config_overrides_pub.publish(current_overrides);
|
||||||
|
overrides_in_out = current_overrides;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ModeManagement::checkConfigControlSetpointUpdates()
|
||||||
|
{
|
||||||
|
bool had_update = false;
|
||||||
|
vehicle_control_mode_s config_control_setpoint;
|
||||||
|
int max_updates = 5;
|
||||||
|
|
||||||
|
while (_config_control_setpoints_sub.update(&config_control_setpoint) && --max_updates >= 0) {
|
||||||
|
if (_modes.valid(config_control_setpoint.source_id)) {
|
||||||
|
_modes.mode(config_control_setpoint.source_id).config_control_setpoint = config_control_setpoint;
|
||||||
|
had_update = true;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
if (!_invalid_mode_printed) {
|
||||||
|
PX4_ERR("Control sp config request for invalid mode: %i", config_control_setpoint.source_id);
|
||||||
|
_invalid_mode_printed = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return had_update;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::checkConfigOverrides()
|
||||||
|
{
|
||||||
|
config_overrides_s override_request;
|
||||||
|
int max_updates = config_overrides_s::ORB_QUEUE_LENGTH;
|
||||||
|
|
||||||
|
while (_config_overrides_request_sub.update(&override_request) && --max_updates >= 0) {
|
||||||
|
switch (override_request.source_type) {
|
||||||
|
case config_overrides_s::SOURCE_TYPE_MODE_EXECUTOR:
|
||||||
|
if (_mode_executors.valid(override_request.source_id)) {
|
||||||
|
ModeExecutors::ModeExecutor &executor = _mode_executors.executor(override_request.source_id);
|
||||||
|
memcpy(&executor.overrides, &override_request, sizeof(executor.overrides));
|
||||||
|
static_assert(sizeof(executor.overrides) == sizeof(override_request), "size mismatch");
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
|
||||||
|
case config_overrides_s::SOURCE_TYPE_MODE:
|
||||||
|
if (_modes.valid(override_request.source_id)) {
|
||||||
|
Modes::Mode &mode = _modes.mode(override_request.source_id);
|
||||||
|
memcpy(&mode.overrides, &override_request, sizeof(mode.overrides));
|
||||||
|
}
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void ModeManagement::getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const
|
||||||
|
{
|
||||||
|
valid_nav_state_mask = mode_util::getValidNavStates();
|
||||||
|
can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION);
|
||||||
|
|
||||||
|
// Add external modes
|
||||||
|
for (int i = Modes::FIRST_EXTERNAL_NAV_STATE; i <= Modes::LAST_EXTERNAL_NAV_STATE; ++i) {
|
||||||
|
if (_modes.valid(i)) {
|
||||||
|
valid_nav_state_mask |= 1u << i;
|
||||||
|
can_set_nav_state_mask |= 1u << i;
|
||||||
|
const Modes::Mode &cur_mode = _modes.mode(i);
|
||||||
|
|
||||||
|
if (cur_mode.replaces_nav_state != Modes::Mode::REPLACES_NAV_STATE_NONE) {
|
||||||
|
// Hide the internal mode if it's replaced
|
||||||
|
can_set_nav_state_mask &= ~(1u << cur_mode.replaces_nav_state);
|
||||||
|
}
|
||||||
|
|
||||||
|
} else {
|
||||||
|
// Still set the mode as valid but not as selectable. This is because an external mode could still
|
||||||
|
// be selected via RC when not yet running, so we make sure to display some mode label indicating it's not
|
||||||
|
// available.
|
||||||
|
valid_nav_state_mask |= 1u << i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* CONSTRAINED_FLASH */
|
|
@ -0,0 +1,225 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <uORB/Publication.hpp>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
#include <uORB/topics/register_ext_component_request.h>
|
||||||
|
#include <uORB/topics/register_ext_component_reply.h>
|
||||||
|
#include <uORB/topics/unregister_ext_component.h>
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/vehicle_control_mode.h>
|
||||||
|
#include <uORB/topics/config_overrides.h>
|
||||||
|
|
||||||
|
#include <lib/modes/ui.hpp>
|
||||||
|
#include "UserModeIntention.hpp"
|
||||||
|
#include "HealthAndArmingChecks/checks/externalChecks.hpp"
|
||||||
|
|
||||||
|
class ModeExecutors
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static constexpr int AUTOPILOT_EXECUTOR_ID = 0;
|
||||||
|
static constexpr int FIRST_EXECUTOR_ID = 1;
|
||||||
|
static constexpr int MAX_NUM = 5;
|
||||||
|
|
||||||
|
struct ModeExecutor {
|
||||||
|
config_overrides_s overrides{};
|
||||||
|
uint8_t owned_nav_state{};
|
||||||
|
bool valid{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
void printStatus(int executor_in_charge) const;
|
||||||
|
|
||||||
|
bool valid(int id) const { return id >= FIRST_EXECUTOR_ID && id < FIRST_EXECUTOR_ID + MAX_NUM && _mode_executors[id - FIRST_EXECUTOR_ID].valid; }
|
||||||
|
const ModeExecutor &executor(int id) const { return _mode_executors[id - FIRST_EXECUTOR_ID]; }
|
||||||
|
ModeExecutor &executor(int id) { return _mode_executors[id - FIRST_EXECUTOR_ID]; }
|
||||||
|
|
||||||
|
bool hasFreeExecutors() const;
|
||||||
|
int addExecutor(const ModeExecutor &executor);
|
||||||
|
void removeExecutor(int id);
|
||||||
|
private:
|
||||||
|
ModeExecutor _mode_executors[MAX_NUM] {};
|
||||||
|
};
|
||||||
|
|
||||||
|
class Modes
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static constexpr uint8_t FIRST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
|
||||||
|
static constexpr uint8_t LAST_EXTERNAL_NAV_STATE = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8;
|
||||||
|
static constexpr int MAX_NUM = LAST_EXTERNAL_NAV_STATE - FIRST_EXTERNAL_NAV_STATE + 1;
|
||||||
|
|
||||||
|
struct Mode {
|
||||||
|
Mode()
|
||||||
|
{
|
||||||
|
// Set defaults for control mode
|
||||||
|
setControlModeDefaults(config_control_setpoint);
|
||||||
|
}
|
||||||
|
static void setControlModeDefaults(vehicle_control_mode_s &config_control_setpoint_)
|
||||||
|
{
|
||||||
|
config_control_setpoint_.flag_control_position_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_velocity_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_altitude_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_climb_rate_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_acceleration_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_rates_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_attitude_enabled = true;
|
||||||
|
config_control_setpoint_.flag_control_allocation_enabled = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
static constexpr uint8_t REPLACES_NAV_STATE_NONE = 0xff;
|
||||||
|
|
||||||
|
char name[sizeof(register_ext_component_request_s::name)] {};
|
||||||
|
bool valid{false};
|
||||||
|
uint8_t replaces_nav_state{REPLACES_NAV_STATE_NONE};
|
||||||
|
bool unresponsive_reported{false};
|
||||||
|
int arming_check_registration_id{-1};
|
||||||
|
int mode_executor_registration_id{-1};
|
||||||
|
config_overrides_s overrides{};
|
||||||
|
vehicle_control_mode_s config_control_setpoint{};
|
||||||
|
};
|
||||||
|
|
||||||
|
void printStatus() const;
|
||||||
|
|
||||||
|
bool valid(uint8_t nav_state) const { return nav_state >= FIRST_EXTERNAL_NAV_STATE && nav_state <= LAST_EXTERNAL_NAV_STATE && _modes[nav_state - FIRST_EXTERNAL_NAV_STATE].valid; }
|
||||||
|
Mode &mode(uint8_t nav_state) { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; }
|
||||||
|
const Mode &mode(uint8_t nav_state) const { return _modes[nav_state - FIRST_EXTERNAL_NAV_STATE]; }
|
||||||
|
|
||||||
|
bool hasFreeExternalModes() const;
|
||||||
|
uint8_t addExternalMode(const Mode &mode);
|
||||||
|
bool removeExternalMode(uint8_t nav_state, const char *name);
|
||||||
|
|
||||||
|
private:
|
||||||
|
Mode _modes[MAX_NUM] {};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef CONSTRAINED_FLASH
|
||||||
|
|
||||||
|
class ModeManagement : public ModeChangeHandler
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ModeManagement(ExternalChecks &external_checks);
|
||||||
|
~ModeManagement() = default;
|
||||||
|
|
||||||
|
struct UpdateRequest {
|
||||||
|
bool change_user_intended_nav_state{false};
|
||||||
|
uint8_t user_intended_nav_state{};
|
||||||
|
bool control_setpoint_update{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Mode executor ID for who is currently in charge (and can send commands etc).
|
||||||
|
* This is ModeExecutors::AUTOPILOT_EXECUTOR_ID if no executor is in charge currently.
|
||||||
|
*/
|
||||||
|
int modeExecutorInCharge() const;
|
||||||
|
|
||||||
|
void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override;
|
||||||
|
uint8_t getReplacedModeIfAny(uint8_t nav_state) override;
|
||||||
|
|
||||||
|
uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true);
|
||||||
|
|
||||||
|
bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const;
|
||||||
|
|
||||||
|
void printStatus() const;
|
||||||
|
|
||||||
|
void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const;
|
||||||
|
|
||||||
|
void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out);
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool checkConfigControlSetpointUpdates();
|
||||||
|
void checkNewRegistrations(UpdateRequest &update_request);
|
||||||
|
void checkUnregistrations(uint8_t user_intended_nav_state, UpdateRequest &update_request);
|
||||||
|
void checkConfigOverrides();
|
||||||
|
|
||||||
|
void removeModeExecutor(int mode_executor_id);
|
||||||
|
|
||||||
|
uORB::Subscription _config_control_setpoints_sub{ORB_ID(config_control_setpoints)};
|
||||||
|
uORB::Subscription _register_ext_component_request_sub{ORB_ID(register_ext_component_request)};
|
||||||
|
uORB::Subscription _unregister_ext_component_sub{ORB_ID(unregister_ext_component)};
|
||||||
|
uORB::Publication<register_ext_component_reply_s> _register_ext_component_reply_pub{ORB_ID(register_ext_component_reply)};
|
||||||
|
uORB::Publication<config_overrides_s> _config_overrides_pub{ORB_ID(config_overrides)};
|
||||||
|
uORB::Subscription _config_overrides_request_sub{ORB_ID(config_overrides_request)};
|
||||||
|
|
||||||
|
ExternalChecks &_external_checks;
|
||||||
|
ModeExecutors _mode_executors;
|
||||||
|
Modes _modes;
|
||||||
|
|
||||||
|
bool _failsafe_action_active{false};
|
||||||
|
int _mode_executor_in_charge{ModeExecutors::AUTOPILOT_EXECUTOR_ID};
|
||||||
|
|
||||||
|
bool _invalid_mode_printed{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
#else /* CONSTRAINED_FLASH */
|
||||||
|
|
||||||
|
class ModeManagement : public ModeChangeHandler
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
ModeManagement() = default;
|
||||||
|
~ModeManagement() = default;
|
||||||
|
|
||||||
|
struct UpdateRequest {
|
||||||
|
bool change_user_intended_nav_state{false};
|
||||||
|
uint8_t user_intended_nav_state{};
|
||||||
|
bool control_setpoint_update{false};
|
||||||
|
};
|
||||||
|
|
||||||
|
void update(bool armed, uint8_t user_intended_nav_state, bool failsafe_action_active, UpdateRequest &update_request) {}
|
||||||
|
|
||||||
|
int modeExecutorInCharge() const { return ModeExecutors::AUTOPILOT_EXECUTOR_ID; }
|
||||||
|
|
||||||
|
void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) override {}
|
||||||
|
uint8_t getReplacedModeIfAny(uint8_t nav_state) override { return nav_state; }
|
||||||
|
|
||||||
|
uint8_t getNavStateReplacementIfValid(uint8_t nav_state, bool report_error = true) { return nav_state; }
|
||||||
|
|
||||||
|
bool updateControlMode(uint8_t nav_state, vehicle_control_mode_s &control_mode) const { return false; }
|
||||||
|
|
||||||
|
void printStatus() const {}
|
||||||
|
|
||||||
|
void getModeStatus(uint32_t &valid_nav_state_mask, uint32_t &can_set_nav_state_mask) const
|
||||||
|
{
|
||||||
|
valid_nav_state_mask = mode_util::getValidNavStates();
|
||||||
|
can_set_nav_state_mask = valid_nav_state_mask & ~(1u << vehicle_status_s::NAVIGATION_STATE_TERMINATION);
|
||||||
|
}
|
||||||
|
|
||||||
|
void updateActiveConfigOverrides(uint8_t nav_state, config_overrides_s &overrides_in_out) { }
|
||||||
|
|
||||||
|
private:
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif /* CONSTRAINED_FLASH */
|
|
@ -0,0 +1,101 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#include <gtest/gtest.h>
|
||||||
|
#include "ModeManagement.hpp"
|
||||||
|
|
||||||
|
static bool modeValid(uint8_t mode)
|
||||||
|
{
|
||||||
|
return mode >= Modes::FIRST_EXTERNAL_NAV_STATE && mode <= Modes::LAST_EXTERNAL_NAV_STATE;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int32_t readHash(int idx)
|
||||||
|
{
|
||||||
|
char buffer[20];
|
||||||
|
snprintf(buffer, sizeof(buffer), "COM_MODE%u_HASH", idx);
|
||||||
|
param_t param = param_find(buffer);
|
||||||
|
int32_t value{};
|
||||||
|
param_get(param, &value);
|
||||||
|
return value;
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST(ModeManagementTest, Hashes)
|
||||||
|
{
|
||||||
|
param_control_autosave(false);
|
||||||
|
|
||||||
|
// Reset parameters
|
||||||
|
for (int i = 0; i < Modes::MAX_NUM; ++i) {
|
||||||
|
char buffer[20];
|
||||||
|
snprintf(buffer, sizeof(buffer), "COM_MODE%u_HASH", i);
|
||||||
|
param_t param = param_find(buffer);
|
||||||
|
param_reset(param);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add full set of modes, which stores the hashes
|
||||||
|
Modes modes;
|
||||||
|
Modes::Mode mode;
|
||||||
|
|
||||||
|
for (int i = 0; i < Modes::MAX_NUM; ++i) {
|
||||||
|
snprintf(mode.name, sizeof(mode.name), "mode %i", i);
|
||||||
|
EXPECT_EQ(modes.addExternalMode(mode), Modes::FIRST_EXTERNAL_NAV_STATE + i);
|
||||||
|
EXPECT_EQ(readHash(i), events::util::hash_32_fnv1a_const(mode.name));
|
||||||
|
}
|
||||||
|
|
||||||
|
EXPECT_FALSE(modes.hasFreeExternalModes());
|
||||||
|
|
||||||
|
// Remove all modes, except last
|
||||||
|
for (int i = 0; i < Modes::MAX_NUM - 1; ++i) {
|
||||||
|
snprintf(mode.name, sizeof(mode.name), "mode %i", i);
|
||||||
|
EXPECT_TRUE(modes.removeExternalMode(Modes::FIRST_EXTERNAL_NAV_STATE + i, mode.name));
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add some mode, ensure it gets the same index
|
||||||
|
const int mode_to_add_idx = 3;
|
||||||
|
snprintf(mode.name, sizeof(mode.name), "mode %i", mode_to_add_idx);
|
||||||
|
EXPECT_EQ(modes.addExternalMode(mode), Modes::FIRST_EXTERNAL_NAV_STATE + mode_to_add_idx);
|
||||||
|
|
||||||
|
// Try to add another one with the same name: should succeed, with the hash of the added index reset
|
||||||
|
uint8_t added_mode_nav_state = modes.addExternalMode(mode);
|
||||||
|
EXPECT_EQ(readHash(added_mode_nav_state - Modes::FIRST_EXTERNAL_NAV_STATE), 0);
|
||||||
|
|
||||||
|
// 3 Modes are used now. Add N-3 new ones which must overwrite previous hashes
|
||||||
|
for (int i = 0; i < Modes::MAX_NUM - 3; ++i) {
|
||||||
|
snprintf(mode.name, sizeof(mode.name), "new mode %i", i);
|
||||||
|
added_mode_nav_state = modes.addExternalMode(mode);
|
||||||
|
EXPECT_TRUE(modeValid(added_mode_nav_state));
|
||||||
|
EXPECT_EQ(readHash(added_mode_nav_state - Modes::FIRST_EXTERNAL_NAV_STATE),
|
||||||
|
events::util::hash_32_fnv1a_const(mode.name));
|
||||||
|
}
|
||||||
|
|
||||||
|
EXPECT_FALSE(modes.hasFreeExternalModes());
|
||||||
|
}
|
|
@ -42,11 +42,10 @@ static bool stabilization_required(uint8_t vehicle_type)
|
||||||
return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
return vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
|
||||||
}
|
}
|
||||||
|
|
||||||
void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
|
void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
|
||||||
const offboard_control_mode_s &offboard_control_mode,
|
const offboard_control_mode_s &offboard_control_mode,
|
||||||
vehicle_control_mode_s &vehicle_control_mode)
|
vehicle_control_mode_s &vehicle_control_mode)
|
||||||
{
|
{
|
||||||
vehicle_control_mode.flag_armed = armed;
|
|
||||||
vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default
|
vehicle_control_mode.flag_control_allocation_enabled = true; // Always enabled by default
|
||||||
|
|
||||||
switch (nav_state) {
|
switch (nav_state) {
|
||||||
|
@ -163,17 +162,11 @@ void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
|
||||||
vehicle_control_mode.flag_control_velocity_enabled = true;
|
vehicle_control_mode.flag_control_velocity_enabled = true;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
vehicle_control_mode.flag_multicopter_position_control_enabled =
|
|
||||||
(vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
|
|
||||||
&& (vehicle_control_mode.flag_control_altitude_enabled
|
|
||||||
|| vehicle_control_mode.flag_control_climb_rate_enabled
|
|
||||||
|| vehicle_control_mode.flag_control_position_enabled
|
|
||||||
|| vehicle_control_mode.flag_control_velocity_enabled
|
|
||||||
|| vehicle_control_mode.flag_control_acceleration_enabled);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace mode_util
|
} // namespace mode_util
|
||||||
|
|
|
@ -41,7 +41,7 @@
|
||||||
namespace mode_util
|
namespace mode_util
|
||||||
{
|
{
|
||||||
|
|
||||||
void getVehicleControlMode(bool armed, uint8_t nav_state, uint8_t vehicle_type,
|
void getVehicleControlMode(uint8_t nav_state, uint8_t vehicle_type,
|
||||||
const offboard_control_mode_s &offboard_control_mode,
|
const offboard_control_mode_s &offboard_control_mode,
|
||||||
vehicle_control_mode_s &vehicle_control_mode);
|
vehicle_control_mode_s &vehicle_control_mode);
|
||||||
|
|
||||||
|
|
|
@ -75,36 +75,27 @@ static inline navigation_mode_t navigation_mode(uint8_t nav_state)
|
||||||
case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;
|
case vehicle_status_s::NAVIGATION_STATE_ORBIT: return navigation_mode_t::orbit;
|
||||||
|
|
||||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
|
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1: return navigation_mode_t::external1;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2: return navigation_mode_t::external2;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3: return navigation_mode_t::external3;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4: return navigation_mode_t::external4;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5: return navigation_mode_t::external5;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6: return navigation_mode_t::external6;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7: return navigation_mode_t::external7;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8: return navigation_mode_t::external8;
|
||||||
}
|
}
|
||||||
|
|
||||||
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "code requires update");
|
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "code requires update");
|
||||||
|
|
||||||
return navigation_mode_t::unknown;
|
return navigation_mode_t::unknown;
|
||||||
}
|
}
|
||||||
|
|
||||||
const char *const nav_state_names[vehicle_status_s::NAVIGATION_STATE_MAX] = {
|
|
||||||
"MANUAL",
|
|
||||||
"ALTCTL",
|
|
||||||
"POSCTL",
|
|
||||||
"AUTO_MISSION",
|
|
||||||
"AUTO_LOITER",
|
|
||||||
"AUTO_RTL",
|
|
||||||
"6: unallocated",
|
|
||||||
"7: unallocated",
|
|
||||||
"AUTO_LANDENGFAIL",
|
|
||||||
"9: unallocated",
|
|
||||||
"ACRO",
|
|
||||||
"11: UNUSED",
|
|
||||||
"DESCEND",
|
|
||||||
"TERMINATION",
|
|
||||||
"OFFBOARD",
|
|
||||||
"STAB",
|
|
||||||
"16: UNUSED2",
|
|
||||||
"AUTO_TAKEOFF",
|
|
||||||
"AUTO_LAND",
|
|
||||||
"AUTO_FOLLOW_TARGET",
|
|
||||||
"AUTO_PRECLAND",
|
|
||||||
"ORBIT"
|
|
||||||
};
|
|
||||||
|
|
||||||
} // namespace mode_util
|
} // namespace mode_util
|
||||||
|
|
|
@ -170,8 +170,9 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_position);
|
||||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt);
|
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF, flags.mode_req_local_alt);
|
||||||
|
|
||||||
|
// NAVIGATION_STATE_EXTERNALx: handled outside
|
||||||
|
|
||||||
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 23, "update mode requirements");
|
static_assert(vehicle_status_s::NAVIGATION_STATE_MAX == 31, "update mode requirements");
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace mode_util
|
} // namespace mode_util
|
||||||
|
|
|
@ -35,14 +35,22 @@
|
||||||
#include "UserModeIntention.hpp"
|
#include "UserModeIntention.hpp"
|
||||||
|
|
||||||
UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
|
UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
|
||||||
const HealthAndArmingChecks &health_and_arming_checks)
|
const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler)
|
||||||
: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks)
|
: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(health_and_arming_checks),
|
||||||
|
_handler(handler)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallback, bool force)
|
bool UserModeIntention::change(uint8_t user_intended_nav_state, ModeChangeSource source, bool allow_fallback,
|
||||||
|
bool force)
|
||||||
{
|
{
|
||||||
_ever_had_mode_change = true;
|
_ever_had_mode_change = true;
|
||||||
|
_had_mode_change = true;
|
||||||
|
|
||||||
|
if (_handler) {
|
||||||
|
// If a replacement mode is selected, select the internal one instead. The replacement will be selected after.
|
||||||
|
user_intended_nav_state = _handler->getReplacedModeIfAny(user_intended_nav_state);
|
||||||
|
}
|
||||||
|
|
||||||
// Always allow mode change while disarmed
|
// Always allow mode change while disarmed
|
||||||
bool always_allow = force || !isArmed();
|
bool always_allow = force || !isArmed();
|
||||||
|
@ -68,6 +76,10 @@ bool UserModeIntention::change(uint8_t user_intended_nav_state, bool allow_fallb
|
||||||
if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
|
if (!_health_and_arming_checks.modePreventsArming(user_intended_nav_state)) {
|
||||||
_nav_state_after_disarming = user_intended_nav_state;
|
_nav_state_after_disarming = user_intended_nav_state;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (_handler) {
|
||||||
|
_handler->onUserIntendedNavStateChange(source, user_intended_nav_state);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return allow_change;
|
return allow_change;
|
||||||
|
|
|
@ -37,21 +37,42 @@
|
||||||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||||
#include <px4_platform_common/module_params.h>
|
#include <px4_platform_common/module_params.h>
|
||||||
|
|
||||||
|
enum class ModeChangeSource {
|
||||||
|
User, ///< RC or MAVLink
|
||||||
|
ModeExecutor,
|
||||||
|
};
|
||||||
|
|
||||||
|
class ModeChangeHandler
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual void onUserIntendedNavStateChange(ModeChangeSource source, uint8_t user_intended_nav_state) = 0;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Get the replaced (internal) mode for a given (external) mode
|
||||||
|
* @param nav_state
|
||||||
|
* @return nav_state or the mode that nav_state replaces
|
||||||
|
*/
|
||||||
|
virtual uint8_t getReplacedModeIfAny(uint8_t nav_state) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
class UserModeIntention : ModuleParams
|
class UserModeIntention : ModuleParams
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
|
UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
|
||||||
const HealthAndArmingChecks &health_and_arming_checks);
|
const HealthAndArmingChecks &health_and_arming_checks, ModeChangeHandler *handler);
|
||||||
~UserModeIntention() = default;
|
~UserModeIntention() = default;
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Change the user intended mode
|
* Change the user intended mode
|
||||||
* @param user_intended_nav_state new mode
|
* @param user_intended_nav_state new mode
|
||||||
|
* @param source calling reason
|
||||||
* @param allow_fallback allow to fallback to a lower mode if current mode cannot run
|
* @param allow_fallback allow to fallback to a lower mode if current mode cannot run
|
||||||
* @param force always set if true
|
* @param force always set if true
|
||||||
* @return true if successfully set (also if unchanged)
|
* @return true if successfully set (also if unchanged)
|
||||||
*/
|
*/
|
||||||
bool change(uint8_t user_intended_nav_state, bool allow_fallback = false, bool force = false);
|
bool change(uint8_t user_intended_nav_state, ModeChangeSource source = ModeChangeSource::User,
|
||||||
|
bool allow_fallback = false, bool force = false);
|
||||||
|
|
||||||
uint8_t get() const { return _user_intented_nav_state; }
|
uint8_t get() const { return _user_intented_nav_state; }
|
||||||
|
|
||||||
|
@ -72,6 +93,7 @@ private:
|
||||||
|
|
||||||
const vehicle_status_s &_vehicle_status;
|
const vehicle_status_s &_vehicle_status;
|
||||||
const HealthAndArmingChecks &_health_and_arming_checks;
|
const HealthAndArmingChecks &_health_and_arming_checks;
|
||||||
|
ModeChangeHandler *const _handler{nullptr};
|
||||||
|
|
||||||
uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
|
uint8_t _user_intented_nav_state{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Current user intended mode
|
||||||
uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming
|
uint8_t _nav_state_after_disarming{vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER}; ///< Mode that is switched into after landing/disarming
|
||||||
|
|
|
@ -345,150 +345,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
|
||||||
*/
|
*/
|
||||||
PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f);
|
PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f);
|
||||||
|
|
||||||
/**
|
|
||||||
* First flightmode slot (1000-1160)
|
|
||||||
*
|
|
||||||
* If the main switch channel is in this range the
|
|
||||||
* selected flight mode will be applied.
|
|
||||||
*
|
|
||||||
* @value -1 Unassigned
|
|
||||||
* @value 0 Manual
|
|
||||||
* @value 1 Altitude
|
|
||||||
* @value 2 Position
|
|
||||||
* @value 3 Mission
|
|
||||||
* @value 4 Hold
|
|
||||||
* @value 10 Takeoff
|
|
||||||
* @value 11 Land
|
|
||||||
* @value 5 Return
|
|
||||||
* @value 6 Acro
|
|
||||||
* @value 7 Offboard
|
|
||||||
* @value 8 Stabilized
|
|
||||||
* @value 12 Follow Me
|
|
||||||
* @value 13 Precision Land
|
|
||||||
* @group Commander
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(COM_FLTMODE1, -1);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Second flightmode slot (1160-1320)
|
|
||||||
*
|
|
||||||
* If the main switch channel is in this range the
|
|
||||||
* selected flight mode will be applied.
|
|
||||||
*
|
|
||||||
* @value -1 Unassigned
|
|
||||||
* @value 0 Manual
|
|
||||||
* @value 1 Altitude
|
|
||||||
* @value 2 Position
|
|
||||||
* @value 3 Mission
|
|
||||||
* @value 4 Hold
|
|
||||||
* @value 10 Takeoff
|
|
||||||
* @value 11 Land
|
|
||||||
* @value 5 Return
|
|
||||||
* @value 6 Acro
|
|
||||||
* @value 7 Offboard
|
|
||||||
* @value 8 Stabilized
|
|
||||||
* @value 12 Follow Me
|
|
||||||
* @value 13 Precision Land
|
|
||||||
* @group Commander
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(COM_FLTMODE2, -1);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Third flightmode slot (1320-1480)
|
|
||||||
*
|
|
||||||
* If the main switch channel is in this range the
|
|
||||||
* selected flight mode will be applied.
|
|
||||||
*
|
|
||||||
* @value -1 Unassigned
|
|
||||||
* @value 0 Manual
|
|
||||||
* @value 1 Altitude
|
|
||||||
* @value 2 Position
|
|
||||||
* @value 3 Mission
|
|
||||||
* @value 4 Hold
|
|
||||||
* @value 10 Takeoff
|
|
||||||
* @value 11 Land
|
|
||||||
* @value 5 Return
|
|
||||||
* @value 6 Acro
|
|
||||||
* @value 7 Offboard
|
|
||||||
* @value 8 Stabilized
|
|
||||||
* @value 12 Follow Me
|
|
||||||
* @value 13 Precision Land
|
|
||||||
* @group Commander
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(COM_FLTMODE3, -1);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Fourth flightmode slot (1480-1640)
|
|
||||||
*
|
|
||||||
* If the main switch channel is in this range the
|
|
||||||
* selected flight mode will be applied.
|
|
||||||
*
|
|
||||||
* @value -1 Unassigned
|
|
||||||
* @value 0 Manual
|
|
||||||
* @value 1 Altitude
|
|
||||||
* @value 2 Position
|
|
||||||
* @value 3 Mission
|
|
||||||
* @value 4 Hold
|
|
||||||
* @value 10 Takeoff
|
|
||||||
* @value 11 Land
|
|
||||||
* @value 5 Return
|
|
||||||
* @value 6 Acro
|
|
||||||
* @value 7 Offboard
|
|
||||||
* @value 8 Stabilized
|
|
||||||
* @value 12 Follow Me
|
|
||||||
* @value 13 Precision Land
|
|
||||||
* @group Commander
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(COM_FLTMODE4, -1);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Fifth flightmode slot (1640-1800)
|
|
||||||
*
|
|
||||||
* If the main switch channel is in this range the
|
|
||||||
* selected flight mode will be applied.
|
|
||||||
*
|
|
||||||
* @value -1 Unassigned
|
|
||||||
* @value 0 Manual
|
|
||||||
* @value 1 Altitude
|
|
||||||
* @value 2 Position
|
|
||||||
* @value 3 Mission
|
|
||||||
* @value 4 Hold
|
|
||||||
* @value 10 Takeoff
|
|
||||||
* @value 11 Land
|
|
||||||
* @value 5 Return
|
|
||||||
* @value 6 Acro
|
|
||||||
* @value 7 Offboard
|
|
||||||
* @value 8 Stabilized
|
|
||||||
* @value 12 Follow Me
|
|
||||||
* @value 13 Precision Land
|
|
||||||
* @group Commander
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(COM_FLTMODE5, -1);
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Sixth flightmode slot (1800-2000)
|
|
||||||
*
|
|
||||||
* If the main switch channel is in this range the
|
|
||||||
* selected flight mode will be applied.
|
|
||||||
*
|
|
||||||
* @value -1 Unassigned
|
|
||||||
* @value 0 Manual
|
|
||||||
* @value 1 Altitude
|
|
||||||
* @value 2 Position
|
|
||||||
* @value 3 Mission
|
|
||||||
* @value 4 Hold
|
|
||||||
* @value 10 Takeoff
|
|
||||||
* @value 11 Land
|
|
||||||
* @value 5 Return
|
|
||||||
* @value 6 Acro
|
|
||||||
* @value 7 Offboard
|
|
||||||
* @value 8 Stabilized
|
|
||||||
* @value 12 Follow Me
|
|
||||||
* @value 13 Precision Land
|
|
||||||
* @group Commander
|
|
||||||
*/
|
|
||||||
PARAM_DEFINE_INT32(COM_FLTMODE6, -1);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Maximum EKF position innovation test ratio that will allow arming
|
* Maximum EKF position innovation test ratio that will allow arming
|
||||||
*
|
*
|
||||||
|
|
|
@ -0,0 +1,50 @@
|
||||||
|
module_name: Commander
|
||||||
|
parameters:
|
||||||
|
- group: Commander
|
||||||
|
definitions:
|
||||||
|
COM_MODE${i}_HASH:
|
||||||
|
description:
|
||||||
|
short: External mode identifier ${i}
|
||||||
|
long: |
|
||||||
|
This parameter is automatically set to identify external modes. It ensures that modes
|
||||||
|
get assigned to the same index independent from their startup order,
|
||||||
|
which is required when mapping an external mode to an RC switch.
|
||||||
|
type: int32
|
||||||
|
num_instances: 8 # Max 8 modes (NAVIGATION_STATE_EXTERNAL8)
|
||||||
|
default: 0
|
||||||
|
volatile: true
|
||||||
|
category: System
|
||||||
|
|
||||||
|
COM_FLTMODE${i}:
|
||||||
|
description:
|
||||||
|
short: Mode slot ${i}
|
||||||
|
long: |
|
||||||
|
If the main switch channel is in this range the
|
||||||
|
selected flight mode will be applied.
|
||||||
|
type: enum
|
||||||
|
values:
|
||||||
|
-1: Unassigned
|
||||||
|
0: Manual
|
||||||
|
1: Altitude
|
||||||
|
2: Position
|
||||||
|
3: Mission
|
||||||
|
4: Hold
|
||||||
|
10: Takeoff
|
||||||
|
11: Land
|
||||||
|
5: Return
|
||||||
|
6: Acro
|
||||||
|
7: Offboard
|
||||||
|
8: Stabilized
|
||||||
|
12: Follow Me
|
||||||
|
13: Precision Land
|
||||||
|
100: External Mode 1
|
||||||
|
101: External Mode 2
|
||||||
|
102: External Mode 3
|
||||||
|
103: External Mode 4
|
||||||
|
104: External Mode 5
|
||||||
|
105: External Mode 6
|
||||||
|
106: External Mode 7
|
||||||
|
107: External Mode 8
|
||||||
|
instance_start: 1
|
||||||
|
num_instances: 6
|
||||||
|
default: -1
|
|
@ -50,7 +50,8 @@ enum PX4_CUSTOM_MAIN_MODE {
|
||||||
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
|
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
|
||||||
PX4_CUSTOM_MAIN_MODE_STABILIZED,
|
PX4_CUSTOM_MAIN_MODE_STABILIZED,
|
||||||
PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
|
PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
|
||||||
PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
|
PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */
|
||||||
|
PX4_CUSTOM_MAIN_MODE_TERMINATION
|
||||||
};
|
};
|
||||||
|
|
||||||
enum PX4_CUSTOM_SUB_MODE_AUTO {
|
enum PX4_CUSTOM_SUB_MODE_AUTO {
|
||||||
|
@ -63,7 +64,15 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
|
||||||
PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
|
PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
|
||||||
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
|
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
|
||||||
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
|
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
|
||||||
PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
|
PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL1,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL2,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL3,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL4,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL5,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL6,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL7,
|
||||||
|
PX4_CUSTOM_SUB_MODE_EXTERNAL8,
|
||||||
};
|
};
|
||||||
|
|
||||||
enum PX4_CUSTOM_SUB_MODE_POSCTL {
|
enum PX4_CUSTOM_SUB_MODE_POSCTL {
|
||||||
|
@ -131,7 +140,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
|
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
|
||||||
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_TERMINATION;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
|
||||||
|
@ -171,6 +180,46 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
|
||||||
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL1:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL1;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL2:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL2;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL3:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL3;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL4:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL4;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL5:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL5;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL6:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL6;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL7:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL7;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case vehicle_status_s::NAVIGATION_STATE_EXTERNAL8:
|
||||||
|
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
|
||||||
|
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_EXTERNAL8;
|
||||||
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
return custom_mode;
|
return custom_mode;
|
||||||
|
|
|
@ -137,7 +137,9 @@ void FlightModeManager::updateParams()
|
||||||
void FlightModeManager::start_flight_task()
|
void FlightModeManager::start_flight_task()
|
||||||
{
|
{
|
||||||
// Do not run any flight task for VTOLs in fixed-wing mode
|
// Do not run any flight task for VTOLs in fixed-wing mode
|
||||||
if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
|
||||||
|
|| ((_vehicle_status_sub.get().nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1)
|
||||||
|
&& (_vehicle_status_sub.get().nav_state <= vehicle_status_s::NAVIGATION_STATE_EXTERNAL8))) {
|
||||||
switchTask(FlightTaskIndex::None);
|
switchTask(FlightTaskIndex::None);
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
|
@ -55,6 +55,7 @@ void LoggedTopics::add_default_topics()
|
||||||
add_optional_topic("camera_trigger");
|
add_optional_topic("camera_trigger");
|
||||||
add_topic("cellular_status", 200);
|
add_topic("cellular_status", 200);
|
||||||
add_topic("commander_state");
|
add_topic("commander_state");
|
||||||
|
add_topic("config_overrides");
|
||||||
add_topic("cpuload");
|
add_topic("cpuload");
|
||||||
add_optional_topic("external_ins_attitude");
|
add_optional_topic("external_ins_attitude");
|
||||||
add_optional_topic("external_ins_global_position");
|
add_optional_topic("external_ins_global_position");
|
||||||
|
|
|
@ -555,6 +555,15 @@ int8_t ManualControl::navStateFromParam(int32_t param_value)
|
||||||
case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
case 13: return vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
||||||
case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
|
case 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
|
||||||
case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
|
case 15: return vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
|
||||||
|
|
||||||
|
case 100: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
|
||||||
|
case 101: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL2;
|
||||||
|
case 102: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL3;
|
||||||
|
case 103: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL4;
|
||||||
|
case 104: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL5;
|
||||||
|
case 105: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL6;
|
||||||
|
case 106: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL7;
|
||||||
|
case 107: return vehicle_status_s::NAVIGATION_STATE_EXTERNAL8;
|
||||||
}
|
}
|
||||||
return -1;
|
return -1;
|
||||||
}
|
}
|
||||||
|
|
|
@ -68,7 +68,8 @@ MavlinkCommandSender::~MavlinkCommandSender()
|
||||||
int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel)
|
int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel)
|
||||||
{
|
{
|
||||||
// commands > uint16 are PX4 internal only
|
// commands > uint16 are PX4 internal only
|
||||||
if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START) {
|
if (command.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
|
||||||
|
|| command.source_component >= vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -1186,8 +1186,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||||
return OK;
|
return OK;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* if we reach here, the stream list does not contain the stream */
|
// if we reach here, the stream list does not contain the stream.
|
||||||
#if defined(CONSTRAINED_FLASH) // flash constrained target's don't include all streams
|
// flash constrained target's don't include all streams, and some are only available for the development dialect
|
||||||
|
#if defined(CONSTRAINED_FLASH) || !defined(MAVLINK_DEVELOPMENT_H)
|
||||||
return PX4_OK;
|
return PX4_OK;
|
||||||
#else
|
#else
|
||||||
PX4_WARN("stream %s not found", stream_name);
|
PX4_WARN("stream %s not found", stream_name);
|
||||||
|
@ -1399,9 +1400,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||||
configure_stream_local("ATTITUDE", 15.0f);
|
configure_stream_local("ATTITUDE", 15.0f);
|
||||||
configure_stream_local("ATTITUDE_QUATERNION", 10.0f);
|
configure_stream_local("ATTITUDE_QUATERNION", 10.0f);
|
||||||
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
||||||
|
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||||
configure_stream_local("COLLISION", unlimited_rate);
|
configure_stream_local("COLLISION", unlimited_rate);
|
||||||
|
configure_stream_local("CURRENT_MODE", 0.5f);
|
||||||
configure_stream_local("DISTANCE_SENSOR", 0.5f);
|
configure_stream_local("DISTANCE_SENSOR", 0.5f);
|
||||||
configure_stream_local("EFI_STATUS", 2.0f);
|
configure_stream_local("EFI_STATUS", 2.0f);
|
||||||
configure_stream_local("ESC_INFO", 1.0f);
|
configure_stream_local("ESC_INFO", 1.0f);
|
||||||
|
@ -1469,9 +1472,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||||
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
||||||
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
|
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
|
||||||
configure_stream_local("ATTITUDE_TARGET", 10.0f);
|
configure_stream_local("ATTITUDE_TARGET", 10.0f);
|
||||||
|
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||||
configure_stream_local("COLLISION", unlimited_rate);
|
configure_stream_local("COLLISION", unlimited_rate);
|
||||||
|
configure_stream_local("CURRENT_MODE", 0.5f);
|
||||||
configure_stream_local("EFI_STATUS", 2.0f);
|
configure_stream_local("EFI_STATUS", 2.0f);
|
||||||
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
||||||
configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
|
configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
|
||||||
|
@ -1543,9 +1548,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||||
|
|
||||||
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
||||||
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
||||||
|
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||||
configure_stream_local("COLLISION", unlimited_rate);
|
configure_stream_local("COLLISION", unlimited_rate);
|
||||||
|
configure_stream_local("CURRENT_MODE", 0.5f);
|
||||||
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
||||||
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
|
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
|
||||||
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
|
configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
|
||||||
|
@ -1625,9 +1632,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||||
configure_stream_local("ATTITUDE", 50.0f);
|
configure_stream_local("ATTITUDE", 50.0f);
|
||||||
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
|
configure_stream_local("ATTITUDE_QUATERNION", 50.0f);
|
||||||
configure_stream_local("ATTITUDE_TARGET", 8.0f);
|
configure_stream_local("ATTITUDE_TARGET", 8.0f);
|
||||||
|
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
|
||||||
configure_stream_local("COLLISION", unlimited_rate);
|
configure_stream_local("COLLISION", unlimited_rate);
|
||||||
|
configure_stream_local("CURRENT_MODE", 0.5f);
|
||||||
configure_stream_local("EFI_STATUS", 10.0f);
|
configure_stream_local("EFI_STATUS", 10.0f);
|
||||||
configure_stream_local("ESC_INFO", 10.0f);
|
configure_stream_local("ESC_INFO", 10.0f);
|
||||||
configure_stream_local("ESC_STATUS", 10.0f);
|
configure_stream_local("ESC_STATUS", 10.0f);
|
||||||
|
@ -1720,8 +1729,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||||
|
|
||||||
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
configure_stream_local("ADSB_VEHICLE", unlimited_rate);
|
||||||
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
configure_stream_local("ATTITUDE_TARGET", 2.0f);
|
||||||
|
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||||
configure_stream_local("COLLISION", unlimited_rate);
|
configure_stream_local("COLLISION", unlimited_rate);
|
||||||
|
configure_stream_local("CURRENT_MODE", 0.5f);
|
||||||
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
||||||
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
|
configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
|
||||||
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
|
configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
|
||||||
|
@ -2354,7 +2365,8 @@ Mavlink::task_main(int argc, char *argv[])
|
||||||
|
|
||||||
if (!command_ack.from_external
|
if (!command_ack.from_external
|
||||||
&& command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
|
&& command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
|
||||||
&& is_target_known) {
|
&& is_target_known
|
||||||
|
&& command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
|
||||||
|
|
||||||
mavlink_command_ack_t msg{};
|
mavlink_command_ack_t msg{};
|
||||||
msg.result = command_ack.result;
|
msg.result = command_ack.result;
|
||||||
|
|
|
@ -124,6 +124,11 @@
|
||||||
#include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
|
#include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
|
||||||
#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
|
#endif // MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS
|
||||||
|
|
||||||
|
#ifdef MAVLINK_MSG_ID_AVAILABLE_MODES // Only defined if development.xml is used
|
||||||
|
#include "streams/AVAILABLE_MODES.hpp"
|
||||||
|
#include "streams/CURRENT_MODE.hpp"
|
||||||
|
#endif
|
||||||
|
|
||||||
#if !defined(CONSTRAINED_FLASH)
|
#if !defined(CONSTRAINED_FLASH)
|
||||||
# include "streams/ADSB_VEHICLE.hpp"
|
# include "streams/ADSB_VEHICLE.hpp"
|
||||||
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
|
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
|
||||||
|
@ -492,8 +497,14 @@ static const StreamListItem streams_list[] = {
|
||||||
create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
|
create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
|
||||||
#endif // UAVIONIX_ADSB_OUT_CFG_HPP
|
#endif // UAVIONIX_ADSB_OUT_CFG_HPP
|
||||||
#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
|
#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
|
||||||
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
|
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
|
||||||
#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
|
#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
|
||||||
|
#if defined(AVAILABLE_MODES_HPP)
|
||||||
|
create_stream_list_item<MavlinkStreamAvailableModes>(),
|
||||||
|
#endif // AVAILABLE_MODES_HPP
|
||||||
|
#if defined(CURRENT_MODE_HPP)
|
||||||
|
create_stream_list_item<MavlinkStreamCurrentMode>(),
|
||||||
|
#endif // CURRENT_MODE_HPP
|
||||||
};
|
};
|
||||||
|
|
||||||
const char *get_stream_name(const uint16_t msg_id)
|
const char *get_stream_name(const uint16_t msg_id)
|
||||||
|
|
|
@ -75,6 +75,4 @@ MavlinkStream *create_mavlink_stream(const char *stream_name, Mavlink *mavlink);
|
||||||
|
|
||||||
MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink);
|
MavlinkStream *create_mavlink_stream(const uint16_t msg_id, Mavlink *mavlink);
|
||||||
|
|
||||||
union px4_custom_mode get_px4_custom_mode(uint8_t nav_state);
|
|
||||||
|
|
||||||
#endif /* MAVLINK_MESSAGES_H_ */
|
#endif /* MAVLINK_MESSAGES_H_ */
|
||||||
|
|
|
@ -0,0 +1,239 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef AVAILABLE_MODES_HPP
|
||||||
|
#define AVAILABLE_MODES_HPP
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <uORB/topics/register_ext_component_reply.h>
|
||||||
|
#include <lib/modes/standard_modes.hpp>
|
||||||
|
#include <lib/modes/ui.hpp>
|
||||||
|
|
||||||
|
class MavlinkStreamAvailableModes : public MavlinkStream
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamAvailableModes(mavlink); }
|
||||||
|
|
||||||
|
~MavlinkStreamAvailableModes() { delete[] _external_mode_names; }
|
||||||
|
|
||||||
|
static constexpr const char *get_name_static() { return "AVAILABLE_MODES"; }
|
||||||
|
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_AVAILABLE_MODES; }
|
||||||
|
|
||||||
|
const char *get_name() const override { return get_name_static(); }
|
||||||
|
uint16_t get_id() override { return get_id_static(); }
|
||||||
|
|
||||||
|
unsigned get_size() override
|
||||||
|
{
|
||||||
|
return _had_dynamic_update ? MAVLINK_MSG_ID_AVAILABLE_MODES_MONITOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
static constexpr int MAX_NUM_EXTERNAL_MODES = vehicle_status_s::NAVIGATION_STATE_EXTERNAL8 -
|
||||||
|
vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 + 1;
|
||||||
|
|
||||||
|
explicit MavlinkStreamAvailableModes(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||||
|
|
||||||
|
struct ExternalModeName {
|
||||||
|
char name[sizeof(register_ext_component_reply_s::name)] {};
|
||||||
|
};
|
||||||
|
ExternalModeName *_external_mode_names{nullptr};
|
||||||
|
|
||||||
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
uORB::Subscription _register_ext_component_reply_sub{ORB_ID(register_ext_component_reply)};
|
||||||
|
|
||||||
|
bool _had_dynamic_update{false};
|
||||||
|
uint8_t _dynamic_update_seq{0};
|
||||||
|
uint32_t _last_valid_nav_states_mask{0};
|
||||||
|
uint32_t _last_can_set_nav_states_mask{0};
|
||||||
|
|
||||||
|
void send_single_mode(const vehicle_status_s &vehicle_status, int mode_index, int total_num_modes, uint8_t nav_state)
|
||||||
|
{
|
||||||
|
mavlink_available_modes_t available_modes{};
|
||||||
|
available_modes.mode_index = mode_index;
|
||||||
|
available_modes.number_modes = total_num_modes;
|
||||||
|
px4_custom_mode custom_mode{get_px4_custom_mode(nav_state)};
|
||||||
|
available_modes.custom_mode = custom_mode.data;
|
||||||
|
const bool cannot_be_selected = (vehicle_status.can_set_nav_states_mask & (1u << nav_state)) == 0;
|
||||||
|
|
||||||
|
// Set the mode name if not a standard mode
|
||||||
|
available_modes.standard_mode = (uint8_t)mode_util::getStandardModeFromNavState(nav_state);
|
||||||
|
|
||||||
|
if (mode_util::isAdvanced(nav_state)) {
|
||||||
|
available_modes.properties |= MAV_MODE_PROPERTY_ADVANCED;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (available_modes.standard_mode == MAV_STANDARD_MODE_NON_STANDARD) {
|
||||||
|
static_assert(sizeof(available_modes.mode_name) >= sizeof(ExternalModeName::name), "mode name too short");
|
||||||
|
|
||||||
|
// Is it an external mode?
|
||||||
|
unsigned external_mode_index = nav_state - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
|
||||||
|
|
||||||
|
if (nav_state >= vehicle_status_s::NAVIGATION_STATE_EXTERNAL1 && external_mode_index < MAX_NUM_EXTERNAL_MODES) {
|
||||||
|
if (cannot_be_selected) {
|
||||||
|
// If not selectable, it's not registered
|
||||||
|
strcpy(available_modes.mode_name, "(Mode not available)");
|
||||||
|
|
||||||
|
} else if (_external_mode_names) {
|
||||||
|
strncpy(available_modes.mode_name, _external_mode_names[external_mode_index].name, sizeof(available_modes.mode_name));
|
||||||
|
available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0';
|
||||||
|
}
|
||||||
|
|
||||||
|
} else { // Internal
|
||||||
|
if (nav_state < sizeof(mode_util::nav_state_names) / sizeof(mode_util::nav_state_names[0])) {
|
||||||
|
strncpy(available_modes.mode_name, mode_util::nav_state_names[nav_state], sizeof(available_modes.mode_name));
|
||||||
|
available_modes.mode_name[sizeof(available_modes.mode_name) - 1] = '\0';
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (cannot_be_selected) {
|
||||||
|
available_modes.properties |= MAV_MODE_PROPERTY_NOT_USER_SELECTABLE;
|
||||||
|
}
|
||||||
|
|
||||||
|
mavlink_msg_available_modes_send_struct(_mavlink->get_channel(), &available_modes);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool request_message(float param2, float param3, float param4,
|
||||||
|
float param5, float param6, float param7) override
|
||||||
|
{
|
||||||
|
bool ret = false;
|
||||||
|
int mode_index = roundf(param2);
|
||||||
|
PX4_DEBUG("AVAILABLE_MODES request (%i)", mode_index);
|
||||||
|
|
||||||
|
vehicle_status_s vehicle_status;
|
||||||
|
|
||||||
|
if (!_vehicle_status_sub.copy(&vehicle_status)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
int total_num_modes = math::countSetBits(vehicle_status.valid_nav_states_mask);
|
||||||
|
|
||||||
|
if (mode_index == 0) { // All
|
||||||
|
int cur_mode_index = 1;
|
||||||
|
|
||||||
|
for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) {
|
||||||
|
if ((1u << nav_state) & vehicle_status.valid_nav_states_mask) {
|
||||||
|
send_single_mode(vehicle_status, cur_mode_index, total_num_modes, nav_state);
|
||||||
|
++cur_mode_index;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = true;
|
||||||
|
|
||||||
|
} else if (mode_index <= total_num_modes) {
|
||||||
|
// Find index
|
||||||
|
int cur_index = 0;
|
||||||
|
uint8_t nav_state = 0;
|
||||||
|
|
||||||
|
for (; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) {
|
||||||
|
if ((1u << nav_state) & vehicle_status.valid_nav_states_mask) {
|
||||||
|
if (++cur_index == mode_index) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (nav_state < vehicle_status_s::NAVIGATION_STATE_MAX) {
|
||||||
|
send_single_mode(vehicle_status, mode_index, total_num_modes, nav_state);
|
||||||
|
}
|
||||||
|
|
||||||
|
ret = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
void update_data() override
|
||||||
|
{
|
||||||
|
// Keep track of externally registered modes
|
||||||
|
register_ext_component_reply_s reply;
|
||||||
|
bool dynamic_update = false;
|
||||||
|
|
||||||
|
if (_register_ext_component_reply_sub.update(&reply)) {
|
||||||
|
if (reply.success && reply.mode_id != -1) {
|
||||||
|
if (!_external_mode_names) {
|
||||||
|
_external_mode_names = new ExternalModeName[MAX_NUM_EXTERNAL_MODES];
|
||||||
|
}
|
||||||
|
|
||||||
|
unsigned mode_index = reply.mode_id - vehicle_status_s::NAVIGATION_STATE_EXTERNAL1;
|
||||||
|
|
||||||
|
if (_external_mode_names && mode_index < MAX_NUM_EXTERNAL_MODES) {
|
||||||
|
memcpy(_external_mode_names[mode_index].name, reply.name, sizeof(ExternalModeName::name));
|
||||||
|
}
|
||||||
|
|
||||||
|
dynamic_update = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
vehicle_status_s vehicle_status;
|
||||||
|
|
||||||
|
if (_vehicle_status_sub.copy(&vehicle_status)) {
|
||||||
|
if (_last_valid_nav_states_mask == 0) {
|
||||||
|
_last_valid_nav_states_mask = vehicle_status.valid_nav_states_mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (_last_can_set_nav_states_mask == 0) {
|
||||||
|
_last_can_set_nav_states_mask = vehicle_status.can_set_nav_states_mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (vehicle_status.valid_nav_states_mask != _last_valid_nav_states_mask) {
|
||||||
|
dynamic_update = true;
|
||||||
|
_last_valid_nav_states_mask = vehicle_status.valid_nav_states_mask;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (vehicle_status.can_set_nav_states_mask != _last_can_set_nav_states_mask) {
|
||||||
|
dynamic_update = true;
|
||||||
|
_last_can_set_nav_states_mask = vehicle_status.can_set_nav_states_mask;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (dynamic_update) {
|
||||||
|
_had_dynamic_update = true;
|
||||||
|
++_dynamic_update_seq;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool send() override
|
||||||
|
{
|
||||||
|
if (_had_dynamic_update) {
|
||||||
|
mavlink_available_modes_monitor_t monitor{};
|
||||||
|
monitor.seq = _dynamic_update_seq;
|
||||||
|
mavlink_msg_available_modes_monitor_send_struct(_mavlink->get_channel(), &monitor);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // AVAILABLE_MODES_HPP
|
|
@ -0,0 +1,78 @@
|
||||||
|
/****************************************************************************
|
||||||
|
*
|
||||||
|
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions
|
||||||
|
* are met:
|
||||||
|
*
|
||||||
|
* 1. Redistributions of source code must retain the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer.
|
||||||
|
* 2. Redistributions in binary form must reproduce the above copyright
|
||||||
|
* notice, this list of conditions and the following disclaimer in
|
||||||
|
* the documentation and/or other materials provided with the
|
||||||
|
* distribution.
|
||||||
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||||
|
* used to endorse or promote products derived from this software
|
||||||
|
* without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||||
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||||
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*
|
||||||
|
****************************************************************************/
|
||||||
|
|
||||||
|
#ifndef CURRENT_MODE_HPP
|
||||||
|
#define CURRENT_MODE_HPP
|
||||||
|
|
||||||
|
#include <uORB/topics/vehicle_status.h>
|
||||||
|
#include <lib/modes/standard_modes.hpp>
|
||||||
|
|
||||||
|
class MavlinkStreamCurrentMode : public MavlinkStream
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCurrentMode(mavlink); }
|
||||||
|
|
||||||
|
static constexpr const char *get_name_static() { return "CURRENT_MODE"; }
|
||||||
|
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CURRENT_MODE; }
|
||||||
|
|
||||||
|
const char *get_name() const override { return get_name_static(); }
|
||||||
|
uint16_t get_id() override { return get_id_static(); }
|
||||||
|
|
||||||
|
unsigned get_size() override
|
||||||
|
{
|
||||||
|
return MAVLINK_MSG_ID_CURRENT_MODE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
explicit MavlinkStreamCurrentMode(Mavlink *mavlink) : MavlinkStream(mavlink) {}
|
||||||
|
|
||||||
|
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||||
|
|
||||||
|
bool send() override
|
||||||
|
{
|
||||||
|
vehicle_status_s vehicle_status;
|
||||||
|
|
||||||
|
if (_vehicle_status_sub.update(&vehicle_status)) {
|
||||||
|
mavlink_current_mode_t current_mode{};
|
||||||
|
current_mode.custom_mode = get_px4_custom_mode(vehicle_status.nav_state).data;
|
||||||
|
current_mode.intended_custom_mode = get_px4_custom_mode(vehicle_status.nav_state_user_intention).data;
|
||||||
|
current_mode.standard_mode = (uint8_t) mode_util::getStandardModeFromNavState(vehicle_status.nav_state);
|
||||||
|
mavlink_msg_current_mode_send_struct(_mavlink->get_channel(), ¤t_mode);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // CURRENT_MODE_HPP
|
|
@ -42,6 +42,8 @@
|
||||||
|
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
#include <string>
|
#include <string>
|
||||||
|
#include <cstdlib>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
GZBridge::GZBridge(const char *world, const char *name, const char *model,
|
GZBridge::GZBridge(const char *world, const char *name, const char *model,
|
||||||
const char *pose_str) :
|
const char *pose_str) :
|
||||||
|
@ -72,7 +74,18 @@ int GZBridge::init()
|
||||||
|
|
||||||
// service call to create model
|
// service call to create model
|
||||||
gz::msgs::EntityFactory req{};
|
gz::msgs::EntityFactory req{};
|
||||||
req.set_sdf_filename(_model_sim + "/model.sdf");
|
std::string filename = "../../../Tools/simulation/gz/models/" + _model_sim + "/model.sdf";
|
||||||
|
|
||||||
|
std::ifstream file(filename);
|
||||||
|
std::string fileContent;
|
||||||
|
|
||||||
|
if (file.is_open()) {
|
||||||
|
// Read the file content into a string
|
||||||
|
fileContent = std::string((std::istreambuf_iterator<char>(file)), (std::istreambuf_iterator<char>()));
|
||||||
|
file.close();
|
||||||
|
}
|
||||||
|
|
||||||
|
req.set_sdf(fileContent);
|
||||||
|
|
||||||
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
|
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
|
||||||
|
|
||||||
|
@ -116,6 +129,34 @@ int GZBridge::init()
|
||||||
bool result;
|
bool result;
|
||||||
std::string create_service = "/world/" + _world_name + "/create";
|
std::string create_service = "/world/" + _world_name + "/create";
|
||||||
|
|
||||||
|
bool gz_called = false;
|
||||||
|
// Check if STANDALONE has been set.
|
||||||
|
char *standalone_val = std::getenv("STANDALONE");
|
||||||
|
|
||||||
|
if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) {
|
||||||
|
// Check if Gazebo has been called and if not attempt to reconnect.
|
||||||
|
while (gz_called == false) {
|
||||||
|
if (_node.Request(create_service, req, 1000, rep, result)) {
|
||||||
|
if (!rep.data() || !result) {
|
||||||
|
PX4_ERR("EntityFactory service call failed");
|
||||||
|
return PX4_ERROR;
|
||||||
|
|
||||||
|
} else {
|
||||||
|
gz_called = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If Gazebo has not been called, wait 2 seconds and try again.
|
||||||
|
else {
|
||||||
|
printf("WARN [gz_bridge] Service call timed out as Gazebo has not been detected. \n");
|
||||||
|
system_usleep(2000000);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// If STANDALONE has not been set, do not retry to reconnect.
|
||||||
|
else {
|
||||||
if (_node.Request(create_service, req, 1000, rep, result)) {
|
if (_node.Request(create_service, req, 1000, rep, result)) {
|
||||||
if (!rep.data() || !result) {
|
if (!rep.data() || !result) {
|
||||||
PX4_ERR("EntityFactory service call failed");
|
PX4_ERR("EntityFactory service call failed");
|
||||||
|
@ -127,6 +168,7 @@ int GZBridge::init()
|
||||||
return PX4_ERROR;
|
return PX4_ERROR;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// clock
|
// clock
|
||||||
std::string clock_topic = "/world/" + _world_name + "/clock";
|
std::string clock_topic = "/world/" + _world_name + "/clock";
|
||||||
|
|
|
@ -5,12 +5,27 @@
|
||||||
#####
|
#####
|
||||||
publications:
|
publications:
|
||||||
|
|
||||||
|
- topic: /fmu/out/register_ext_component_reply
|
||||||
|
type: px4_msgs::msg::RegisterExtComponentReply
|
||||||
|
|
||||||
|
- topic: /fmu/out/arming_check_request
|
||||||
|
type: px4_msgs::msg::ArmingCheckRequest
|
||||||
|
|
||||||
|
- topic: /fmu/out/mode_completed
|
||||||
|
type: px4_msgs::msg::ModeCompleted
|
||||||
|
|
||||||
- topic: /fmu/out/collision_constraints
|
- topic: /fmu/out/collision_constraints
|
||||||
type: px4_msgs::msg::CollisionConstraints
|
type: px4_msgs::msg::CollisionConstraints
|
||||||
|
|
||||||
- topic: /fmu/out/failsafe_flags
|
- topic: /fmu/out/failsafe_flags
|
||||||
type: px4_msgs::msg::FailsafeFlags
|
type: px4_msgs::msg::FailsafeFlags
|
||||||
|
|
||||||
|
- topic: /fmu/out/manual_control_setpoint
|
||||||
|
type: px4_msgs::msg::ManualControlSetpoint
|
||||||
|
|
||||||
|
- topic: /fmu/out/message_format_response
|
||||||
|
type: px4_msgs::msg::MessageFormatResponse
|
||||||
|
|
||||||
- topic: /fmu/out/position_setpoint_triplet
|
- topic: /fmu/out/position_setpoint_triplet
|
||||||
type: px4_msgs::msg::PositionSetpointTriplet
|
type: px4_msgs::msg::PositionSetpointTriplet
|
||||||
|
|
||||||
|
@ -29,6 +44,9 @@ publications:
|
||||||
- topic: /fmu/out/vehicle_control_mode
|
- topic: /fmu/out/vehicle_control_mode
|
||||||
type: px4_msgs::msg::VehicleControlMode
|
type: px4_msgs::msg::VehicleControlMode
|
||||||
|
|
||||||
|
- topic: /fmu/out/vehicle_command_ack
|
||||||
|
type: px4_msgs::msg::VehicleCommandAck
|
||||||
|
|
||||||
- topic: /fmu/out/vehicle_global_position
|
- topic: /fmu/out/vehicle_global_position
|
||||||
type: px4_msgs::msg::VehicleGlobalPosition
|
type: px4_msgs::msg::VehicleGlobalPosition
|
||||||
|
|
||||||
|
@ -48,6 +66,26 @@ publications:
|
||||||
type: px4_msgs::msg::VehicleTrajectoryWaypoint
|
type: px4_msgs::msg::VehicleTrajectoryWaypoint
|
||||||
|
|
||||||
subscriptions:
|
subscriptions:
|
||||||
|
- topic: /fmu/in/register_ext_component_request
|
||||||
|
type: px4_msgs::msg::RegisterExtComponentRequest
|
||||||
|
|
||||||
|
- topic: /fmu/in/unregister_ext_component
|
||||||
|
type: px4_msgs::msg::UnregisterExtComponent
|
||||||
|
|
||||||
|
- topic: /fmu/in/config_overrides_request
|
||||||
|
type: px4_msgs::msg::ConfigOverrides
|
||||||
|
|
||||||
|
- topic: /fmu/in/arming_check_reply
|
||||||
|
type: px4_msgs::msg::ArmingCheckReply
|
||||||
|
|
||||||
|
- topic: /fmu/in/message_format_request
|
||||||
|
type: px4_msgs::msg::MessageFormatRequest
|
||||||
|
|
||||||
|
- topic: /fmu/in/mode_completed
|
||||||
|
type: px4_msgs::msg::ModeCompleted
|
||||||
|
|
||||||
|
- topic: /fmu/in/config_control_setpoints
|
||||||
|
type: px4_msgs::msg::VehicleControlMode
|
||||||
|
|
||||||
- topic: /fmu/in/offboard_control_mode
|
- topic: /fmu/in/offboard_control_mode
|
||||||
type: px4_msgs::msg::OffboardControlMode
|
type: px4_msgs::msg::OffboardControlMode
|
||||||
|
@ -82,6 +120,9 @@ subscriptions:
|
||||||
- topic: /fmu/in/vehicle_command
|
- topic: /fmu/in/vehicle_command
|
||||||
type: px4_msgs::msg::VehicleCommand
|
type: px4_msgs::msg::VehicleCommand
|
||||||
|
|
||||||
|
- topic: /fmu/in/vehicle_command_mode_executor
|
||||||
|
type: px4_msgs::msg::VehicleCommand
|
||||||
|
|
||||||
- topic: /fmu/in/vehicle_trajectory_bezier
|
- topic: /fmu/in/vehicle_trajectory_bezier
|
||||||
type: px4_msgs::msg::VehicleTrajectoryBezier
|
type: px4_msgs::msg::VehicleTrajectoryBezier
|
||||||
|
|
||||||
|
|
|
@ -164,6 +164,67 @@ UxrceddsClient::~UxrceddsClient()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void fillMessageFormatResponse(const message_format_request_s &message_format_request,
|
||||||
|
message_format_response_s &message_format_response)
|
||||||
|
{
|
||||||
|
message_format_response.protocol_version = message_format_request_s::LATEST_PROTOCOL_VERSION;
|
||||||
|
message_format_response.success = false;
|
||||||
|
|
||||||
|
if (message_format_request.protocol_version == message_format_request_s::LATEST_PROTOCOL_VERSION) {
|
||||||
|
static_assert(sizeof(message_format_request.topic_name) == sizeof(message_format_response.topic_name), "size mismatch");
|
||||||
|
memcpy(message_format_response.topic_name, message_format_request.topic_name,
|
||||||
|
sizeof(message_format_response.topic_name));
|
||||||
|
|
||||||
|
// Get the topic name by searching for the last '/'
|
||||||
|
int idx_last_slash = -1;
|
||||||
|
bool found_null = false;
|
||||||
|
|
||||||
|
for (int i = 0; i < (int)sizeof(message_format_request.topic_name); ++i) {
|
||||||
|
if (message_format_request.topic_name[i] == 0) {
|
||||||
|
found_null = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (message_format_request.topic_name[i] == '/') {
|
||||||
|
idx_last_slash = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (found_null && idx_last_slash != -1) {
|
||||||
|
const char *topic_name = message_format_request.topic_name + idx_last_slash + 1;
|
||||||
|
// Find the format
|
||||||
|
const orb_metadata *const *topics = orb_get_topics();
|
||||||
|
const orb_metadata *topic_meta{nullptr};
|
||||||
|
|
||||||
|
for (size_t i = 0; i < orb_topics_count(); i++) {
|
||||||
|
if (strcmp(topic_name, topics[i]->o_name) == 0) {
|
||||||
|
topic_meta = topics[i];
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (topic_meta) {
|
||||||
|
message_format_response.message_hash = topic_meta->message_hash;
|
||||||
|
// The topic type is already checked by DDS
|
||||||
|
message_format_response.success = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
message_format_response.timestamp = hrt_absolute_time();
|
||||||
|
}
|
||||||
|
|
||||||
|
void UxrceddsClient::handleMessageFormatRequest()
|
||||||
|
{
|
||||||
|
message_format_request_s message_format_request;
|
||||||
|
|
||||||
|
if (_message_format_request_sub.update(&message_format_request)) {
|
||||||
|
message_format_response_s message_format_response;
|
||||||
|
fillMessageFormatResponse(message_format_request, message_format_response);
|
||||||
|
_message_format_response_pub.publish(message_format_response);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
void UxrceddsClient::run()
|
void UxrceddsClient::run()
|
||||||
{
|
{
|
||||||
if (!_comm) {
|
if (!_comm) {
|
||||||
|
@ -359,8 +420,18 @@ void UxrceddsClient::run()
|
||||||
|
|
||||||
_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
|
_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
|
||||||
|
|
||||||
|
// Run the session until we receive no more data or up to a maximum number of iterations.
|
||||||
|
// The maximum observed number of iterations was 6 (SITL). If we were to run only once, data starts to get
|
||||||
|
// delayed, causing registered flight modes to time out.
|
||||||
|
for (int i = 0; i < 10; ++i) {
|
||||||
|
const uint32_t prev_num_payload_received = _pubs->num_payload_received;
|
||||||
uxr_run_session_timeout(&session, 0);
|
uxr_run_session_timeout(&session, 0);
|
||||||
|
|
||||||
|
if (_pubs->num_payload_received == prev_num_payload_received) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// time sync session
|
// time sync session
|
||||||
if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
|
if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
|
||||||
if (uxr_sync_session(&session, 100)) {
|
if (uxr_sync_session(&session, 100)) {
|
||||||
|
@ -369,6 +440,8 @@ void UxrceddsClient::run()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
handleMessageFormatRequest();
|
||||||
|
|
||||||
// Check for a ping response
|
// Check for a ping response
|
||||||
/* PONG_IN_SESSION_STATUS */
|
/* PONG_IN_SESSION_STATUS */
|
||||||
if (session.on_pong_flag == 1) {
|
if (session.on_pong_flag == 1) {
|
||||||
|
|
|
@ -38,6 +38,10 @@
|
||||||
|
|
||||||
#include <src/modules/uxrce_dds_client/dds_topics.h>
|
#include <src/modules/uxrce_dds_client/dds_topics.h>
|
||||||
|
|
||||||
|
#include <uORB/topics/message_format_request.h>
|
||||||
|
#include <uORB/topics/message_format_response.h>
|
||||||
|
#include <uORB/Subscription.hpp>
|
||||||
|
|
||||||
#include <lib/timesync/Timesync.hpp>
|
#include <lib/timesync/Timesync.hpp>
|
||||||
|
|
||||||
class UxrceddsClient : public ModuleBase<UxrceddsClient>, public ModuleParams
|
class UxrceddsClient : public ModuleBase<UxrceddsClient>, public ModuleParams
|
||||||
|
@ -74,6 +78,11 @@ public:
|
||||||
private:
|
private:
|
||||||
int setBaudrate(int fd, unsigned baud);
|
int setBaudrate(int fd, unsigned baud);
|
||||||
|
|
||||||
|
void handleMessageFormatRequest();
|
||||||
|
|
||||||
|
uORB::Publication<message_format_response_s> _message_format_response_pub{ORB_ID(message_format_response)};
|
||||||
|
uORB::Subscription _message_format_request_sub{ORB_ID(message_format_request)};
|
||||||
|
|
||||||
const bool _localhost_only;
|
const bool _localhost_only;
|
||||||
const bool _custom_participant;
|
const bool _custom_participant;
|
||||||
const char *_client_namespace;
|
const char *_client_namespace;
|
||||||
|
|
Loading…
Reference in New Issue