From 3cd9b3c2cffb885a6e5a5d42d7ccac29b94c7e39 Mon Sep 17 00:00:00 2001 From: garfieldG Date: Sun, 31 Jan 2021 05:36:27 -0800 Subject: [PATCH] Added support in Mavlink Ethernet channel parameters Mavlink Ethernet channel settings such as udp port, remote port and broadcast mode now can be changed dynamically via parameters. --- ROMFS/CMakeLists.txt | 3 + .../airframes/1015_iris_obs_avoid.post | 2 +- .../init.d-posix/airframes/17001_tf-g1 | 1 - .../airframes/6011_typhoon_h480.post | 2 +- .../airframes/6012_typhoon_ctrlalloc.post | 2 +- ROMFS/px4fmu_common/init.d-posix/rcS | 6 +- Tools/serial/generate_config.py | 97 ++++++++++++++++++- Tools/serial/rc.serial_port.jinja | 6 ++ Tools/serial/serial_params.c.jinja | 6 ++ boards/av/x-v1/default.cmake | 1 + boards/av/x-v1/init/rc.board_mavlink | 2 +- boards/px4/fmu-v5x/default.cmake | 1 + cmake/px4_add_board.cmake | 7 ++ posix-configs/SITL/init/test/cmd_template.in | 2 +- posix-configs/SITL/init/test/test_mavlink | 2 +- posix-configs/bbblue/px4.config | 3 +- posix-configs/bbblue/px4_fw.config | 3 +- posix-configs/eagle/200qx/mainapp.config | 3 +- posix-configs/eagle/210qc/mainapp.config | 3 +- posix-configs/eagle/flight/mainapp.config | 3 +- posix-configs/eagle/hil/mainapphil.config | 3 +- posix-configs/eagle/init/rcS | 3 +- posix-configs/excelsior/mainapp.config | 3 +- posix-configs/ocpoc/px4.config | 1 - posix-configs/rpi/pilotpi_fw.config | 3 +- posix-configs/rpi/pilotpi_mc.config | 3 +- posix-configs/rpi/px4.config | 3 +- posix-configs/rpi/px4_fw.config | 3 +- posix-configs/rpi/px4_hil.config | 3 +- posix-configs/rpi/px4_test.config | 3 +- src/lib/parameters/CMakeLists.txt | 3 + src/modules/mavlink/mavlink_main.cpp | 56 ++++++----- src/modules/mavlink/mavlink_main.h | 9 +- src/modules/mavlink/mavlink_params.c | 13 --- src/modules/mavlink/module.yaml | 55 ++++++++++- validation/module_schema.yaml | 8 ++ 36 files changed, 242 insertions(+), 85 deletions(-) diff --git a/ROMFS/CMakeLists.txt b/ROMFS/CMakeLists.txt index b7fcfb3044..4159170aaa 100644 --- a/ROMFS/CMakeLists.txt +++ b/ROMFS/CMakeLists.txt @@ -81,6 +81,9 @@ file(GLOB jinja_templates ${PX4_SOURCE_DIR}/Tools/serial/*.jinja) if (px4_constrained_flash_build) set(added_arguments --constrained-flash) endif() +if(PX4_ETHERNET) + set(added_arguments ${added_arguments} --ethernet) +endif() # create list of relative romfs file names set(romfs_copy_files_relative) foreach(romfs_file IN LISTS romfs_copy_files) diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid.post index 240f5d3740..64b8ce8b19 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid.post +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1015_iris_obs_avoid.post @@ -1,2 +1,2 @@ # shellcheck disable=SC2154 -mavlink start -x -u 14558 -r 4000000 -m onboard -o 14541 # add mavlink stream for SDK +mavlink start -x -u 14558 -r 4000000 -m onboard -o 14541 -p # add mavlink stream for SDK diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 index 72ebb6da13..fa7d78da31 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 @@ -38,7 +38,6 @@ then param set NAV_LOITER_RAD 100 param set RWTO_TKOFF 1 - param set MAV_BROADCAST 1 param set FW_ARSP_SCALE_EN 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480.post b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480.post index 80264373dc..0af01438b7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480.post +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6011_typhoon_h480.post @@ -1,7 +1,7 @@ mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix -mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p # shellcheck disable=SC2154 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post index 80264373dc..0af01438b7 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_ctrlalloc.post @@ -1,7 +1,7 @@ mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix -mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 +mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p # shellcheck disable=SC2154 mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 7b535701ae..6cee00668b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -244,7 +244,7 @@ then sh etc/init.d-posix/rc.mavlink_override else # GCS link - mavlink start -x -u $udp_gcs_port_local -r 4000000 -f + mavlink start -x -u $udp_gcs_port_local -r 4000000 -f -p mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u $udp_gcs_port_local mavlink stream -r 50 -s LOCAL_POSITION_NED -u $udp_gcs_port_local mavlink stream -r 50 -s GLOBAL_POSITION_INT -u $udp_gcs_port_local @@ -256,10 +256,10 @@ else mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local # API/Offboard link - mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote + mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote -p # Onboard link to camera - mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote + mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote -p # Onboard link to gimbal mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote diff --git a/Tools/serial/generate_config.py b/Tools/serial/generate_config.py index 368d158f3e..27003b904b 100755 --- a/Tools/serial/generate_config.py +++ b/Tools/serial/generate_config.py @@ -36,6 +36,7 @@ except ImportError as e: # with QGC (parameter metadata) serial_ports = { # index 0 means disabled + # index 1000 means ethernet condiguration # Generic # "URT1": { @@ -155,6 +156,8 @@ parser.add_argument('--rc-dir', type=str, action='store', help='ROMFS output directory', default=None) parser.add_argument('--params-file', type=str, action='store', help='Parameter output file', default=None) +parser.add_argument('--ethernet', action='store_true', + help='Ethernet support') parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help='Verbose Output') @@ -169,6 +172,7 @@ serial_params_output_file = args.params_file serial_params_template = 'serial_params.c.jinja' generate_for_all_ports = args.all_ports constrained_flash = args.constrained_flash +ethernet_supported = args.ethernet if generate_for_all_ports: board_ports = [(key, "") for key in serial_ports] @@ -184,7 +188,16 @@ if rc_serial_output_dir is None and serial_params_output_file is None: # parse the YAML files serial_commands = [] +ethernet_configuration = [] additional_params = "" +additional_ethernet_params = "" + +if ethernet_supported: + ethernet_configuration.append({ + 'tag': "ETH", + 'label': "Ethernet", + 'index': 1000 + }) def parse_yaml_serial_config(yaml_config): """ parse the serial_config section from the yaml config file """ @@ -211,6 +224,80 @@ def parse_yaml_parameters_config(yaml_config): param_group = parameters_section.get('group', None) for param_name in definitions: param = definitions[param_name] + if 'ethernet' in param: + continue + num_instances = param.get('num_instances', 1) + instance_start = param.get('instance_start', 0) # offset + + # get the type and extract all tags + tags = '@group {:}'.format(param_group) + if param['type'] == 'enum': + param_type = 'INT32' + for key in param['values']: + tags += '\n * @value {:} {:}'.format(key, param['values'][key]) + elif param['type'] == 'boolean': + param_type = 'INT32' + tags += '\n * @boolean' + elif param['type'] == 'int32': + param_type = 'INT32' + elif param['type'] == 'float': + param_type = 'FLOAT' + else: + raise Exception("unknown param type {:}".format(param['type'])) + + for tag in ['decimal', 'increment', 'category', 'volatile', 'bit', + 'min', 'max', 'unit', 'reboot_required']: + if tag in param: + tags += '\n * @{:} {:}'.format(tag, param[tag]) + + for i in range(num_instances): + # default value + default_value = 0 + if 'default' in param: + # default can be a list of num_instances or a single value + if type(param['default']) == list: + assert len(param['default']) == num_instances + default_value = param['default'][i] + else: + default_value = param['default'] + + if type(default_value) == bool: + default_value = int(default_value) + + # output the existing C-style format + ret += ''' +/** + * {short_descr} + * + * {long_descr} + * + * {tags} + */ +PARAM_DEFINE_{param_type}({name}, {default_value}); +'''.format(short_descr=param['description']['short'].replace("\n", "\n * "), + long_descr=param['description']['long'].replace("\n", "\n * "), + tags=tags, + param_type=param_type, + name=param_name, + default_value=default_value, + ).replace('${i}', str(i+instance_start)) + return ret + +def parse_yaml_ethernet_parameters_config(yaml_config): + """ parse the parameters section from the yaml config file """ + if 'parameters' not in yaml_config: + return '' + parameters_section_list = yaml_config['parameters'] + for parameters_section in parameters_section_list: + if 'definitions' not in parameters_section: + return '' + definitions = parameters_section['definitions'] + ret = '' + param_group = parameters_section.get('group', None) + for param_name in definitions: + param = definitions[param_name] + if 'ethernet' not in param: + continue num_instances = param.get('num_instances', 1) instance_start = param.get('instance_start', 0) # offset @@ -276,6 +363,8 @@ for yaml_file in args.config_files: # TODO: additional params should be parsed in a separate script additional_params += parse_yaml_parameters_config(yaml_config) + if ethernet_supported: + additional_ethernet_params += parse_yaml_ethernet_parameters_config(yaml_config) except yaml.YAMLError as exc: print(exc) @@ -339,7 +428,8 @@ for serial_command in serial_commands: 'port_param_name': port_param_name, 'default_port': default_port, 'param_group': port_config['group'], - 'description_extended': port_config.get('description_extended', '') + 'description_extended': port_config.get('description_extended', ''), + 'ethernet_config': serial_command.get('ethernet', 'none') }) if verbose: @@ -372,6 +462,7 @@ if rc_serial_output_dir is not None: template = jinja_env.get_template(rc_serial_port_template) with open(rc_serial_port_output_file, 'w') as fid: fid.write(template.render(serial_devices=serial_devices, + ethernet_configuration=ethernet_configuration, constrained_flash=constrained_flash)) # parameter definitions @@ -380,6 +471,8 @@ if serial_params_output_file is not None: template = jinja_env.get_template(serial_params_template) with open(serial_params_output_file, 'w') as fid: fid.write(template.render(serial_devices=serial_devices, + ethernet_configuration=ethernet_configuration, commands=commands, serial_ports=serial_ports, - additional_definitions=additional_params)) + additional_definitions=additional_params, + additional_ethernet_definitions=additional_ethernet_params)) diff --git a/Tools/serial/rc.serial_port.jinja b/Tools/serial/rc.serial_port.jinja index 1abcb8f81a..c3f7c9651e 100644 --- a/Tools/serial/rc.serial_port.jinja +++ b/Tools/serial/rc.serial_port.jinja @@ -17,3 +17,9 @@ if param compare "$PRT" {{ serial_device.index }}; then fi {% endfor %} +{% for config in ethernet_configuration -%} +if param compare "$PRT" {{ config.index }}; then + set SERIAL_DEV ethernet +fi + +{% endfor %} diff --git a/Tools/serial/serial_params.c.jinja b/Tools/serial/serial_params.c.jinja index f92bc5568f..4bed33a2c9 100644 --- a/Tools/serial/serial_params.c.jinja +++ b/Tools/serial/serial_params.c.jinja @@ -58,6 +58,10 @@ PARAM_DEFINE_INT32(SER_{{ serial_device.tag }}_BAUD, {{ serial_device.default_ba {%- for serial_device in serial_devices %} * @value {{ serial_device.index }} {{ serial_device.label }} {%- endfor %} +{%- if command.ethernet_config != "none" %} +{%- for config in ethernet_configuration %} + * @value {{ config.index }} {{ config.label }} +{%- endfor %}{% endif %} * @group {{ command.param_group }} * @reboot_required true */ @@ -68,3 +72,5 @@ PARAM_DEFINE_INT32({{ command.port_param_name }}, {{ command.default_port }}); {{ additional_definitions }} +{{ additional_ethernet_definitions }} + diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake index 7c3e41573b..c1fd053c99 100644 --- a/boards/av/x-v1/default.cmake +++ b/boards/av/x-v1/default.cmake @@ -9,6 +9,7 @@ px4_add_board( ROMFSROOT px4fmu_common TESTING UAVCAN_INTERFACES 2 + ETHERNET SERIAL_PORTS GPS1:/dev/ttyS6 TEL1:/dev/ttyS0 diff --git a/boards/av/x-v1/init/rc.board_mavlink b/boards/av/x-v1/init/rc.board_mavlink index d7ea448203..339d1141fa 100644 --- a/boards/av/x-v1/init/rc.board_mavlink +++ b/boards/av/x-v1/init/rc.board_mavlink @@ -11,4 +11,4 @@ mavlink stream -d /dev/ttyS5 -s LOCAL_POSITION_NED -r 50 # AV-X: start MAVLink UDP port 14570 -mavlink start -x -u 14570 +mavlink start -x -u 14570 -p diff --git a/boards/px4/fmu-v5x/default.cmake b/boards/px4/fmu-v5x/default.cmake index dd7e71762d..ca766cff6b 100644 --- a/boards/px4/fmu-v5x/default.cmake +++ b/boards/px4/fmu-v5x/default.cmake @@ -10,6 +10,7 @@ px4_add_board( IO px4_io-v2_default TESTING UAVCAN_INTERFACES 2 + ETHERNET SERIAL_PORTS GPS1:/dev/ttyS0 TEL1:/dev/ttyS6 diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index 38c3341152..1cb021a02b 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -61,6 +61,7 @@ # [ TESTING ] # [ LINKER_PREFIX ] # [ EMBEDDED_METADATA ] +# [ ETHERNET ] # ) # # Input: @@ -86,6 +87,7 @@ # CONSTRAINED_MEMORY : flag to enable constrained memory options (eg limit maximum number of uORB publications) # TESTING : flag to enable automatic inclusion of PX4 testing modules # LINKER_PREFIX : optional to prefix on the Linker script. +# ETHERNET : flag to indicate that ethernet is enabled # # # Example: @@ -163,6 +165,7 @@ function(px4_add_board) CONSTRAINED_FLASH CONSTRAINED_MEMORY TESTING + ETHERNET REQUIRED PLATFORM VENDOR @@ -269,6 +272,10 @@ function(px4_add_board) set(PX4_TESTING "1" CACHE INTERNAL "testing enabled" FORCE) endif() + if(ETHERNET) + set(PX4_ETHERNET "1" CACHE INTERNAL "ethernet enabled" FORCE) + endif() + if(LINKER_PREFIX) set(PX4_BOARD_LINKER_PREFIX ${LINKER_PREFIX} CACHE STRING "PX4 board linker prefix" FORCE) else() diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index c758f6a8ec..702483c198 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -16,7 +16,7 @@ pwm_out_sim start ver all -mavlink start -x -u 14556 -r 2000000 +mavlink start -x -u 14556 -r 2000000 -p mavlink boot_complete @cmd_name@ start diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index e5db40970f..52da1c4318 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -16,7 +16,7 @@ pwm_out_sim start ver all -mavlink start -x -u 14556 -r 2000000 +mavlink start -x -u 14556 -r 2000000 -p mavlink boot_complete tests mavlink diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index da94108f2c..afe9f4bc57 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -29,7 +29,6 @@ param set SYS_AUTOSTART 4011 # Broadcast heartbeats on local network. This allows a ground control station # to automatically find the drone on the local network. -param set MAV_BROADCAST 1 # MAV_TYPE: 1 Fixed wing aircraft, 2 Quadrotor param set MAV_TYPE 2 @@ -73,7 +72,7 @@ mc_pos_control start mc_att_control start mc_rate_control start -mavlink start -n SoftAp -x -u 14556 -r 1000000 +mavlink start -n SoftAp -x -u 14556 -r 1000000 -p # -n name of wifi interface: SoftAp, wlan or your custom interface, # e.g., -n SoftAp . The default is wlan diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 030173a4bc..15d506d48a 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -29,7 +29,6 @@ param set SYS_AUTOSTART 4011 # Broadcast heartbeats on local network. This allows a ground control station # to automatically find the drone on the local network. -param set MAV_BROADCAST 1 # MAV_TYPE: 1 Fixed wing aircraft, 2 Quadrotor param set MAV_TYPE 2 @@ -68,7 +67,7 @@ land_detector start fixedwing fw_att_control start fw_pos_control_l1 start -mavlink start -n SoftAp -x -u 14556 -r 1000000 +mavlink start -n SoftAp -x -u 14556 -r 1000000 -p # -n name of wifi interface: SoftAp, wlan or your custom interface, # e.g., -n wlan . The default on BBBlue is SoftAp diff --git a/posix-configs/eagle/200qx/mainapp.config b/posix-configs/eagle/200qx/mainapp.config index 43495fd679..251eadb0bc 100644 --- a/posix-configs/eagle/200qx/mainapp.config +++ b/posix-configs/eagle/200qx/mainapp.config @@ -9,10 +9,9 @@ logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize sleep 1 param set CBRK_SUPPLY_CHK 894281 -param set MAV_BROADCAST 1 dataman start navigator start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 diff --git a/posix-configs/eagle/210qc/mainapp.config b/posix-configs/eagle/210qc/mainapp.config index 43495fd679..251eadb0bc 100644 --- a/posix-configs/eagle/210qc/mainapp.config +++ b/posix-configs/eagle/210qc/mainapp.config @@ -9,10 +9,9 @@ logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize sleep 1 param set CBRK_SUPPLY_CHK 894281 -param set MAV_BROADCAST 1 dataman start navigator start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 diff --git a/posix-configs/eagle/flight/mainapp.config b/posix-configs/eagle/flight/mainapp.config index 43495fd679..251eadb0bc 100644 --- a/posix-configs/eagle/flight/mainapp.config +++ b/posix-configs/eagle/flight/mainapp.config @@ -9,10 +9,9 @@ logger start -t -b 200 # Wait 1s before setting parameters for muorb to initialize sleep 1 param set CBRK_SUPPLY_CHK 894281 -param set MAV_BROADCAST 1 dataman start navigator start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config index edfc7d21b7..b61bd58bbc 100644 --- a/posix-configs/eagle/hil/mainapphil.config +++ b/posix-configs/eagle/hil/mainapphil.config @@ -2,8 +2,7 @@ muorb start # Wait 1s before setting parameters for muorb to initialize sleep 1 param set CBRK_SUPPLY_CHK 894281 -param set MAV_BROADCAST 1 -mavlink start -x -u 14556 +mavlink start -x -u 14556 -p sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index 387dba1ebb..f8a0dca0c8 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -94,10 +94,9 @@ qshell mc_rate_control start logger start -b 200 -t -param set MAV_BROADCAST 1 dataman start navigator start -mavlink start -u 14556 -r 1000000 +mavlink start -u 14556 -r 1000000 -p mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 mavlink stream -u 14556 -s RC_CHANNELS -r 20 diff --git a/posix-configs/excelsior/mainapp.config b/posix-configs/excelsior/mainapp.config index c2a8740fc0..4146bfd4e4 100644 --- a/posix-configs/excelsior/mainapp.config +++ b/posix-configs/excelsior/mainapp.config @@ -6,11 +6,10 @@ muorb start logger start -e -t param set CBRK_SUPPLY_CHK 894281 -param set MAV_BROADCAST 1 dataman start navigator start sleep 2 -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index cf44f727b8..31d8482b1a 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -10,7 +10,6 @@ then fi param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 -param set MAV_BROADCAST 1 param set MAV_TYPE 2 param set MAV_SYS_ID 1 diff --git a/posix-configs/rpi/pilotpi_fw.config b/posix-configs/rpi/pilotpi_fw.config index 977b27649e..9fe4da4f7d 100644 --- a/posix-configs/rpi/pilotpi_fw.config +++ b/posix-configs/rpi/pilotpi_fw.config @@ -11,7 +11,6 @@ fi # system_power not implemented param set CBRK_SUPPLY_CHK 894281 # broadcast to LAN -param set MAV_BROADCAST 1 # always keep current config param set SYS_AUTOCONFIG 0 # useless but required for parameter completeness @@ -71,7 +70,7 @@ flight_mode_manager start fw_att_control start fw_pos_control_l1 start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p # Telem mavlink start -x -Z -d /dev/ttySC1 diff --git a/posix-configs/rpi/pilotpi_mc.config b/posix-configs/rpi/pilotpi_mc.config index d09bd21c0e..0d686e0350 100644 --- a/posix-configs/rpi/pilotpi_mc.config +++ b/posix-configs/rpi/pilotpi_mc.config @@ -11,7 +11,6 @@ fi # system_power not implemented param set CBRK_SUPPLY_CHK 894281 # broadcast to LAN -param set MAV_BROADCAST 1 # always keep current config param set SYS_AUTOCONFIG 0 # useless but required for parameter completeness @@ -69,7 +68,7 @@ mc_pos_control start mc_att_control start mc_rate_control start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p # Telem mavlink start -x -Z -d /dev/ttySC1 diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 1a8ec392b6..4cefd5d422 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -11,7 +11,6 @@ then fi param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 -param set MAV_BROADCAST 1 param set MAV_TYPE 2 param set IMU_GYRO_RATEMAX 400 @@ -48,7 +47,7 @@ mc_pos_control start mc_att_control start mc_rate_control start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p if [ -c /dev/ttyUSB0 ] then diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 81a4d0e2f7..31436ecdf9 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -10,7 +10,6 @@ then param load fi param set CBRK_SUPPLY_CHK 894281 -param set MAV_BROADCAST 1 param set SYS_AUTOSTART 2100 param set MAV_TYPE 1 param set IMU_GYRO_RATEMAX 400 @@ -46,7 +45,7 @@ land_detector start fixedwing fw_att_control start fw_pos_control_l1 start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p if [ -c /dev/ttyUSB0 ] then diff --git a/posix-configs/rpi/px4_hil.config b/posix-configs/rpi/px4_hil.config index 8d1abad588..072446f29e 100644 --- a/posix-configs/rpi/px4_hil.config +++ b/posix-configs/rpi/px4_hil.config @@ -13,7 +13,6 @@ then fi param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 1001 -param set MAV_BROADCAST 1 param set MAV_TYPE 2 # Multi-EKF @@ -38,7 +37,7 @@ mc_pos_control start mc_att_control start mc_rate_control start -mavlink start -x -u 14577 -r 1000000 +mavlink start -x -u 14577 -r 1000000 -p navio_sysfs_rc_in start pwm_out_sim start mixer load /dev/pwm_output0 etc/mixers/quad_x.main.mix diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index efb0fafc03..ee58a5414c 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -11,7 +11,6 @@ then fi param set CBRK_SUPPLY_CHK 894281 param set SYS_AUTOSTART 4001 -param set MAV_BROADCAST 1 param set MAV_TYPE 2 param set IMU_GYRO_RATEMAX 400 @@ -47,7 +46,7 @@ flight_mode_manager start mc_pos_control start mc_att_control start -mavlink start -x -u 14556 -r 1000000 +mavlink start -x -u 14556 -r 1000000 -p if [ -c /dev/ttyUSB0 ] then diff --git a/src/lib/parameters/CMakeLists.txt b/src/lib/parameters/CMakeLists.txt index 4978af39d1..6236b14f62 100644 --- a/src/lib/parameters/CMakeLists.txt +++ b/src/lib/parameters/CMakeLists.txt @@ -75,6 +75,9 @@ file(GLOB jinja_templates ${PX4_SOURCE_DIR}/Tools/serial/*.jinja) if (px4_constrained_flash_build) set(added_arguments --constrained-flash) endif() +if(PX4_ETHERNET) + set(added_arguments ${added_arguments} --ethernet) +endif() add_custom_command(OUTPUT ${generated_serial_params_file} COMMAND ${CMAKE_COMMAND} -E make_directory ${generated_params_dir} COMMAND ${PYTHON_EXECUTABLE} ${PX4_SOURCE_DIR}/Tools/serial/generate_config.py diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 049bb7a249..8b3b148b36 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1809,11 +1809,10 @@ Mavlink::task_main(int argc, char *argv[]) int myoptind = 1; const char *myoptarg = nullptr; #if defined(CONFIG_NET) || defined(__PX4_POSIX) - char *eptr; int temp_int_arg; #endif - while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZ", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, _baudrate) != 0) { @@ -1859,29 +1858,25 @@ Mavlink::task_main(int argc, char *argv[]) #if defined(MAVLINK_UDP) case 'u': - temp_int_arg = strtoul(myoptarg, &eptr, 10); - - if (*eptr == '\0') { - _network_port = temp_int_arg; - set_protocol(Protocol::UDP); + if (px4_get_parameter_value(myoptarg, temp_int_arg) != 0) { + PX4_ERR("invalid data udp_port"); + err_flag = true; } else { - PX4_ERR("invalid data udp_port '%s'", myoptarg); - err_flag = true; + _network_port = temp_int_arg; + set_protocol(Protocol::UDP); } break; case 'o': - temp_int_arg = strtoul(myoptarg, &eptr, 10); - - if (*eptr == '\0') { - _remote_port = temp_int_arg; - set_protocol(Protocol::UDP); + if (px4_get_parameter_value(myoptarg, temp_int_arg) != 0) { + PX4_ERR("invalid remote udp_port"); + err_flag = true; } else { - PX4_ERR("invalid remote udp_port '%s'", myoptarg); - err_flag = true; + _remote_port = temp_int_arg; + set_protocol(Protocol::UDP); } break; @@ -1899,6 +1894,10 @@ Mavlink::task_main(int argc, char *argv[]) break; + case 'p': + _mav_broadcast = BROADCAST_MODE_ON; + break; + #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) // multicast @@ -1907,6 +1906,7 @@ Mavlink::task_main(int argc, char *argv[]) if (inet_aton(myoptarg, &_src_addr.sin_addr)) { _src_addr_initialized = true; + _mav_broadcast = BROADCAST_MODE_MULTICAST; } else { PX4_ERR("invalid partner ip '%s'", myoptarg); @@ -1923,6 +1923,7 @@ Mavlink::task_main(int argc, char *argv[]) #endif #else + case 'p': case 'u': case 'o': case 't': @@ -2206,7 +2207,7 @@ Mavlink::task_main(int argc, char *argv[]) #if defined(CONFIG_NET) - if (_param_mav_broadcast.get() != BROADCAST_MODE_MULTICAST) { + if (!multicast_enabled()) { _src_addr_initialized = false; } @@ -2767,6 +2768,12 @@ Mavlink::display_status() case Protocol::UDP: printf("UDP (%i, remote port: %i)\n", _network_port, _remote_port); + printf("\tBroadcast enabled: %s\n", + broadcast_enabled() ? "YES" : "NO"); +#if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) + printf("\tMulticast enabled: %s\n", + multicast_enabled() ? "YES" : "NO"); +#endif #ifdef __PX4_POSIX if (get_client_source_initialized()) { @@ -2833,7 +2840,6 @@ Mavlink::stream_command(int argc, char *argv[]) float rate = -1.0f; const char *stream_name = nullptr; #ifdef MAVLINK_UDP - char *eptr; int temp_int_arg; unsigned short network_port = 0; #endif // MAVLINK_UDP @@ -2879,13 +2885,12 @@ Mavlink::stream_command(int argc, char *argv[]) } else if (0 == strcmp(argv[i], "-u") && i < argc - 1) { provided_network_port = true; - temp_int_arg = strtoul(argv[i + 1], &eptr, 10); - if (*eptr == '\0') { - network_port = temp_int_arg; + if (px4_get_parameter_value(argv[i + 1], temp_int_arg) != 0) { + err_flag = true; } else { - err_flag = true; + network_port = temp_int_arg; } i++; @@ -2963,7 +2968,7 @@ Mavlink::set_boot_complete() if (inst && (inst->get_mode() != MAVLINK_MODE_ONBOARD) && !inst->broadcast_enabled() && inst->get_protocol() == Protocol::UDP) { - PX4_INFO("MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)"); + PX4_INFO("MAVLink only on localhost (set param MAV_{i}_BROADCAST = 1 to enable network)"); } } @@ -3010,16 +3015,17 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50 PRINT_MODULE_USAGE_PARAM_INT('b', 57600, 9600, 3000000, "Baudrate (can also be p:)", true); PRINT_MODULE_USAGE_PARAM_INT('r', 0, 10, 10000000, "Maximum sending data rate in B/s (if 0, use baudrate / 20)", true); #if defined(CONFIG_NET) || defined(__PX4_POSIX) + PRINT_MODULE_USAGE_PARAM_FLAG('p', "Enable Broadcast", true); PRINT_MODULE_USAGE_PARAM_INT('u', 14556, 0, 65536, "Select UDP Network Port (local)", true); PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true); PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, - "Partner IP (broadcasting can be enabled via MAV_BROADCAST param)", true); + "Partner IP (broadcasting can be enabled via MAV_{i}_BROADCAST param)", true); #endif PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal", "Mode: sets default streams and rates", true); PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "", "wifi/ethernet interface name", true); #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE) - PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_BROADCAST param)", true); + PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_{i}_BROADCAST param)", true); #endif PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true); PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 4992cd8759..d16cf978e5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -265,7 +265,9 @@ public: #if defined(MAVLINK_UDP) static Mavlink *get_instance_for_network_port(unsigned long port); - bool broadcast_enabled() { return _param_mav_broadcast.get() == BROADCAST_MODE_ON; } + bool broadcast_enabled() { return _mav_broadcast == BROADCAST_MODE_ON; } + + bool multicast_enabled() { return _mav_broadcast == BROADCAST_MODE_MULTICAST; } #endif // MAVLINK_UDP /** @@ -610,6 +612,8 @@ private: hrt_abstime _bytes_timestamp{0}; #if defined(MAVLINK_UDP) + BROADCAST_MODE _mav_broadcast {BROADCAST_MODE_OFF}; + sockaddr_in _myaddr {}; sockaddr_in _src_addr {}; sockaddr_in _bcast_addr {}; @@ -659,9 +663,6 @@ private: (ParamInt) _param_mav_type, (ParamBool) _param_mav_usehilgps, (ParamBool) _param_mav_fwdextsp, -#if defined(MAVLINK_UDP) - (ParamInt) _param_mav_broadcast, -#endif // MAVLINK_UDP (ParamBool) _param_mav_hash_chk_en, (ParamBool) _param_mav_hb_forw_en, (ParamBool) _param_mav_odom_lp, diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 25dce24603..fb88876134 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -130,19 +130,6 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); */ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); -/** - * Broadcast heartbeats on local network - * - * This allows a ground control station to automatically find the drone - * on the local network. - * - * @value 0 Never broadcast - * @value 1 Always broadcast - * @value 2 Only multicast - * @group MAVLink - */ -PARAM_DEFINE_INT32(MAV_BROADCAST, 0); - /** * Parameter hash check. * diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index 2924f8c21e..45d84ab2bc 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -3,7 +3,15 @@ __max_num_config_instances: &max_num_config_instances 3 module_name: MAVLink serial_config: - command: | - set MAV_ARGS "-b p:${BAUD_PARAM} -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE" + if [ $SERIAL_DEV != ethernet ]; then + set MAV_ARGS "-d ${SERIAL_DEV} -b p:${BAUD_PARAM} -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE" + else + set MAV_ARGS "-u p:MAV_${i}_UDP_PRT -o p:MAV_${i}_REMOTE_PRT -m p:MAV_${i}_MODE -r p:MAV_${i}_RATE" + if param compare MAV_${i}_BROADCAST 1 + then + set MAV_ARGS "${MAV_ARGS} -p" + fi + fi if param compare MAV_${i}_FORWARD 1 then set MAV_ARGS "${MAV_ARGS} -f" @@ -12,7 +20,7 @@ serial_config: then set MAV_ARGS "${MAV_ARGS} -s" fi - mavlink start -d ${SERIAL_DEV} ${MAV_ARGS} -x + mavlink start ${MAV_ARGS} -x port_config_param: name: MAV_${i}_CONFIG group: MAVLink @@ -22,6 +30,7 @@ serial_config: # 2: Board-specific / no fixed function or port default: [TEL1, "", ""] num_instances: *max_num_config_instances + ethernet: parameters: - group: MAVLink @@ -97,3 +106,45 @@ parameters: reboot_required: true num_instances: *max_num_config_instances default: [true, true, true] + + MAV_${i}_UDP_PRT: + description: + short: MAVLink Network Port for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected udp port will be set and used in MAVLink instance ${i}. + + type: int32 + reboot_required: true + num_instances: *max_num_config_instances + default: [14556, 0, 0] + ethernet: + + MAV_${i}_REMOTE_PRT: + description: + short: MAVLink Remote Port for instance ${i} + long: | + If ethernet enabled and selected as configuration for MAVLink instance ${i}, + selected remote port will be set and used in MAVLink instance ${i}. + + type: int32 + reboot_required: true + num_instances: *max_num_config_instances + default: [14550, 0, 0] + ethernet: + + MAV_${i}_BROADCAST: + description: + short: Broadcast heartbeats on local network for MAVLink instance ${i} + long: | + This allows a ground control station to automatically find the drone + on the local network. + + type: enum + values: + 0: Never broadcast + 1: Always broadcast + 2: Only multicast + num_instances: *max_num_config_instances + default: [1, 0, 0] + ethernet: diff --git a/validation/module_schema.yaml b/validation/module_schema.yaml index 6982daf66c..cf954e401a 100644 --- a/validation/module_schema.yaml +++ b/validation/module_schema.yaml @@ -75,6 +75,10 @@ serial_config: # Default: 1 type: integer min: 1 + ethernet: + # Optional command ethernet. + # If omitted, ethernet condiguration value will be omitted. + nullable: True parameters: @@ -206,5 +210,9 @@ parameters: # [0, N-1] # Default: 0 type: integer + ethernet: + # Optional command ethernet. + # If specified, parameter will be generated only if board supports Ethernet. + nullable: True