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
|
||||
|
||||
|
||||
# set local coordinate frame reference
|
||||
if [ -n "${PX4_HOME_LAT}" ]; then
|
||||
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
|
||||
|
@ -59,36 +60,40 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
|
|||
. ../gz_env.sh
|
||||
fi
|
||||
|
||||
# "gz sim" only avaiilable in Garden and later
|
||||
GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
|
||||
if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
|
||||
then
|
||||
# "gz sim" from Garden on
|
||||
gz_command="gz"
|
||||
gz_sub_command="sim"
|
||||
else
|
||||
echo "ERROR [init] Gazebo gz please install gz-garden"
|
||||
exit 1
|
||||
fi
|
||||
# Only start up Gazebo if STANDALONE set to false
|
||||
if [ "$STANDALONE" != '1' ]; then
|
||||
|
||||
# look for running ${gz_command} gazebo world
|
||||
gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
|
||||
|
||||
# shellcheck disable=SC2153
|
||||
if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
|
||||
|
||||
echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
|
||||
|
||||
${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
|
||||
|
||||
if [ -z "${HEADLESS}" ]; then
|
||||
# HEADLESS not set, starting gui
|
||||
${gz_command} ${gz_sub_command} -g &
|
||||
# "gz sim" only avaiilable in Garden and later
|
||||
GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
|
||||
if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
|
||||
then
|
||||
# "gz sim" from Garden on
|
||||
gz_command="gz"
|
||||
gz_sub_command="sim"
|
||||
else
|
||||
echo "ERROR [init] Gazebo gz please install gz-garden"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
else
|
||||
echo "INFO [init] gazebo already running world: ${gz_world}"
|
||||
PX4_GZ_WORLD=${gz_world}
|
||||
# look for running ${gz_command} gazebo world
|
||||
gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
|
||||
|
||||
# shellcheck disable=SC2153
|
||||
if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
|
||||
|
||||
echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
|
||||
|
||||
${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
|
||||
|
||||
if [ -z "${HEADLESS}" ]; then
|
||||
# HEADLESS not set, starting gui
|
||||
${gz_command} ${gz_sub_command} -g &
|
||||
fi
|
||||
|
||||
else
|
||||
echo "INFO [init] gazebo already running world: ${gz_world}"
|
||||
PX4_GZ_WORLD=${gz_world}
|
||||
fi
|
||||
fi
|
||||
|
||||
# start gz_bridge
|
||||
|
|
|
@ -171,6 +171,61 @@ def get_children_fields(base_type, search_path):
|
|||
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):
|
||||
"""
|
||||
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
|
||||
|
||||
message_hash = get_message_hash(spec.parsed_fields(), search_path)
|
||||
sorted_fields = sorted(spec.parsed_fields(), key=sizeof_field_type, reverse=True)
|
||||
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]@
|
||||
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]
|
||||
|
||||
void print_message(const orb_metadata *meta, const @uorb_struct& message)
|
||||
|
|
|
@ -250,6 +250,22 @@ class SourceParser(object):
|
|||
event.group = "arming_check"
|
||||
event.prepend_arguments([('navigation_mode_group_t', 'modes'),
|
||||
('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:
|
||||
raise Exception("unknown event method call: {}, args: {}".format(call, args))
|
||||
|
||||
|
|
|
@ -69,7 +69,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -184,7 +184,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -229,7 +229,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -385,7 +385,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -412,7 +412,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -439,7 +439,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -466,7 +466,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -493,7 +493,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
|
@ -43,7 +43,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -183,7 +183,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -246,7 +246,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -309,7 +309,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -372,7 +372,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -436,7 +436,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -483,7 +483,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -510,7 +510,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
|
|
@ -23,7 +23,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -32,7 +32,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -41,7 +41,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -50,7 +50,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -59,7 +59,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -77,7 +77,7 @@
|
|||
<specular>1.0 1.0 1.0</specular>
|
||||
<pbr>
|
||||
<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>
|
||||
</pbr>
|
||||
</material>
|
||||
|
@ -96,7 +96,7 @@
|
|||
<specular>1.0 1.0 1.0</specular>
|
||||
<pbr>
|
||||
<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>
|
||||
</pbr>
|
||||
</material>
|
||||
|
@ -115,7 +115,7 @@
|
|||
<specular>1.0 1.0 1.0</specular>
|
||||
<pbr>
|
||||
<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>
|
||||
</pbr>
|
||||
</material>
|
||||
|
@ -250,7 +250,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -265,7 +265,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -322,7 +322,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -337,7 +337,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -394,7 +394,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -409,7 +409,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
@ -466,7 +466,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
<material>
|
||||
|
@ -481,7 +481,7 @@
|
|||
<geometry>
|
||||
<mesh>
|
||||
<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>
|
||||
</geometry>
|
||||
</visual>
|
||||
|
|
|
@ -111,3 +111,4 @@ CONFIG_SYSTEMCMDS_UORB=y
|
|||
CONFIG_SYSTEMCMDS_USB_CONNECTED=y
|
||||
CONFIG_SYSTEMCMDS_VER=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
|
||||
AirspeedValidated.msg
|
||||
AirspeedWind.msg
|
||||
ArmingCheckReply.msg
|
||||
ArmingCheckRequest.msg
|
||||
AutotuneAttitudeControlStatus.msg
|
||||
BatteryStatus.msg
|
||||
ButtonEvent.msg
|
||||
|
@ -58,6 +60,7 @@ set(msg_files
|
|||
CellularStatus.msg
|
||||
CollisionConstraints.msg
|
||||
CollisionReport.msg
|
||||
ConfigOverrides.msg
|
||||
ControlAllocatorStatus.msg
|
||||
Cpuload.msg
|
||||
DatamanRequest.msg
|
||||
|
@ -130,6 +133,8 @@ set(msg_files
|
|||
ManualControlSwitches.msg
|
||||
MavlinkLog.msg
|
||||
MavlinkTunnel.msg
|
||||
MessageFormatRequest.msg
|
||||
MessageFormatResponse.msg
|
||||
Mission.msg
|
||||
MissionResult.msg
|
||||
MountOrientation.msg
|
||||
|
@ -161,6 +166,8 @@ set(msg_files
|
|||
RateCtrlStatus.msg
|
||||
RcChannels.msg
|
||||
RcParameterMap.msg
|
||||
RegisterExtComponentReply.msg
|
||||
RegisterExtComponentRequest.msg
|
||||
Rpm.msg
|
||||
RtlTimeEstimate.msg
|
||||
SatelliteInfo.msg
|
||||
|
@ -198,6 +205,7 @@ set(msg_files
|
|||
UavcanParameterValue.msg
|
||||
UlogStream.msg
|
||||
UlogStreamAck.msg
|
||||
UnregisterExtComponent.msg
|
||||
VehicleAcceleration.msg
|
||||
VehicleAirData.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_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_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_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)
|
||||
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_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_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_component # Component which should execute the command, 0 for all components
|
||||
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)
|
||||
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
|
||||
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_component
|
||||
uint16 target_component # Target component / mode executor
|
||||
|
||||
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_termination_enabled # true if flighttermination 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_ORBIT = 21 # Orbit in a circle
|
||||
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
|
||||
uint16 failure_detector_status
|
||||
|
@ -78,8 +91,13 @@ uint8 VEHICLE_TYPE_FIXED_WING = 2
|
|||
uint8 VEHICLE_TYPE_ROVER = 3
|
||||
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_and_user_took_over # true if system is in failsafe state but the user took over control
|
||||
uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_*
|
||||
|
||||
# Link loss
|
||||
bool gcs_connection_lost # datalink to GCS lost
|
||||
|
|
|
@ -51,6 +51,7 @@ struct orb_metadata {
|
|||
const char *o_name; /**< unique object name */
|
||||
const uint16_t o_size; /**< object size */
|
||||
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 */
|
||||
};
|
||||
|
||||
|
@ -99,13 +100,15 @@ typedef const struct orb_metadata *orb_id_t;
|
|||
* @param _name The name of the topic.
|
||||
* @param _struct The structure the topic provides.
|
||||
* @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
|
||||
*/
|
||||
#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 = { \
|
||||
#_name, \
|
||||
sizeof(_struct), \
|
||||
_size_no_padding, \
|
||||
_message_hash, \
|
||||
_orb_id_enum \
|
||||
}; struct hack
|
||||
|
||||
|
|
|
@ -110,6 +110,38 @@
|
|||
"4194304": {
|
||||
"name": "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",
|
||||
"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": {
|
||||
"name": "unknown",
|
||||
"description": "[Unknown]"
|
||||
|
@ -705,7 +769,15 @@
|
|||
"19": [134479872],
|
||||
"20": [151257088],
|
||||
"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" ]
|
||||
|
|
|
@ -38,7 +38,7 @@
|
|||
#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
|
||||
{
|
||||
|
@ -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:
|
||||
static constexpr int max_num_actuators = 6;
|
||||
|
|
|
@ -57,7 +57,7 @@ static const FunctionProvider all_function_providers[] = {
|
|||
{OutputFunction::Constant_Max, &FunctionConstantMax::allocate},
|
||||
{OutputFunction::Motor1, OutputFunction::MotorMax, &FunctionMotors::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_Wheel, &FunctionLandingGearWheel::allocate},
|
||||
{OutputFunction::Parachute, &FunctionParachute::allocate},
|
||||
|
|
|
@ -13,7 +13,7 @@ functions:
|
|||
Servo:
|
||||
start: 201
|
||||
count: 8
|
||||
Offboard_Actuator_Set:
|
||||
Peripheral_via_Actuator_Set:
|
||||
start: 301
|
||||
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
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
@ -31,11 +31,12 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(Arming)
|
||||
add_subdirectory(failsafe)
|
||||
add_subdirectory(failure_detector)
|
||||
add_subdirectory(HealthAndArmingChecks)
|
||||
add_subdirectory(failsafe)
|
||||
add_subdirectory(Arming)
|
||||
add_subdirectory(ModeUtil)
|
||||
add_subdirectory(MulticopterThrowLaunch)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__commander
|
||||
|
@ -52,24 +53,30 @@ px4_add_module(
|
|||
factory_calibration_storage.cpp
|
||||
gyro_calibration.cpp
|
||||
HomePosition.cpp
|
||||
ModeManagement.cpp
|
||||
UserModeIntention.cpp
|
||||
level_calibration.cpp
|
||||
lm_fit.cpp
|
||||
mag_calibration.cpp
|
||||
rc_calibration.cpp
|
||||
Safety.cpp
|
||||
UserModeIntention.cpp
|
||||
worker_thread.cpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
ArmAuthorization
|
||||
circuit_breaker
|
||||
failsafe
|
||||
failure_detector
|
||||
geo
|
||||
health_and_arming_checks
|
||||
hysteresis
|
||||
ArmAuthorization
|
||||
mode_util
|
||||
MulticopterThrowLaunch
|
||||
sensor_calibration
|
||||
world_magnetic_model
|
||||
mode_util
|
||||
failsafe
|
||||
)
|
||||
|
||||
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 "ModeUtil/control_mode.hpp"
|
||||
#include "ModeUtil/conversions.hpp"
|
||||
#include <lib/modes/ui.hpp>
|
||||
#include <lib/modes/standard_modes.hpp>
|
||||
|
||||
/* PX4 headers */
|
||||
#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,
|
||||
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 {
|
||||
PX4_ERR("argument %s unsupported.", argv[1]);
|
||||
}
|
||||
|
@ -475,8 +481,9 @@ int Commander::print_status()
|
|||
{
|
||||
PX4_INFO("%s", isArmed() ? "Armed" : "Disarmed");
|
||||
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");
|
||||
_mode_management.printStatus();
|
||||
perf_print_counter(_loop_perf);
|
||||
perf_print_counter(_preflight_check_perf);
|
||||
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);
|
||||
|
||||
} 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;
|
||||
|
||||
} else {
|
||||
|
@ -807,6 +814,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
desired_nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
|
||||
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:
|
||||
main_ret = TRANSITION_DENIED;
|
||||
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 (_user_mode_intention.change(desired_nav_state)) {
|
||||
if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
|
||||
main_ret = TRANSITION_CHANGED;
|
||||
|
||||
} else {
|
||||
|
@ -869,6 +880,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
}
|
||||
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: {
|
||||
|
||||
// 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: {
|
||||
/* 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");
|
||||
events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
|
||||
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: {
|
||||
/* 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;
|
||||
|
||||
} else {
|
||||
|
@ -997,7 +1020,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
|
||||
|
||||
/* 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;
|
||||
|
||||
} else {
|
||||
|
@ -1011,7 +1034,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
break;
|
||||
|
||||
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");
|
||||
events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
|
||||
"Landing at current position");
|
||||
|
@ -1025,7 +1048,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
break;
|
||||
|
||||
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");
|
||||
events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
|
||||
"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)) {
|
||||
|
||||
// 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))) {
|
||||
|
||||
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) {
|
||||
// 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;
|
||||
|
||||
} else {
|
||||
|
@ -1097,7 +1120,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
|
||||
} else {
|
||||
// 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;
|
||||
|
||||
} else {
|
||||
|
@ -1382,6 +1405,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
|
|||
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:
|
||||
_health_and_arming_checks.update(true);
|
||||
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;
|
||||
}
|
||||
|
||||
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)
|
||||
{
|
||||
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:
|
||||
|
||||
if (!_user_mode_intention.change(action_request.mode, true)) {
|
||||
if (!_user_mode_intention.change(action_request.mode, ModeChangeSource::User, true)) {
|
||||
printRejectMode(action_request.mode);
|
||||
}
|
||||
|
||||
|
@ -1717,6 +1795,8 @@ void Commander::run()
|
|||
_status_changed = true;
|
||||
}
|
||||
|
||||
modeManagementUpdate();
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
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
|
||||
handleCommandsFromModeExecutors();
|
||||
|
||||
if (_vehicle_command_sub.updated()) {
|
||||
// got command
|
||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||
|
@ -1799,6 +1881,7 @@ void Commander::run()
|
|||
updateControlMode();
|
||||
|
||||
// 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_pub.publish(_vehicle_status);
|
||||
|
||||
|
@ -1880,7 +1963,8 @@ void Commander::checkForMissionUpdate()
|
|||
|
||||
if (isArmed() && !_vehicle_land_detected.landed
|
||||
&& (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
|
||||
|| _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)
|
||||
&& !_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_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 FailsafeBase::Action prev_failsafe_action = _failsafe.selectedAction();
|
||||
const uint8_t prev_failsafe_defer_state = _vehicle_status.failsafe_defer_state;
|
||||
|
||||
FailsafeBase::State state{};
|
||||
state.armed = isArmed();
|
||||
|
@ -2200,13 +2287,16 @@ bool Commander::handleModeIntentionAndFailsafe()
|
|||
|
||||
// Force intended mode if changed by the failsafe state machine
|
||||
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();
|
||||
}
|
||||
|
||||
// Handle failsafe action
|
||||
_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
|
||||
_vehicle_status.nav_state = FailsafeBase::modeFromAction(_failsafe.selectedAction(), _user_mode_intention.get());
|
||||
_vehicle_status.nav_state_user_intention = _mode_management.getNavStateReplacementIfValid(_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()) {
|
||||
case FailsafeBase::Action::Disarm:
|
||||
|
@ -2228,7 +2318,24 @@ bool Commander::handleModeIntentionAndFailsafe()
|
|||
_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()
|
||||
|
@ -2248,6 +2355,21 @@ void Commander::checkAndInformReadyForTakeoff()
|
|||
#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)
|
||||
{
|
||||
switch (blink_msg_state()) {
|
||||
|
@ -2393,8 +2515,19 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
|||
void Commander::updateControlMode()
|
||||
{
|
||||
_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);
|
||||
_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_pub.publish(_vehicle_control_mode);
|
||||
}
|
||||
|
@ -2744,7 +2877,7 @@ void Commander::manualControlCheck()
|
|||
if (override_enabled) {
|
||||
// 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 (_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);
|
||||
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");
|
||||
|
@ -2761,7 +2894,7 @@ void Commander::manualControlCheck()
|
|||
|
||||
// if there's never been a mode change force position control as initial state
|
||||
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_DESCR("transition", "VTOL transition");
|
||||
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);
|
||||
PRINT_MODULE_USAGE_COMMAND("pair");
|
||||
PRINT_MODULE_USAGE_COMMAND("lockdown");
|
||||
|
|
|
@ -34,13 +34,13 @@
|
|||
#pragma once
|
||||
|
||||
/* Helper classes */
|
||||
#include "failure_detector/FailureDetector.hpp"
|
||||
#include "failsafe/failsafe.h"
|
||||
#include "Safety.hpp"
|
||||
#include "worker_thread.hpp"
|
||||
#include "failure_detector/FailureDetector.hpp"
|
||||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||
#include "HomePosition.hpp"
|
||||
#include "ModeManagement.hpp"
|
||||
#include "UserModeIntention.hpp"
|
||||
#include "worker_thread.hpp"
|
||||
|
||||
#include <lib/controllib/blocks.hpp>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
|
@ -80,7 +80,6 @@
|
|||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
|
||||
using math::constrain;
|
||||
|
@ -124,6 +123,7 @@ public:
|
|||
|
||||
private:
|
||||
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);
|
||||
|
||||
|
@ -174,6 +174,10 @@ private:
|
|||
|
||||
void safetyButtonUpdate();
|
||||
|
||||
bool isThrowLaunchInProgress() const;
|
||||
|
||||
void throwLaunchUpdate();
|
||||
|
||||
void vtolStatusUpdate();
|
||||
|
||||
void updateTunes();
|
||||
|
@ -190,6 +194,10 @@ private:
|
|||
|
||||
void checkAndInformReadyForTakeoff();
|
||||
|
||||
void handleCommandsFromModeExecutors();
|
||||
|
||||
void modeManagementUpdate();
|
||||
|
||||
enum class PrearmedMode {
|
||||
DISABLED = 0,
|
||||
SAFETY_BUTTON = 1,
|
||||
|
@ -210,12 +218,19 @@ private:
|
|||
FailsafeBase &_failsafe{_failsafe_instance};
|
||||
FailureDetector _failure_detector{this};
|
||||
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
|
||||
MulticopterThrowLaunch _multicopter_throw_launch{this};
|
||||
Safety _safety{};
|
||||
UserModeIntention _user_mode_intention{this, _vehicle_status, _health_and_arming_checks};
|
||||
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()};
|
||||
HomePosition _home_position{_failsafe_flags};
|
||||
config_overrides_s _config_overrides{};
|
||||
|
||||
|
||||
Hysteresis _auto_disarm_landed{false};
|
||||
|
@ -276,6 +291,7 @@ private:
|
|||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||
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 _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<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_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_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
|
||||
|
||||
|
|
|
@ -65,6 +65,7 @@ px4_add_library(health_and_arming_checks
|
|||
checks/vtolCheck.cpp
|
||||
checks/windCheck.cpp
|
||||
|
||||
checks/externalChecks.cpp
|
||||
)
|
||||
add_dependencies(health_and_arming_checks mode_util)
|
||||
|
||||
|
|
|
@ -87,6 +87,30 @@ Report::EventBufferHeader *Report::addEventToBuffer(uint32_t event_id, const eve
|
|||
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)
|
||||
{
|
||||
// Make sure optional checks are still shown in the UI
|
||||
|
|
|
@ -249,8 +249,8 @@ public:
|
|||
void clearArmingBits(NavModes modes);
|
||||
|
||||
/**
|
||||
* Clear can_run bits for certain modes. This will prevent mode switching and trigger failsafe if the
|
||||
* mode is being run.
|
||||
* Clear can_run bits for certain modes. This will prevent mode switching.
|
||||
* For failsafe use the mode requirements instead, which then will clear the can_run bits.
|
||||
* @param modes affected modes
|
||||
*/
|
||||
void clearCanRunBits(NavModes modes);
|
||||
|
@ -259,6 +259,8 @@ public:
|
|||
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 addExternalEvent(const event_s &event, NavModes modes);
|
||||
private:
|
||||
|
||||
/**
|
||||
|
@ -307,6 +309,7 @@ private:
|
|||
NavModes getModeGroup(uint8_t nav_state) const;
|
||||
|
||||
friend class HealthAndArmingChecks;
|
||||
friend class ExternalChecks;
|
||||
FRIEND_TEST(ReporterTest, basic_no_checks);
|
||||
FRIEND_TEST(ReporterTest, basic_fail_all_modes);
|
||||
FRIEND_TEST(ReporterTest, arming_checks_mode_category);
|
||||
|
|
|
@ -69,6 +69,7 @@
|
|||
#include "checks/vtolCheck.hpp"
|
||||
#include "checks/offboardCheck.hpp"
|
||||
#include "checks/openDroneIDCheck.hpp"
|
||||
#include "checks/externalChecks.hpp"
|
||||
|
||||
class HealthAndArmingChecks : public ModuleParams
|
||||
{
|
||||
|
@ -101,6 +102,10 @@ public:
|
|||
|
||||
const failsafe_flags_s &failsafeFlags() const { return _failsafe_flags; }
|
||||
|
||||
#ifndef CONSTRAINED_FLASH
|
||||
ExternalChecks &externalChecks() { return _external_checks; }
|
||||
#endif
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
private:
|
||||
|
@ -143,8 +148,14 @@ private:
|
|||
RcAndDataLinkChecks _rc_and_data_link_checks;
|
||||
VtolChecks _vtol_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,
|
||||
&_airspeed_checks,
|
||||
&_arm_permission_checks,
|
||||
|
@ -161,7 +172,7 @@ private:
|
|||
&_home_position_checks,
|
||||
&_mission_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,
|
||||
&_parachute_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;
|
||||
}
|
||||
|
||||
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,
|
||||
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
|
||||
|
||||
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;
|
||||
break;
|
||||
|
||||
// vehicle_status_s::NAVIGATION_STATE_EXTERNALx: handled in ModeManagement
|
||||
default:
|
||||
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
|
||||
|
|
|
@ -41,7 +41,7 @@
|
|||
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,
|
||||
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_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;
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
@ -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_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
|
||||
|
|
|
@ -35,14 +35,22 @@
|
|||
#include "UserModeIntention.hpp"
|
||||
|
||||
UserModeIntention::UserModeIntention(ModuleParams *parent, const vehicle_status_s &vehicle_status,
|
||||
const HealthAndArmingChecks &health_and_arming_checks)
|
||||
: ModuleParams(parent), _vehicle_status(vehicle_status), _health_and_arming_checks(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),
|
||||
_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;
|
||||
_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
|
||||
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)) {
|
||||
_nav_state_after_disarming = user_intended_nav_state;
|
||||
}
|
||||
|
||||
if (_handler) {
|
||||
_handler->onUserIntendedNavStateChange(source, user_intended_nav_state);
|
||||
}
|
||||
}
|
||||
|
||||
return allow_change;
|
||||
|
|
|
@ -37,21 +37,42 @@
|
|||
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
|
||||
#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
|
||||
{
|
||||
public:
|
||||
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;
|
||||
|
||||
/**
|
||||
* Change the user intended 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 force always set if true
|
||||
* @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; }
|
||||
|
||||
|
@ -72,6 +93,7 @@ private:
|
|||
|
||||
const vehicle_status_s &_vehicle_status;
|
||||
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 _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);
|
||||
|
||||
/**
|
||||
* 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
|
||||
*
|
||||
|
|
|
@ -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_STABILIZED,
|
||||
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 {
|
||||
|
@ -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_FOLLOW_TARGET,
|
||||
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 {
|
||||
|
@ -131,7 +140,7 @@ static inline union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
|
|||
break;
|
||||
|
||||
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;
|
||||
|
||||
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.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
|
||||
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;
|
||||
|
|
|
@ -137,7 +137,9 @@ void FlightModeManager::updateParams()
|
|||
void FlightModeManager::start_flight_task()
|
||||
{
|
||||
// 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);
|
||||
return;
|
||||
}
|
||||
|
|
|
@ -55,6 +55,7 @@ void LoggedTopics::add_default_topics()
|
|||
add_optional_topic("camera_trigger");
|
||||
add_topic("cellular_status", 200);
|
||||
add_topic("commander_state");
|
||||
add_topic("config_overrides");
|
||||
add_topic("cpuload");
|
||||
add_optional_topic("external_ins_attitude");
|
||||
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 14: return vehicle_status_s::NAVIGATION_STATE_ORBIT;
|
||||
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;
|
||||
}
|
||||
|
|
|
@ -68,7 +68,8 @@ MavlinkCommandSender::~MavlinkCommandSender()
|
|||
int MavlinkCommandSender::handle_vehicle_command(const vehicle_command_s &command, mavlink_channel_t channel)
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
|
@ -1186,8 +1186,9 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
|
|||
return OK;
|
||||
}
|
||||
|
||||
/* 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
|
||||
// if we reach here, the stream list does not contain the stream.
|
||||
// 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;
|
||||
#else
|
||||
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_QUATERNION", 10.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("CAMERA_IMAGE_CAPTURED", 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("EFI_STATUS", 2.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("ATTITUDE_QUATERNION", 50.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("CAMERA_IMAGE_CAPTURED", 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("ESTIMATOR_STATUS", 1.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("ATTITUDE_TARGET", 2.0f);
|
||||
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||
configure_stream_local("CAMERA_IMAGE_CAPTURED", 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("EXTENDED_SYS_STATE", 1.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_QUATERNION", 50.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("CAMERA_IMAGE_CAPTURED", 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("ESC_INFO", 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("ATTITUDE_TARGET", 2.0f);
|
||||
configure_stream_local("AVAILABLE_MODES", 0.3f);
|
||||
configure_stream_local("BATTERY_STATUS", 0.5f);
|
||||
configure_stream_local("COLLISION", unlimited_rate);
|
||||
configure_stream_local("CURRENT_MODE", 0.5f);
|
||||
configure_stream_local("ESTIMATOR_STATUS", 1.0f);
|
||||
configure_stream_local("EXTENDED_SYS_STATE", 1.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
|
||||
&& 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{};
|
||||
msg.result = command_ack.result;
|
||||
|
|
|
@ -124,6 +124,11 @@
|
|||
#include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
|
||||
#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)
|
||||
# include "streams/ADSB_VEHICLE.hpp"
|
||||
# include "streams/AUTOPILOT_STATE_FOR_GIMBAL_DEVICE.hpp"
|
||||
|
@ -492,8 +497,14 @@ static const StreamListItem streams_list[] = {
|
|||
create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
|
||||
#endif // UAVIONIX_ADSB_OUT_CFG_HPP
|
||||
#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
|
||||
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>()
|
||||
create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
|
||||
#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)
|
||||
|
|
|
@ -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);
|
||||
|
||||
union px4_custom_mode get_px4_custom_mode(uint8_t nav_state);
|
||||
|
||||
#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 <string>
|
||||
#include <cstdlib>
|
||||
#include <fstream>
|
||||
|
||||
GZBridge::GZBridge(const char *world, const char *name, const char *model,
|
||||
const char *pose_str) :
|
||||
|
@ -72,7 +74,18 @@ int GZBridge::init()
|
|||
|
||||
// service call to create model
|
||||
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.
|
||||
|
||||
|
@ -116,15 +129,44 @@ int GZBridge::init()
|
|||
bool result;
|
||||
std::string create_service = "/world/" + _world_name + "/create";
|
||||
|
||||
if (_node.Request(create_service, req, 1000, rep, result)) {
|
||||
if (!rep.data() || !result) {
|
||||
PX4_ERR("EntityFactory service call failed");
|
||||
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 (!rep.data() || !result) {
|
||||
PX4_ERR("EntityFactory service call failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Service call timed out");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Service call timed out");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -5,12 +5,27 @@
|
|||
#####
|
||||
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
|
||||
type: px4_msgs::msg::CollisionConstraints
|
||||
|
||||
- topic: /fmu/out/failsafe_flags
|
||||
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
|
||||
type: px4_msgs::msg::PositionSetpointTriplet
|
||||
|
||||
|
@ -29,6 +44,9 @@ publications:
|
|||
- topic: /fmu/out/vehicle_control_mode
|
||||
type: px4_msgs::msg::VehicleControlMode
|
||||
|
||||
- topic: /fmu/out/vehicle_command_ack
|
||||
type: px4_msgs::msg::VehicleCommandAck
|
||||
|
||||
- topic: /fmu/out/vehicle_global_position
|
||||
type: px4_msgs::msg::VehicleGlobalPosition
|
||||
|
||||
|
@ -48,6 +66,26 @@ publications:
|
|||
type: px4_msgs::msg::VehicleTrajectoryWaypoint
|
||||
|
||||
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
|
||||
type: px4_msgs::msg::OffboardControlMode
|
||||
|
@ -82,6 +120,9 @@ subscriptions:
|
|||
- topic: /fmu/in/vehicle_command
|
||||
type: px4_msgs::msg::VehicleCommand
|
||||
|
||||
- topic: /fmu/in/vehicle_command_mode_executor
|
||||
type: px4_msgs::msg::VehicleCommand
|
||||
|
||||
- topic: /fmu/in/vehicle_trajectory_bezier
|
||||
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()
|
||||
{
|
||||
if (!_comm) {
|
||||
|
@ -359,7 +420,17 @@ void UxrceddsClient::run()
|
|||
|
||||
_subs->update(&session, reliable_out, best_effort_out, participant_id, _client_namespace);
|
||||
|
||||
uxr_run_session_timeout(&session, 0);
|
||||
// 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);
|
||||
|
||||
if (_pubs->num_payload_received == prev_num_payload_received) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// time sync session
|
||||
if (_synchronize_timestamps && hrt_elapsed_time(&last_sync_session) > 1_s) {
|
||||
|
@ -369,6 +440,8 @@ void UxrceddsClient::run()
|
|||
}
|
||||
}
|
||||
|
||||
handleMessageFormatRequest();
|
||||
|
||||
// Check for a ping response
|
||||
/* PONG_IN_SESSION_STATUS */
|
||||
if (session.on_pong_flag == 1) {
|
||||
|
|
|
@ -38,6 +38,10 @@
|
|||
|
||||
#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>
|
||||
|
||||
class UxrceddsClient : public ModuleBase<UxrceddsClient>, public ModuleParams
|
||||
|
@ -74,6 +78,11 @@ public:
|
|||
private:
|
||||
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 _custom_participant;
|
||||
const char *_client_namespace;
|
||||
|
|
Loading…
Reference in New Issue