diff --git a/Makefile b/Makefile index 2b9a34553f..e2f1814f0c 100644 --- a/Makefile +++ b/Makefile @@ -89,7 +89,7 @@ endif # -------------------------------------------------------------------- # describe how to build a cmake config define cmake-build -+@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi ++@if [ ! -e $(PWD)/build_$@/CMakeCache.txt ]; then git submodule update --init --recursive --force && mkdir -p $(PWD)/build_$@ && cd $(PWD)/build_$@ && cmake .. -G$(PX4_CMAKE_GENERATOR) -DCONFIG=$(1); fi +$(PX4_MAKE) -C $(PWD)/build_$@ $(PX4_MAKE_ARGS) $(ARGS) endef @@ -104,13 +104,13 @@ endef # -------------------------------------------------------------------- # Do not put any spaces between function arguments. -px4fmu-v1_default: git-init +px4fmu-v1_default: $(call cmake-build,nuttx_px4fmu-v1_default) -px4fmu-v2_default: git-init +px4fmu-v2_default: $(call cmake-build,nuttx_px4fmu-v2_default) -px4fmu-v2_simple: git-init +px4fmu-v2_simple: $(call cmake-build,nuttx_px4fmu-v2_simple) nuttx_sim_simple: @@ -172,19 +172,8 @@ check_format: clean: @rm -rf build_*/ - -distclean: clean - @cd NuttX - @git clean -d -f -x - @cd .. - @cd src/modules/uavcan/libuavcan - @git clean -d -f -x - @cd ../../../.. - -# XXX this is not the right way to fix it, but we need a temporary solution -# for average joe -git-init: - @git submodule update --init --recursive + @(cd NuttX && git clean -d -f -x) + @(cd src/modules/uavcan/libuavcan && git clean -d -f -x) # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak libuavcan diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh deleted file mode 100755 index 5fa0831438..0000000000 --- a/Tools/check_submodules.sh +++ /dev/null @@ -1,144 +0,0 @@ -#!/bin/sh - -[ -n "$GIT_SUBMODULES_ARE_EVIL" ] && { - # GIT_SUBMODULES_ARE_EVIL is set, meaning user doesn't want submodules - echo "Skipping submodules. NUTTX_SRC is set to $NUTTX_SRC" - exit 0 -} - -if [ -d NuttX/nuttx ]; - then - STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<") - if [ -z "$STATUSRETVAL" ]; then - echo "Checked NuttX submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo " NuttX sub repo not at correct version. Try 'git submodule update'" - echo " or follow instructions on http://pixhawk.org/dev/git/submodules" - echo "" - echo " DO NOT FORGET TO RUN 'make distclean && make archives' AFTER EACH NUTTX UPDATE!" - exit 1 - fi -else - git submodule update --init --recursive -fi - - -if [ -d mavlink/include/mavlink/v1.0 ]; - then - STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<") - if [ -z "$STATUSRETVAL" ]; then - echo "Checked mavlink submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "mavlink sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - - -if [ -d uavcan ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i uavcan | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked uavcan submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "uavcan sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -if [ -d src/lib/eigen ] -then - echo "ARG = $1" - if [ $1 = "qurt" ] - then - # QuRT needs to use Eigen 3.2 because the toolchain doews not support C++11 - STATUSRETVAL=$(true) - else - STATUSRETVAL=$(git submodule summary | grep -A20 -i eigen | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked Eigen submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "eigen sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi - fi -else - git submodule update --init --recursive -fi - -if [ -d Tools/gencpp ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i gencpp | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked gencpp submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "gencpp sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -if [ -d Tools/genmsg ] -then - STATUSRETVAL=$(git submodule summary | grep -A20 -i genmsg | grep "<") - if [ -z "$STATUSRETVAL" ] - then - echo "Checked genmsg submodule, correct version found" - else - echo "" - echo "" - echo "New commits required:" - echo "$(git submodule summary)" - echo "" - echo "" - echo "genmsg sub repo not at correct version. Try 'git submodule update'" - echo "or follow instructions on http://pixhawk.org/dev/git/submodules" - exit 1 - fi -else - git submodule update --init --recursive -fi - -exit 0 diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index d32bbc0f9c..1f240ddbf6 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -24,11 +24,16 @@ for index,m in enumerate(raw_messages): if ('float32[' in line.split(' ')[0]): num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) temp_list.append(("float_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) - if ('uint64[' in line.split(' ')[0]): + elif ('float64[' in line.split(' ')[0]): + num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) + temp_list.append(("double_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) + elif ('uint64[' in line.split(' ')[0]): num_floats = int(line.split(" ")[0].split("[")[1].split("]")[0]) temp_list.append(("uint64_array",line.split(' ')[1].split('\t')[0].split('\n')[0],num_floats)) elif(line.split(' ')[0] == "float32"): temp_list.append(("float",line.split(' ')[1].split('\t')[0].split('\n')[0])) + elif(line.split(' ')[0] == "float64"): + temp_list.append(("double",line.split(' ')[1].split('\t')[0].split('\n')[0])) elif(line.split(' ')[0] == "uint64") and len(line.split('=')) == 1: temp_list.append(("uint64",line.split(' ')[1].split('\t')[0].split('\n')[0])) elif(line.split(' ')[0] == "uint32") and len(line.split('=')) == 1: @@ -114,6 +119,10 @@ print(""" #define PRIu64 "llu" #endif +#ifndef PRI64 +#define PRI64 "lld" +#endif + """) for m in messages: print("#include " % m) @@ -151,11 +160,19 @@ for index,m in enumerate(messages[1:]): print("\t\t\torb_copy(ID,sub,&container);") for item in message_elements[index+1]: if item[0] == "float": - print("\t\t\tprintf(\"%s: %%f\\n\",(double)container.%s);" % (item[1], item[1])) + print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1])) elif item[0] == "float_array": print("\t\t\tprintf(\"%s: \");" % item[1]) print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2]) - print("\t\t\t\tprintf(\"%%f \",(double)container.%s[j]);" % item[1]) + print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1]) + print("\t\t\t}") + print("\t\t\tprintf(\"\\n\");") + elif item[0] == "double": + print("\t\t\tprintf(\"%s: %%8.4f\\n\",(double)container.%s);" % (item[1], item[1])) + elif item[0] == "double_array": + print("\t\t\tprintf(\"%s: \");" % item[1]) + print("\t\t\tfor (int j = 0; j < %d; j++) {" % item[2]) + print("\t\t\t\tprintf(\"%%8.4f \",(double)container.%s[j]);" % item[1]) print("\t\t\t}") print("\t\t\tprintf(\"\\n\");") elif item[0] == "uint64": diff --git a/Tools/px4airframes/rcout.py b/Tools/px4airframes/rcout.py index de5af0f305..48bb1774a9 100644 --- a/Tools/px4airframes/rcout.py +++ b/Tools/px4airframes/rcout.py @@ -1,5 +1,6 @@ from xml.sax.saxutils import escape import codecs +import os class RCOutput(): def __init__(self, groups, board): @@ -30,7 +31,7 @@ class RCOutput(): for group in groups: result += "# GROUP: %s\n\n" % group.GetName() for param in group.GetParams(): - path = param.GetPath().rsplit('/', 1)[1] + path = os.path.split(param.GetPath())[1] id_val = param.GetId() name = param.GetFieldValue("short_desc") long_desc = param.GetFieldValue("long_desc") diff --git a/Tools/px4airframes/srcparser.py b/Tools/px4airframes/srcparser.py index fb491ede15..e1d522dcd8 100644 --- a/Tools/px4airframes/srcparser.py +++ b/Tools/px4airframes/srcparser.py @@ -1,5 +1,6 @@ import sys import re +import os class ParameterGroup(object): """ @@ -160,7 +161,7 @@ class SourceParser(object): """ airframe_id = None - airframe_id = path.rsplit('/',1)[1].split('_',1)[0] + airframe_id = os.path.split(path)[1].split('_',1)[0] # Skip if not numeric if (not self.IsNumber(airframe_id)): diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 9a0b17b7a2..5c9b8aefdf 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -136,14 +136,17 @@ function(px4_add_git_submodule) REQUIRED TARGET PATH ARGN ${ARGN}) string(REPLACE "/" "_" NAME ${PATH}) - add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_${NAME}.stamp + add_custom_command(OUTPUT ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - COMMAND git submodule update --init --recursive -f ${PATH} - COMMAND touch ${CMAKE_BINARY_DIR}/git_${NAME}.stamp + COMMAND git submodule init ${PATH} + COMMAND touch ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp + DEPENDS ${CMAKE_SOURCE_DIR}/.gitmodules ) add_custom_target(${TARGET} - DEPENDS git_${NAME}.stamp - ) + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + COMMAND git submodule update --recursive ${PATH} + DEPENDS ${CMAKE_BINARY_DIR}/git_init_${NAME}.stamp + ) endfunction() #============================================================================= @@ -418,7 +421,8 @@ function(px4_add_upload) ) elseif(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Windows") foreach(port RANGE 32 0) - list(APPEND "COM${port}") + list(APPEND serial_ports + "COM${port}") endforeach() endif() px4_join(OUT serial_ports LIST "${serial_ports}" GLUE ",") @@ -543,6 +547,7 @@ function(px4_add_common_flags) endif() set(c_warnings + -Wbad-function-cast -Wstrict-prototypes -Wmissing-prototypes -Wnested-externs diff --git a/cmake/nuttx/px4_impl_nuttx.cmake b/cmake/nuttx/px4_impl_nuttx.cmake index 2586478383..af7391f45c 100644 --- a/cmake/nuttx/px4_impl_nuttx.cmake +++ b/cmake/nuttx/px4_impl_nuttx.cmake @@ -213,8 +213,8 @@ function(px4_nuttx_add_export) # copy add_custom_command(OUTPUT nuttx_copy_${CONFIG}.stamp COMMAND ${MKDIR} -p ${CMAKE_BINARY_DIR}/${CONFIG} - COMMAND ${RM} -rf ${nuttx_src} - COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/NuttX ${nuttx_src} + COMMAND ${MKDIR} -p ${nuttx_src} + COMMAND ${CP} -a ${CMAKE_SOURCE_DIR}/NuttX/. ${nuttx_src}/ COMMAND ${RM} -rf ${nuttx_src}/.git COMMAND ${TOUCH} nuttx_copy_${CONFIG}.stamp DEPENDS ${DEPENDS}) @@ -229,7 +229,7 @@ function(px4_nuttx_add_export) COMMAND ${CP} -r ${CMAKE_SOURCE_DIR}/nuttx-configs/${CONFIG} ${nuttx_src}/nuttx/configs COMMAND cd ${nuttx_src}/nuttx/tools && ./configure.sh ${CONFIG}/nsh COMMAND ${ECHO} Exporting NuttX for ${CONFIG} - COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export + COMMAND ${MAKE} --no-print-directory --quiet -C ${nuttx_src}/nuttx -j${THREADS} -r CONFIG_ARCH_BOARD=${CONFIG} export > /dev/null COMMAND ${CP} -r ${nuttx_src}/nuttx/nuttx-export.zip ${CMAKE_BINARY_DIR}/${CONFIG}.export DEPENDS ${config_files} ${DEPENDS} __nuttx_copy_${CONFIG}) diff --git a/msg/uavcan_parameter_request.msg b/msg/uavcan_parameter_request.msg new file mode 100644 index 0000000000..9ced52bbae --- /dev/null +++ b/msg/uavcan_parameter_request.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge request type +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/uavcan_parameter_value.msg b/msg/uavcan_parameter_value.msg new file mode 100644 index 0000000000..091c5fd278 --- /dev/null +++ b/msg/uavcan_parameter_value.msg @@ -0,0 +1,8 @@ +# UAVCAN-MAVLink parameter bridge response type +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg index 6ac66d22c7..3a4fb3a596 100644 --- a/msg/vehicle_command.msg +++ b/msg/vehicle_command.msg @@ -53,6 +53,7 @@ uint32 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-b uint32 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition uint32 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan uint32 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment +uint32 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started uint32 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | uint32 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 871b214f49..3340fdd0c9 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -801,11 +801,11 @@ CONFIG_SCHED_WORKQUEUE=y CONFIG_SCHED_HPWORK=y CONFIG_SCHED_WORKPRIORITY=192 CONFIG_SCHED_WORKPERIOD=5000 -CONFIG_SCHED_WORKSTACKSIZE=1800 +CONFIG_SCHED_WORKSTACKSIZE=1600 CONFIG_SCHED_LPWORK=y CONFIG_SCHED_LPWORKPRIORITY=50 CONFIG_SCHED_LPWORKPERIOD=50000 -CONFIG_SCHED_LPWORKSTACKSIZE=1800 +CONFIG_SCHED_LPWORKSTACKSIZE=1600 # CONFIG_LIB_KBDCODEC is not set # CONFIG_LIB_SLCDCODEC is not set diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index a402f327b7..0c1ab48267 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -121,5 +121,10 @@ */ #define SENSORIOCGROTATION _SENSORIOC(6) +/** + * Test the sensor calibration + */ +#define SENSORIOCCALTEST _SENSORIOC(7) + #endif /* _DRV_SENSOR_H */ diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index a403f2fa24..97a525a8e9 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -57,8 +57,10 @@ __EXPORT int ex_hwtest_main(int argc, char *argv[]); int ex_hwtest_main(int argc, char *argv[]) { - warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); - warnx("(run to do so)"); + warnx("DO NOT FORGET TO STOP THE DEFAULT CONTROL APPS!"); + warnx("(run ,)"); + warnx("( and)"); + warnx("( to do so)"); warnx("usage: http://px4.io/dev/examples/write_output"); struct actuator_controls_s actuators; diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index bbf5f8ec6b..2a8a0695cf 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -71,7 +71,50 @@ namespace Commander { -static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) + +static int check_calibration(int fd, const char* param_template, int &devid); + +int check_calibration(int fd, const char* param_template, int &devid) +{ + bool calibration_found; + + /* new style: ask device for calibration state */ + int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0); + + calibration_found = (ret == OK); + + devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); + + char s[20]; + int instance = 0; + + /* old style transition: check param values */ + while (!calibration_found) { + sprintf(s, param_template, instance); + param_t parm = param_find(s); + + /* if the calibration param is not present, abort */ + if (parm == PARAM_INVALID) { + break; + } + + /* if param get succeeds */ + int calibration_devid; + if (!param_get(parm, &(calibration_devid))) { + + /* if the devid matches, exit early */ + if (devid == calibration_devid) { + calibration_found = true; + break; + } + } + instance++; + } + + return !calibration_found; +} + +static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_MAG%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance); success = false; @@ -115,7 +154,7 @@ out: return success; } -static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic) +static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id) { bool success = true; @@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_ACC%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance); success = false; @@ -184,7 +219,7 @@ out: return success; } -static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } - int calibration_devid; - int ret; - int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - sprintf(s, "CAL_GYRO%u_ID", instance); - param_get(param_find(s), &(calibration_devid)); + int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id); - if (devid != calibration_devid) { + if (ret) { mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance); success = false; @@ -228,7 +259,7 @@ out: return success; } -static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) +static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id) { bool success = true; @@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional) return false; } + device_id = -1000; + + // TODO: There is no baro calibration yet, since no external baros exist + // int ret = check_calibration(fd, "CAL_BARO%u_ID"); + + // if (ret) { + // mavlink_and_console_log_critical(mavlink_fd, + // "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance); + // success = false; + // goto out; + // } + +//out: + px4_close(fd); return success; } @@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- MAG ---- */ if (checkMag) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_MAG_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_mag_count; i++) { bool required = (i < max_mandatory_mag_count); + int device_id = -1; - if (!magnometerCheck(mavlink_fd, i, !required) && required) { + if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary compass not operational"); + failed = true; } } /* ---- ACCEL ---- */ if (checkAcc) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_ACC_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_accel_count; i++) { bool required = (i < max_mandatory_accel_count); + int device_id = -1; - if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) { + if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational"); + failed = true; } } /* ---- GYRO ---- */ if (checkGyro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_GYRO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_gyro_count; i++) { bool required = (i < max_mandatory_gyro_count); + int device_id = -1; - if (!gyroCheck(mavlink_fd, i, !required) && required) { + if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + /* check if the primary device is present */ + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational"); + failed = true; } } /* ---- BARO ---- */ if (checkBaro) { + bool prime_found = false; + int32_t prime_id = 0; + param_get(param_find("CAL_BARO_PRIME"), &prime_id); + /* check all sensors, but fail only for mandatory ones */ for (unsigned i = 0; i < max_optional_baro_count; i++) { bool required = (i < max_mandatory_baro_count); + int device_id = -1; - if (!baroCheck(mavlink_fd, i, !required) && required) { + if (!baroCheck(mavlink_fd, i, !required, device_id) && required) { failed = true; } + + if (device_id == prime_id) { + prime_found = true; + } + } + + // TODO there is no logic in place to calibrate the primary baro yet + // // check if the primary device is present + if (!prime_found && prime_id != 0) { + mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational"); + failed = true; } } @@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro /* ---- RC CALIBRATION ---- */ if (checkRC) { if (rc_calibration_check(mavlink_fd) != OK) { + mavlink_log_critical(mavlink_fd, "RC calibration check failed"); failed = true; } } /* ---- Global Navigation Satellite System receiver ---- */ - if(checkGNSS) { + if (checkGNSS) { if(!gnssCheck(mavlink_fd)) { failed = true; } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 6a38d54cb6..d7a58e6254 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -156,6 +156,10 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; +static int32_t device_id[max_accel_sens]; +static int device_prio_max = 0; +static int32_t device_id_primary = 0; + calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num); int mat_invert3(float src[3][3], float dst[3][3]); @@ -172,7 +176,6 @@ typedef struct { int do_accel_calibration(int mavlink_fd) { int fd; - int32_t device_id[max_accel_sens]; mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); @@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd) bool failed = false; + failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); + /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); @@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel struct accel_report arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); timestamps[i] = arp.timestamp; + + // Get priority + int32_t prio; + orb_priority(worker_data.subs[i], &prio); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = device_id[i]; + } } if (result == calibrate_return_ok) { diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e86b7cfb9c..79d0811b72 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -802,6 +802,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE: + case vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN: case vehicle_command_s::VEHICLE_CMD_CUSTOM_0: case vehicle_command_s::VEHICLE_CMD_CUSTOM_1: case vehicle_command_s::VEHICLE_CMD_CUSTOM_2: diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 817cbcdb0e..54866d8cb3 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, // z scale }; + int device_prio_max = 0; + int32_t device_id_primary = 0; + for (unsigned s = 0; s < max_gyros; s++) { char str[30]; @@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd) for (unsigned s = 0; s < max_gyros; s++) { worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + + // Get priority + int32_t prio; + orb_priority(worker_data.gyro_sensor_sub[s], &prio); + + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = worker_data.device_id[s]; + } } int cancel_sub = calibrate_cancel_subscribe(); @@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd) } if (res == OK) { + /* set offset parameters to new values */ bool failed = false; + failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary))); + for (unsigned s = 0; s < max_gyros; s++) { if (worker_data.device_id[s] != 0) { char str[30]; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4511882c7b..eba8feec99 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take +int32_t device_ids[max_mags]; +int device_prio_max = 0; +int32_t device_id_primary = 0; + calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); /// Data passed to calibration worker routine @@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd) // Determine which mags are available and reset each - int32_t device_ids[max_mags]; char str[30]; for (size_t i=0; i device_prio_max) { + device_prio_max = prio; + device_id_primary = device_ids[cur_mag]; + } } } } @@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } if (result == calibrate_return_ok) { + + (void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary)); + for (unsigned cur_mag=0; cur_magdAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); + PX4_INFO("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); PX4_INFO("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5], @@ -1338,7 +1339,7 @@ void AttitudePositionEstimatorEKF::pollData() } else { _ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]); _ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]); - _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f; + _ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]); } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]; @@ -1414,21 +1415,21 @@ void AttitudePositionEstimatorEKF::pollData() static hrt_abstime lastprint = 0; if (hrt_elapsed_time(&lastprint) > 1000000) { - warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", + PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, dtoverflow5, dtoverflow10); - warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z, (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z); - warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]), (double)(_sensor_combined.accelerometer_integral_m_s[0]), (double)(_sensor_combined.accelerometer_integral_m_s[1]), (double)(_sensor_combined.accelerometer_integral_m_s[2])); - warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", + PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f", (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), @@ -1738,8 +1739,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } if (!strcmp(argv[1], "status")) { - PX4_INFO("running"); - estimator::g_estimator->print_status(); return 0; diff --git a/src/modules/fw_att_control/CMakeLists.txt b/src/modules/fw_att_control/CMakeLists.txt index b434a36a86..b093c71cee 100644 --- a/src/modules/fw_att_control/CMakeLists.txt +++ b/src/modules/fw_att_control/CMakeLists.txt @@ -38,7 +38,6 @@ px4_add_module( -Os SRCS fw_att_control_main.cpp - fw_att_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 5e640fcc12..51c06fabb9 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -40,11 +40,6 @@ * @author Thomas Gubler */ -#include - -#include - - /* * Controller parameters, accessible via MAVLink * diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt index 78313eed84..e74b6e4b37 100644 --- a/src/modules/fw_pos_control_l1/CMakeLists.txt +++ b/src/modules/fw_pos_control_l1/CMakeLists.txt @@ -39,11 +39,9 @@ px4_add_module( -Os SRCS fw_pos_control_l1_main.cpp - fw_pos_control_l1_params.c landingslope.cpp mtecs/mTecs.cpp mtecs/limitoverride.cpp - mtecs/mTecs_params.c DEPENDS platforms__common lib__external_lgpl diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index e06814d5f3..cfd1988f44 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -39,9 +39,6 @@ * @author Lorenz Meier */ -#include -#include - /* * Controller parameters, accessible via MAVLink */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 3b98454b95..0d1025f7d4 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -39,9 +39,6 @@ * @author Thomas Gubler */ -#include -#include - /* * Controller parameters, accessible via MAVLink */ diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index b005f3d80d..368712e752 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -41,27 +41,29 @@ #include +#include +#include + #include "mavlink_parameters.h" #include "mavlink_main.h" +ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s); +ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s); #define HASH_PARAM "_HASH_CHECK" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), _send_all_index(-1), _rc_param_map_pub(nullptr), - _rc_param_map() + _rc_param_map(), + _uavcan_parameter_request_pub(nullptr), + _uavcan_parameter_value_sub(-1) { } unsigned MavlinkParametersManager::get_size() { - if (_send_all_index >= 0) { - return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; - - } else { - return 0; - } + return MAVLINK_MSG_ID_PARAM_VALUE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } void @@ -78,36 +80,75 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _send_all_index = 0; } + + if (req_list.target_system == mavlink_system.sysid && req_list.target_component < 127 && + (req_list.target_component != mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) { + // publish list request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_list.target_component; + req.param_index = 0; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } case MAVLINK_MSG_ID_PARAM_SET: { /* set parameter */ - if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { - mavlink_param_set_t set; - mavlink_msg_param_set_decode(msg, &set); + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); - if (set.target_system == mavlink_system.sysid && - (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + if (set.target_system == mavlink_system.sysid && + (set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { - /* local name buffer to enforce null-terminated string */ - char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; - strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); - /* enforce null termination */ - name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; - /* attempt to find parameter, set and send it */ - param_t param = param_find_no_notification(name); + /* local name buffer to enforce null-terminated string */ + char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; + strncpy(name, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + /* attempt to find parameter, set and send it */ + param_t param = param_find_no_notification(name); - if (param == PARAM_INVALID) { - char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; - sprintf(buf, "[pm] unknown param: %s", name); - _mavlink->send_statustext_info(buf); + if (param == PARAM_INVALID) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param: %s", name); + _mavlink->send_statustext_info(buf); - } else { - /* set and send parameter */ - param_set(param, &(set.param_value)); - send_param(param); - } + } else { + /* set and send parameter */ + param_set(param, &(set.param_value)); + send_param(param); + } + } + + if (set.target_system == mavlink_system.sysid && set.target_component < 127 && + (set.target_component != mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = set.target_component; + req.param_index = -1; + strncpy(req.param_id, set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + if (set.param_type == MAV_PARAM_TYPE_REAL32) { + req.param_type = MAV_PARAM_TYPE_REAL32; + req.real_value = set.param_value; + } else { + int32_t val; + memcpy(&val, &set.param_value, sizeof(int32_t)); + req.param_type = MAV_PARAM_TYPE_INT64; + req.int_value = val; + } + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); } } break; @@ -160,6 +201,23 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } } } + + if (req_read.target_system == mavlink_system.sysid && req_read.target_component < 127 && + (req_read.target_component != mavlink_system.compid || req_read.target_component == MAV_COMP_ID_ALL)) { + // publish set request to UAVCAN driver via uORB. + uavcan_parameter_request_s req; + req.message_type = msg->msgid; + req.node_id = req_read.target_component; + req.param_index = req_read.param_index; + strncpy(req.param_id, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1); + req.param_id[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; + + if (_uavcan_parameter_request_pub == nullptr) { + _uavcan_parameter_request_pub = orb_advertise(ORB_ID(uavcan_parameter_request), &req); + } else { + orb_publish(ORB_ID(uavcan_parameter_request), _uavcan_parameter_request_pub, &req); + } + } break; } @@ -208,11 +266,38 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) void MavlinkParametersManager::send(const hrt_abstime t) { - /* send all parameters if requested, but only after the system has booted */ - if (_send_all_index >= 0 && _mavlink->boot_complete()) { + bool space_available = _mavlink->get_free_tx_buf() >= get_size(); + + /* Send parameter values received from the UAVCAN topic */ + if (_uavcan_parameter_value_sub < 0) { + _uavcan_parameter_value_sub = orb_subscribe(ORB_ID(uavcan_parameter_value)); + } + + bool param_value_ready; + orb_check(_uavcan_parameter_value_sub, ¶m_value_ready); + if (space_available && param_value_ready) { + struct uavcan_parameter_value_s value; + orb_copy(ORB_ID(uavcan_parameter_value), _uavcan_parameter_value_sub, &value); + + mavlink_param_value_t msg; + msg.param_count = value.param_count; + msg.param_index = value.param_index; + strncpy(msg.param_id, value.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + if (value.param_type == MAV_PARAM_TYPE_REAL32) { + msg.param_type = MAVLINK_TYPE_FLOAT; + msg.param_value = value.real_value; + } else { + int32_t val; + val = (int32_t)value.int_value; + memcpy(&msg.param_value, &val, sizeof(int32_t)); + msg.param_type = MAVLINK_TYPE_INT32_T; + } + _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg, value.node_id); + } else if (_send_all_index >= 0 && _mavlink->boot_complete()) { + /* send all parameters if requested, but only after the system has booted */ /* skip if no space is available */ - if (_mavlink->get_free_tx_buf() < get_size()) { + if (!space_available) { return; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 3dfed084b3..d258f3b240 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -118,4 +118,7 @@ protected: orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; + + orb_advert_t _uavcan_parameter_request_pub; + int _uavcan_parameter_value_sub; }; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 88123b3e45..d0b95b7f89 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1444,12 +1444,12 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2; hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2; hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2; - hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt; - hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt; - hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f; + hil_sensors.accelerometer_m_s2[0] = imu.xacc; + hil_sensors.accelerometer_m_s2[1] = imu.yacc; + hil_sensors.accelerometer_m_s2[2] = imu.zacc; hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f; hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f; + hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f; memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel)); hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk deleted file mode 100644 index e104860937..0000000000 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ /dev/null @@ -1,53 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 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. -# -############################################################################ - -# -# System state machine tests. -# - -MODULE_COMMAND = mavlink_tests -SRCS = mavlink_tests.cpp \ - mavlink_ftp_test.cpp \ - ../mavlink_stream.cpp \ - ../mavlink_ftp.cpp \ - ../mavlink.c - -INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink - -MODULE_STACKSIZE = 5000 - -MAXOPTIMIZATION = -Os - -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed - -EXTRACFLAGS = -Wno-packed diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt index 177f172755..d8399ccafd 100644 --- a/src/modules/navigator/CMakeLists.txt +++ b/src/modules/navigator/CMakeLists.txt @@ -39,24 +39,17 @@ px4_add_module( -Os SRCS navigator_main.cpp - navigator_params.c navigator_mode.cpp mission_block.cpp mission.cpp - mission_params.c loiter.cpp rtl.cpp - rtl_params.c mission_feasibility_checker.cpp geofence.cpp - geofence_params.c datalinkloss.cpp - datalinkloss_params.c rcloss.cpp - rcloss_params.c enginefailure.cpp gpsfailure.cpp - gpsfailure_params.c DEPENDS platforms__common ) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index a1ba7869ed..9eb0299ab7 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Data Link Loss parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 7c1d8a266a..63eec7213d 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * Geofence parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c index ca2f540505..6c835d062e 100644 --- a/src/modules/navigator/gpsfailure_params.c +++ b/src/modules/navigator/gpsfailure_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * GPS Failure Navigation Mode parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index a31aa25805..880d4bd98e 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI } else { /* item is too far from home */ - mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp); warning_issued = true; return false; } diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index b2094f62e3..7c2942f53e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include - -#include - /* * Mission parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index e1d29334c6..17e580c664 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -40,10 +40,6 @@ * @author Thomas Gubler */ -#include - -#include - /** * Loiter radius (FW only) * diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 3c9a6e40ef..958dab9266 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -39,10 +39,6 @@ * @author Thomas Gubler */ -#include - -#include - /* * OBC RC Loss mode parameters, accessible via MAVLink */ diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index a33ded28ab..e65a41b775 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -39,10 +39,6 @@ * @author Julian Oes */ -#include - -#include - /* * RTL parameters, accessible via MAVLink */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index df8773b169..53a7c678f6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -897,7 +897,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; - dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms + dt = fmaxf(fminf(0.02, dt), 0.0002); // constrain dt from 0.2 to 20 ms t_prev = t; /* increase EPH/EPV on each step */ diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c index 9e25dd2f23..2768994dde 100644 --- a/src/modules/sdlog2/logbuffer.c +++ b/src/modules/sdlog2/logbuffer.c @@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size) lb->size = size; lb->write_ptr = 0; lb->read_ptr = 0; - lb->data = malloc(lb->size); - return (lb->data == 0) ? PX4_ERROR : PX4_OK; + lb->data = NULL; + return PX4_OK; } int logbuffer_count(struct logbuffer_s *lb) @@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb) bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size) { + // allocate buffer if not yet present + if (lb->data == NULL) { + lb->data = malloc(lb->size); + } + + // allocation failed, bail out + if (lb->data == NULL) { + return false; + } + // bytes available to write int available = lb->read_ptr - lb->write_ptr - 1; diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index fdfc2dd774..e296ebe103 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( -O3 SRCS sensors.cpp - sensor_params.c DEPENDS platforms__common ) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 1db6ed1b77..393cd63a06 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f); +/** + * Primary accel ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0); + +/** + * Primary gyro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0); + +/** + * Primary mag ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0); + +/** + * Primary baro ID + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0); + /** * Differential pressure sensor offset * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 60e1a9aabb..c14afb322f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced) (void)sprintf(str, "CAL_MAG%u_ID", i); int device_id; failed = failed || (OK != param_get(param_find(str), &device_id)); + (void)sprintf(str, "CAL_MAG%u_ROT", i); + (void)param_find(str); if (failed) { px4_close(fd); @@ -2020,6 +2022,11 @@ Sensors::task_main() * do subscriptions */ + unsigned gcount_prev = _gyro_count; + unsigned mcount_prev = _mag_count; + unsigned acount_prev = _accel_count; + unsigned bcount_prev = _baro_count; + _gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0], &raw.gyro_priority[0], &raw.gyro_errcount[0]); @@ -2032,6 +2039,15 @@ Sensors::task_main() _baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0], &raw.baro_priority[0], &raw.baro_errcount[0]); + if (gcount_prev != _gyro_count || + mcount_prev != _mag_count || + acount_prev != _accel_count || + bcount_prev != _baro_count) { + + /* reload calibration params */ + parameter_update_poll(true); + } + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); diff --git a/src/modules/uavcan/CMakeLists.txt b/src/modules/uavcan/CMakeLists.txt index 3c7ad32998..b8c899840a 100644 --- a/src/modules/uavcan/CMakeLists.txt +++ b/src/modules/uavcan/CMakeLists.txt @@ -63,7 +63,7 @@ px4_add_module( COMPILE_FLAGS -Wframe-larger-than=1500 -Wno-deprecated-declarations - -O3 + -Os SRCS # Main uavcan_main.cpp @@ -81,7 +81,7 @@ px4_add_module( DEPENDS platforms__common - uavcan + uavcan ) ## vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 2b5e01e246..6a1d2f391d 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -94,7 +94,7 @@ class UavcanNode : public device::CDev */ static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At - static constexpr unsigned StackSize = 1600; + static constexpr unsigned StackSize = 1800; public: typedef uavcan::Node Node; diff --git a/src/modules/uavcan/uavcan_servers.cpp b/src/modules/uavcan/uavcan_servers.cpp index 451c5bbd81..713a4e00f7 100644 --- a/src/modules/uavcan/uavcan_servers.cpp +++ b/src/modules/uavcan/uavcan_servers.cpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -53,14 +54,22 @@ #include "uavcan_servers.hpp" #include "uavcan_virtual_can_driver.hpp" -# include -# include -# include +#include +#include +#include + +#include +#include +#include + +#include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined # define OK 0 + + /** * @file uavcan_servers.cpp * @@ -87,8 +96,29 @@ UavcanServers::UavcanServers(uavcan::INode &main_node) : _node_info_retriever(_subnode), _fw_upgrade_trigger(_subnode, _fw_version_checker), _fw_server(_subnode, _fileserver_backend), + _count_in_progress(false), + _count_index(0), + _param_in_progress(0), + _param_index(0), + _param_list_in_progress(false), + _param_list_all_nodes(false), + _param_list_node_id(1), + _param_dirty_bitmap{0, 0, 0, 0}, + _param_save_opcode(0), + _cmd_in_progress(false), + _param_response_pub(nullptr), + _param_getset_client(_subnode), + _param_opcode_client(_subnode), + _param_restartnode_client(_subnode), _mutex_inited(false), - _check_fw(false) + _check_fw(false), + _esc_enumeration_active(false), + _esc_enumeration_index(0), + _beep_pub(_subnode), + _enumeration_indication_sub(_subnode), + _enumeration_client(_subnode), + _enumeration_getset_client(_subnode), + _enumeration_save_client(_subnode) { } @@ -270,13 +300,40 @@ int UavcanServers::init() return OK; } -__attribute__((optimize("-O0"))) pthread_addr_t UavcanServers::run(pthread_addr_t) { prctl(PR_SET_NAME, "uavcan fw srv", 0); Lock lock(_subnode_mutex); + /* + Copy any firmware bundled in the ROMFS to the appropriate location on the + SD card, unless the user has copied other firmware for that device. + */ + unpackFwFromROMFS(UAVCAN_FIRMWARE_PATH, UAVCAN_ROMFS_FW_PATH); + + /* the subscribe call needs to happen in the same thread, + * so not in the constructor */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + int param_request_sub = orb_subscribe(ORB_ID(uavcan_parameter_request)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + + /* Set up shared service clients */ + _param_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_getset)); + _param_opcode_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_opcode)); + _param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanServers::cb_restart)); + _enumeration_client.setCallback(EnumerationBeginCallback(this, &UavcanServers::cb_enumeration_begin)); + _enumeration_indication_sub.start(EnumerationIndicationCallback(this, &UavcanServers::cb_enumeration_indication)); + _enumeration_getset_client.setCallback(GetSetCallback(this, &UavcanServers::cb_enumeration_getset)); + _enumeration_save_client.setCallback(ExecuteOpcodeCallback(this, &UavcanServers::cb_enumeration_save)); + + _count_in_progress = _param_in_progress = _param_list_in_progress = _cmd_in_progress = _param_list_all_nodes = false; + memset(_param_counts, 0, sizeof(_param_counts)); + + _esc_enumeration_active = false; + memset(_esc_enumeration_ids, 0, sizeof(_esc_enumeration_ids)); + _esc_enumeration_index = 0; + while (1) { if (_check_fw == true) { @@ -284,14 +341,799 @@ pthread_addr_t UavcanServers::run(pthread_addr_t) _node_info_retriever.invalidateAll(); } - const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(100)); - + const int spin_res = _subnode.spin(uavcan::MonotonicDuration::fromMSec(10)); if (spin_res < 0) { warnx("node spin error %i", spin_res); } + + // Check for parameter requests (get/set/list) + bool param_request_ready; + orb_check(param_request_sub, ¶m_request_ready); + + if (param_request_ready && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + struct uavcan_parameter_request_s request; + orb_copy(ORB_ID(uavcan_parameter_request), param_request_sub, &request); + + if (_param_counts[request.node_id]) { + /* + * We know how many parameters are exposed by this node, so + * process the request. + */ + if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_READ) { + uavcan::protocol::param::GetSet::Request req; + if (request.param_index >= 0) { + req.index = request.param_index; + } else { + req.name = (char*)request.param_id; + } + + int call_res = _param_getset_client.call(request.node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } else { + _param_in_progress = true; + _param_index = request.param_index; + } + } else if (request.message_type == MAVLINK_MSG_ID_PARAM_SET) { + uavcan::protocol::param::GetSet::Request req; + if (request.param_index >= 0) { + req.index = request.param_index; + } else { + req.name = (char*)request.param_id; + } + + if (request.param_type == MAV_PARAM_TYPE_REAL32) { + req.value.to() = request.real_value; + } else if (request.param_type == MAV_PARAM_TYPE_UINT8) { + req.value.to() = request.int_value; + } else { + req.value.to() = request.int_value; + } + + // Set the dirty bit for this node + set_node_params_dirty(request.node_id); + + int call_res = _param_getset_client.call(request.node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send GetSet: %d", call_res); + } else { + _param_in_progress = true; + _param_index = request.param_index; + } + } else if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + // This triggers the _param_list_in_progress case below. + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = request.node_id; + _param_list_all_nodes = false; + + warnx("UAVCAN command bridge: starting component-specific param list"); + } + } else if (request.node_id == MAV_COMP_ID_ALL) { + if (request.message_type == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { + /* + * This triggers the _param_list_in_progress case below, + * but additionally iterates over all active nodes. + */ + _param_index = 0; + _param_list_in_progress = true; + _param_list_node_id = get_next_active_node_id(1); + _param_list_all_nodes = true; + + warnx("UAVCAN command bridge: starting global param list with node %hhu", _param_list_node_id); + + if (_param_counts[_param_list_node_id] == 0) { + param_count(_param_list_node_id); + } + } + } else { + /* + * Need to know how many parameters this node has before we can + * continue; count them now and then process the request. + */ + param_count(request.node_id); + } + } + + // Handle parameter listing index/node ID advancement + if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + if (_param_index >= _param_counts[_param_list_node_id]) { + warnx("UAVCAN command bridge: completed param list for node %hhu", _param_list_node_id); + // Reached the end of the current node's parameter set. + _param_list_in_progress = false; + + if (_param_list_all_nodes) { + // We're listing all parameters for all nodes -- get the next node ID + uint8_t next_id = get_next_active_node_id(_param_list_node_id); + if (next_id < 128) { + _param_list_node_id = next_id; + /* + * If there is a next node ID, check if that node's parameters + * have been counted before. If not, do it now. + */ + if (_param_counts[_param_list_node_id] == 0) { + param_count(_param_list_node_id); + } + // Keep on listing. + _param_index = 0; + _param_list_in_progress = true; + warnx("UAVCAN command bridge: started param list for node %hhu", _param_list_node_id); + } + } + } + } + + // Check if we're still listing, and need to get the next parameter + if (_param_list_in_progress && !_param_in_progress && !_count_in_progress) { + // Ready to request the next value -- _param_index is incremented + // after each successful fetch by cb_getset + uavcan::protocol::param::GetSet::Request req; + req.index = _param_index; + + int call_res = _param_getset_client.call(_param_list_node_id, req); + if (call_res < 0) { + _param_list_in_progress = false; + warnx("UAVCAN command bridge: couldn't send param list GetSet: %d", call_res); + } else { + _param_in_progress = true; + } + } + + // Check for ESC enumeration commands + bool cmd_ready; + orb_check(cmd_sub, &cmd_ready); + + if (cmd_ready && !_cmd_in_progress) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_UAVCAN) { + int command_id = static_cast(cmd.param1 + 0.5f); + int node_id = static_cast(cmd.param2 + 0.5f); + int call_res; + + warnx("UAVCAN command bridge: received UAVCAN command ID %d, node ID %d", command_id, node_id); + + switch (command_id) { + case 0: + case 1: { + _esc_enumeration_active = command_id; + _esc_enumeration_index = 0; + _esc_count = 0; + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = _esc_enumeration_active ? 65535 : 0; + call_res = _enumeration_client.call(get_next_active_node_id(1), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send initial Begin request: %d", call_res); + } + break; + } + default: { + warnx("UAVCAN command bridge: unknown command ID %d", command_id); + break; + } + } + } else if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) { + int command_id = static_cast(cmd.param1 + 0.5f); + + warnx("UAVCAN command bridge: received storage command ID %d", command_id); + + switch (command_id) { + case 1: { + // Param save request + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE; + param_opcode(get_next_dirty_node_id(1)); + break; + } + case 2: { + // Command is a param erase request -- apply it to all active nodes by setting the dirty bit + _param_save_opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE; + for (int i = 1; i < 128; i = get_next_active_node_id(i)) { + set_node_params_dirty(i); + } + param_opcode(get_next_dirty_node_id(1)); + break; + } + } + } + } + + // Shut down once armed + // TODO (elsewhere): start up again once disarmed? + bool updated; + orb_check(armed_sub, &updated); + if (updated) { + struct actuator_armed_s armed; + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + + if (armed.armed && !armed.lockdown) { + warnx("UAVCAN command bridge: system armed, exiting now."); + break; + } + } } warnx("exiting."); return (pthread_addr_t) 0; } +void UavcanServers::cb_getset(const uavcan::ServiceCallResult &result) +{ + if (_count_in_progress) { + /* + * Currently in parameter count mode: + * Iterate over all parameters for the node to which the request was + * originally sent, in order to find the maximum parameter ID. If a + * request fails, set the node's parameter count to zero. + */ + uint8_t node_id = result.getCallID().server_node_id.get(); + + if (result.isSuccessful()) { + uavcan::protocol::param::GetSet::Response resp = result.getResponse(); + if (resp.name.size()) { + _param_counts[node_id] = _count_index++; + + uavcan::protocol::param::GetSet::Request req; + req.index = _count_index; + + int call_res = _param_getset_client.call(result.getCallID().server_node_id, req); + if (call_res < 0) { + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: couldn't send GetSet during param count: %d", call_res); + } + } else { + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: completed param count for node %hhu: %hhu", node_id, _param_counts[node_id]); + } + } else { + _param_counts[node_id] = 0; + _count_in_progress = false; + _count_index = 0; + warnx("UAVCAN command bridge: GetSet error during param count"); + } + } else { + /* + * Currently in parameter get/set mode: + * Publish a uORB uavcan_parameter_value message containing the current value + * of the parameter. + */ + if (result.isSuccessful()) { + uavcan::protocol::param::GetSet::Response param = result.getResponse(); + + struct uavcan_parameter_value_s response; + response.node_id = result.getCallID().server_node_id.get(); + strncpy(response.param_id, param.name.c_str(), sizeof(response.param_id) - 1); + response.param_id[16] = '\0'; + response.param_index = _param_index; + response.param_count = _param_counts[response.node_id]; + + if (param.value.is(uavcan::protocol::param::Value::Tag::integer_value)) { + response.param_type = MAV_PARAM_TYPE_INT64; + response.int_value = param.value.to(); + } else if (param.value.is(uavcan::protocol::param::Value::Tag::real_value)) { + response.param_type = MAV_PARAM_TYPE_REAL32; + response.real_value = param.value.to(); + } else if (param.value.is(uavcan::protocol::param::Value::Tag::boolean_value)) { + response.param_type = MAV_PARAM_TYPE_UINT8; + response.int_value = param.value.to(); + } + + if (_param_response_pub == nullptr) { + _param_response_pub = orb_advertise(ORB_ID(uavcan_parameter_value), &response); + } else { + orb_publish(ORB_ID(uavcan_parameter_value), _param_response_pub, &response); + } + } else { + warnx("UAVCAN command bridge: GetSet error"); + } + + _param_in_progress = false; + _param_index++; + } +} + +void UavcanServers::param_count(uavcan::NodeID node_id) +{ + uavcan::protocol::param::GetSet::Request req; + req.index = 0; + int call_res = _param_getset_client.call(node_id, req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't start parameter count: %d", call_res); + } else { + _count_in_progress = true; + _count_index = 0; + warnx("UAVCAN command bridge: starting param count"); + } +} + +void UavcanServers::param_opcode(uavcan::NodeID node_id) +{ + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = _param_save_opcode; + int call_res = _param_opcode_client.call(node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send ExecuteOpcode: %d", call_res); + } else { + _cmd_in_progress = true; + warnx("UAVCAN command bridge: sent ExecuteOpcode"); + } +} + +void UavcanServers::cb_opcode(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uint8_t node_id = result.getCallID().server_node_id.get(); + uavcan::protocol::param::ExecuteOpcode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; + + if (!result.isSuccessful()) { + warnx("UAVCAN command bridge: save request for node %hhu timed out.", node_id); + } else if (!result.getResponse().ok) { + warnx("UAVCAN command bridge: save request for node %hhu rejected.", node_id); + } else { + warnx("UAVCAN command bridge: save request for node %hhu completed OK, restarting.", node_id); + + uavcan::protocol::RestartNode::Request restart_req; + restart_req.magic_number = restart_req.MAGIC_NUMBER; + int call_res = _param_restartnode_client.call(node_id, restart_req); + if (call_res < 0) { + warnx("UAVCAN command bridge: couldn't send RestartNode: %d", call_res); + } else { + warnx("UAVCAN command bridge: sent RestartNode"); + _cmd_in_progress = true; + } + } + + if (!_cmd_in_progress) { + /* + * Something went wrong, so cb_restart is never going to be called as a result of this request. + * To ensure we try to execute the opcode on all nodes that permit it, get the next dirty node + * ID and keep processing here. The dirty bit on the current node is still set, so the + * save/erase attempt will occur when the next save/erase command is received over MAVLink. + */ + node_id = get_next_dirty_node_id(node_id); + if (node_id < 128) { + param_opcode(node_id); + } + } +} + +void UavcanServers::cb_restart(const uavcan::ServiceCallResult &result) +{ + bool success = result.isSuccessful(); + uint8_t node_id = result.getCallID().server_node_id.get(); + uavcan::protocol::RestartNode::Response resp = result.getResponse(); + success &= resp.ok; + _cmd_in_progress = false; + + if (success) { + warnx("UAVCAN command bridge: restart request for node %hhu completed OK.", node_id); + // Clear the dirty flag + clear_node_params_dirty(node_id); + } else { + warnx("UAVCAN command bridge: restart request for node %hhu failed.", node_id); + } + + // Get the next dirty node ID and send the same command to it + node_id = get_next_dirty_node_id(node_id); + if (node_id < 128) { + param_opcode(node_id); + } +} + +uint8_t UavcanServers::get_next_active_node_id(uint8_t base) +{ + base++; + for (; base < 128 && (!_node_info_retriever.isNodeKnown(base) || + _subnode.getNodeID().get() == base); base++); + return base; +} + +uint8_t UavcanServers::get_next_dirty_node_id(uint8_t base) +{ + base++; + for (; base < 128 && !are_node_params_dirty(base); base++); + return base; +} + +void UavcanServers::cb_enumeration_begin(const uavcan::ServiceCallResult &result) +{ + uint8_t next_id = get_next_active_node_id(result.getCallID().server_node_id.get()); + + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: begin request for node %hhu timed out.", result.getCallID().server_node_id.get()); + } else if (result.getResponse().error) { + warnx("UAVCAN ESC enumeration: begin request for node %hhu rejected: %hhu", result.getCallID().server_node_id.get(), result.getResponse().error); + } else { + _esc_count++; + warnx("UAVCAN ESC enumeration: begin request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + } + + if (next_id < 128) { + // Still other active nodes to send the request to + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = _esc_enumeration_active ? 65535 : 0; + + int call_res = _enumeration_client.call(next_id, req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send Begin request: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent Begin request"); + } + } else { + warnx("UAVCAN ESC enumeration: begun enumeration on all nodes."); + } +} + +void UavcanServers::cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg) +{ + // Called whenever an ESC thinks it has received user input. + warnx("UAVCAN ESC enumeration: got indication"); + + if (!_esc_enumeration_active) { + // Ignore any messages received when we're not expecting them + return; + } + + // First, check if we've already seen an indication from this ESC. If so, + // just re-issue the previous get/set request. + int i; + for (i = 0; i < _esc_enumeration_index; i++) { + if (_esc_enumeration_ids[i] == msg.getSrcNodeID().get()) { + warnx("UAVCAN ESC enumeration: already enumerated ESC ID %hhu as index %d", _esc_enumeration_ids[i], i); + break; + } + } + + uavcan::protocol::param::GetSet::Request req; + req.name = "esc_index"; + req.value.to() = i; + + int call_res = _enumeration_getset_client.call(msg.getSrcNodeID(), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send GetSet: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent GetSet to node %hhu (index %d)", _esc_enumeration_ids[i], i); + } +} + +void UavcanServers::cb_enumeration_getset(const uavcan::ServiceCallResult &result) +{ + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + } else { + warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + + uavcan::protocol::param::GetSet::Response resp = result.getResponse(); + uint8_t esc_index = (uint8_t)resp.value.to(); + esc_index = std::min((uint8_t)(uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1), esc_index); + _esc_enumeration_index = std::max(_esc_enumeration_index, (uint8_t)(esc_index + 1)); + + _esc_enumeration_ids[esc_index] = result.getCallID().server_node_id.get(); + + uavcan::protocol::param::ExecuteOpcode::Request opcode_req; + opcode_req.opcode = opcode_req.OPCODE_SAVE; + int call_res = _enumeration_save_client.call(result.getCallID().server_node_id, opcode_req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send ExecuteOpcode: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent ExecuteOpcode to node %hhu (index %hhu)", _esc_enumeration_ids[esc_index], esc_index); + } + } +} + +void UavcanServers::cb_enumeration_save(const uavcan::ServiceCallResult &result) +{ + uavcan::equipment::indication::BeepCommand beep; + + if (!result.isSuccessful()) { + warnx("UAVCAN ESC enumeration: save request for node %hhu timed out.", result.getCallID().server_node_id.get()); + beep.frequency = 880.0f; + beep.duration = 1.0f; + } else if (!result.getResponse().ok) { + warnx("UAVCAN ESC enumeration: save request for node %hhu rejected", result.getCallID().server_node_id.get()); + beep.frequency = 880.0f; + beep.duration = 1.0f; + } else { + warnx("UAVCAN ESC enumeration: save request for node %hhu completed OK.", result.getCallID().server_node_id.get()); + beep.frequency = 440.0f; + beep.duration = 0.25f; + } + + (void)_beep_pub.broadcast(beep); + + warnx("UAVCAN ESC enumeration: completed %hhu of %hhu", _esc_enumeration_index, _esc_count); + + if (_esc_enumeration_index == uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize - 1 || + _esc_enumeration_index == _esc_count) { + _esc_enumeration_active = false; + + // Tell all ESCs to stop enumerating + uavcan::protocol::enumeration::Begin::Request req; + req.parameter_name = "esc_index"; + req.timeout_sec = 0; + int call_res = _enumeration_client.call(get_next_active_node_id(1), req); + if (call_res < 0) { + warnx("UAVCAN ESC enumeration: couldn't send Begin request to stop enumeration: %d", call_res); + } else { + warnx("UAVCAN ESC enumeration: sent Begin request to stop enumeration"); + } + } +} + +void UavcanServers::unpackFwFromROMFS(const char* sd_path, const char* romfs_path) { + /* + Copy the ROMFS firmware directory to the appropriate location on SD, without + overriding any firmware the user has already loaded there. + + The SD firmware directory structure is along the lines of: + + /fs/microsd/fw + - /c + - - /1a2b3c4d.bin cache file (copy of /fs/microsd/fw/org.pixhawk.nodename-v1/1.1/1a2b3c4d.bin) + - /org.pixhawk.nodename-v1 device directory for org.pixhawk.nodename-v1 + - - /1.0 version directory for hardware 1.0 + - - /1.1 version directory for hardware 1.1 + - - - /1a2b3c4d.bin firmware file for org.pixhawk.nodename-v1 nodes, hardware version 1.1 + - /com.example.othernode-v3 device directory for com.example.othernode-v3 + - - /1.0 version directory for hardawre 1.0 + - - - /deadbeef.bin firmware file for com.example.othernode-v3, hardware version 1.0 + + The ROMFS directory structure is the same, but located at /etc/uavcan/fw + + We iterate over all device directories in the ROMFS base directory, and create + corresponding device directories on the SD card if they don't already exist. + + In each device directory, we iterate over each version directory and create a + corresponding version directory on the SD card if it doesn't already exist. + + In each version directory, we remove any files with a name starting with "romfs_" + in the corresponding directory on the SD card that don't match the bundled firmware + filename; if the directory is empty after that process, we copy the bundled firmware. + */ + const size_t maxlen = UAVCAN_MAX_PATH_LENGTH; + const size_t sd_path_len = strlen(sd_path); + const size_t romfs_path_len = strlen(romfs_path); + struct stat sb; + int rv; + char dstpath[maxlen + 1]; + char srcpath[maxlen + 1]; + + DIR* const romfs_dir = opendir(romfs_path); + if (!romfs_dir) { + warnx("base: couldn't open %s", romfs_path); + return; + } + + memcpy(dstpath, sd_path, sd_path_len + 1); + memcpy(srcpath, romfs_path, romfs_path_len + 1); + + // Iterate over all device directories in ROMFS + struct dirent* dev_dirent = NULL; + while ((dev_dirent = readdir(romfs_dir)) != NULL) { + // Skip if not a directory + if (!DIRENT_ISDIRECTORY(dev_dirent->d_type)) { + continue; + } + + // Make sure the path fits + size_t dev_dirname_len = strlen(dev_dirent->d_name); + size_t srcpath_dev_len = romfs_path_len + 1 + dev_dirname_len; + if (srcpath_dev_len > maxlen) { + warnx("dev: srcpath '%s/%s' too long", romfs_path, dev_dirent->d_name); + continue; + } + size_t dstpath_dev_len = sd_path_len + 1 + dev_dirname_len; + if (dstpath_dev_len > maxlen) { + warnx("dev: dstpath '%s/%s' too long", sd_path, dev_dirent->d_name); + continue; + } + + // Create the device name directory on the SD card if it doesn't already exist + dstpath[sd_path_len] = '/'; + memcpy(&dstpath[sd_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1); + + if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) { + rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); + if (rv != 0) { + warnx("dev: couldn't create '%s'", dstpath); + continue; + } + } + + // Set up the source path + srcpath[romfs_path_len] = '/'; + memcpy(&srcpath[romfs_path_len + 1], dev_dirent->d_name, dev_dirname_len + 1); + + DIR* const dev_dir = opendir(srcpath); + if (!dev_dir) { + warnx("dev: couldn't open '%s'", srcpath); + continue; + } + + // Iterate over all version directories in the current ROMFS device directory + struct dirent* ver_dirent = NULL; + while ((ver_dirent = readdir(dev_dir)) != NULL) { + // Skip if not a directory + if (!DIRENT_ISDIRECTORY(ver_dirent->d_type)) { + continue; + } + + // Make sure the path fits + size_t ver_dirname_len = strlen(ver_dirent->d_name); + size_t srcpath_ver_len = srcpath_dev_len + 1 + ver_dirname_len; + if (srcpath_ver_len > maxlen) { + warnx("ver: srcpath '%s/%s' too long", srcpath, ver_dirent->d_name); + continue; + } + size_t dstpath_ver_len = dstpath_dev_len + 1 + ver_dirname_len; + if (dstpath_ver_len > maxlen) { + warnx("ver: dstpath '%s/%s' too long", dstpath, ver_dirent->d_name); + continue; + } + + // Create the device version directory on the SD card if it doesn't already exist + dstpath[dstpath_dev_len] = '/'; + memcpy(&dstpath[dstpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1); + + if (stat(dstpath, &sb) != 0 || !S_ISDIR(sb.st_mode)) { + rv = mkdir(dstpath, S_IRWXU | S_IRWXG | S_IRWXO); + if (rv != 0) { + warnx("ver: couldn't create '%s'", dstpath); + continue; + } + } + + // Set up the source path + srcpath[srcpath_dev_len] = '/'; + memcpy(&srcpath[srcpath_dev_len + 1], ver_dirent->d_name, ver_dirname_len + 1); + + // Find the name of the bundled firmware file, or move on to the + // next directory if there's no file here. + DIR* const src_ver_dir = opendir(srcpath); + if (!src_ver_dir) { + warnx("ver: couldn't open '%s'", srcpath); + continue; + } + + struct dirent* src_fw_dirent = NULL; + while ((src_fw_dirent = readdir(src_ver_dir)) != NULL && + !DIRENT_ISFILE(src_fw_dirent->d_type)); + + if (!src_fw_dirent) { + (void)closedir(src_ver_dir); + continue; + } + + size_t fw_len = strlen(src_fw_dirent->d_name); + + bool copy_fw = true; + + // Clear out any romfs_ files in the version directory on the SD card + DIR* const dst_ver_dir = opendir(dstpath); + if (!dst_ver_dir) { + warnx("unlink: couldn't open '%s'", dstpath); + } else { + struct dirent* fw_dirent = NULL; + while ((fw_dirent = readdir(dst_ver_dir)) != NULL) { + // Skip if not a file + if (!DIRENT_ISFILE(fw_dirent->d_type)) { + continue; + } + + if (!memcmp(&fw_dirent->d_name[sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1], src_fw_dirent->d_name, fw_len)) { + /* + * Exact match between SD card filename and ROMFS filename; must be the same version + * so don't bother deleting and rewriting it. + */ + copy_fw = false; + } else if (!memcmp(fw_dirent->d_name, UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX) - 1)) { + size_t fw_len = strlen(fw_dirent->d_name); + size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + if (dstpath_fw_len > maxlen) { + // sizeof(prefix) includes trailing NUL, cancelling out the +1 for the path separator + warnx("unlink: path '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + } else { + // File name starts with "romfs_", delete it. + dstpath[dstpath_ver_len] = '/'; + memcpy(&dstpath[dstpath_ver_len + 1], fw_dirent->d_name, fw_len + 1); + unlink(dstpath); + + warnx("unlink: removed '%s/%s%s'", dstpath, UAVCAN_ROMFS_FW_PREFIX, fw_dirent->d_name); + } + } else { + // User file, don't copy firmware + copy_fw = false; + } + } + (void)closedir(dst_ver_dir); + } + + // If we need to, copy the file from ROMFS to the SD card + if (copy_fw) { + size_t srcpath_fw_len = srcpath_ver_len + 1 + fw_len; + size_t dstpath_fw_len = dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX) + fw_len; + + if (srcpath_fw_len > maxlen) { + warnx("copy: srcpath '%s/%s' too long", srcpath, src_fw_dirent->d_name); + } else if (dstpath_fw_len > maxlen) { + warnx("copy: dstpath '%s/%s%s' too long", dstpath, UAVCAN_ROMFS_FW_PREFIX, src_fw_dirent->d_name); + } else { + // All OK, make the paths and copy the file + srcpath[srcpath_ver_len] = '/'; + memcpy(&srcpath[srcpath_ver_len + 1], src_fw_dirent->d_name, fw_len + 1); + + dstpath[dstpath_ver_len] = '/'; + memcpy(&dstpath[dstpath_ver_len + 1], UAVCAN_ROMFS_FW_PREFIX, sizeof(UAVCAN_ROMFS_FW_PREFIX)); + memcpy(&dstpath[dstpath_ver_len + sizeof(UAVCAN_ROMFS_FW_PREFIX)], src_fw_dirent->d_name, fw_len + 1); + + rv = copyFw(dstpath, srcpath); + if (rv != 0) { + warnx("copy: '%s' -> '%s' failed: %d", srcpath, dstpath, rv); + } else { + warnx("copy: '%s' -> '%s' succeeded", srcpath, dstpath); + } + } + } + + (void)closedir(src_ver_dir); + } + + (void)closedir(dev_dir); + } + + (void)closedir(romfs_dir); +} + +int UavcanServers::copyFw(const char* dst, const char* src) { + int rv = 0; + int dfd, sfd; + uint8_t buffer[512]; + + dfd = open(dst, O_WRONLY | O_CREAT, 0666); + if (dfd < 0) { + warnx("copyFw: couldn't open dst"); + return -errno; + } + + sfd = open(src, O_RDONLY, 0); + if (sfd < 0) { + (void)close(dfd); + warnx("copyFw: couldn't open src"); + return -errno; + } + + ssize_t size = 0; + do { + size = read(sfd, buffer, sizeof(buffer)); + if (size < 0) { + warnx("copyFw: couldn't read"); + rv = -errno; + } else if (size > 0) { + rv = 0; + ssize_t remaining = size; + ssize_t total_written = 0; + ssize_t written = 0; + do { + written = write(dfd, &buffer[total_written], remaining); + if (written < 0) { + warnx("copyFw: couldn't write"); + rv = -errno; + } else { + total_written += written; + remaining -= written; + } + } while (written > 0 && remaining > 0); + } + } while (rv == 0 && size != 0); + + (void)close(dfd); + (void)close(sfd); + + return rv; +} diff --git a/src/modules/uavcan/uavcan_servers.hpp b/src/modules/uavcan/uavcan_servers.hpp index 9d21b8f93f..0b83baa941 100644 --- a/src/modules/uavcan/uavcan_servers.hpp +++ b/src/modules/uavcan/uavcan_servers.hpp @@ -42,16 +42,20 @@ #include #include -# include -# include -# include -# include -# include -# include -# include -# include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -# include "uavcan_virtual_can_driver.hpp" +#include "uavcan_virtual_can_driver.hpp" /** * @file uavcan_servers.hpp @@ -65,6 +69,9 @@ #define UAVCAN_DEVICE_PATH "/dev/uavcan/esc" #define UAVCAN_NODE_DB_PATH "/fs/microsd/uavcan.db" #define UAVCAN_FIRMWARE_PATH "/fs/microsd/fw" +#define UAVCAN_ROMFS_FW_PATH "/etc/uavcan/fw" +#define UAVCAN_ROMFS_FW_PREFIX "romfs_" +#define UAVCAN_MAX_PATH_LENGTH (128 + 40) #define UAVCAN_LOG_FILE UAVCAN_NODE_DB_PATH"/trace.log" /** @@ -88,7 +95,7 @@ class UavcanServers static constexpr unsigned QueuePoolSize = (NumIfaces * uavcan::MemPoolBlockSize * MaxCanFramesPerTransfer); - static constexpr unsigned StackSize = 3500; + static constexpr unsigned StackSize = 6000; static constexpr unsigned Priority = 120; typedef uavcan::SubNode SubNode; @@ -140,7 +147,84 @@ private: uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::BasicFileServer _fw_server; + /* + * The MAVLink parameter bridge needs to know the maximum parameter index + * of each node so that clients can determine when parameter listings have + * finished. We do that by querying a node's entire parameter set whenever + * a parameter message is received for a node with a zero _param_count, + * and storing the count here. If a node doesn't respond, or doesn't have + * any parameters, its count will stay at zero and we'll try to query the + * set again next time. + * + * The node's UAVCAN ID is used as the index into the _param_counts array. + */ + uint8_t _param_counts[128]; + bool _count_in_progress; + uint8_t _count_index; + + bool _param_in_progress; + uint8_t _param_index; + bool _param_list_in_progress; + bool _param_list_all_nodes; + uint8_t _param_list_node_id; + + uint32_t _param_dirty_bitmap[4]; + uint8_t _param_save_opcode; + + bool _cmd_in_progress; + + // uORB topic handle for MAVLink parameter responses + orb_advert_t _param_response_pub; + + typedef uavcan::MethodBinder &)> GetSetCallback; + typedef uavcan::MethodBinder &)> ExecuteOpcodeCallback; + typedef uavcan::MethodBinder &)> RestartNodeCallback; + void cb_getset(const uavcan::ServiceCallResult &result); + void cb_count(const uavcan::ServiceCallResult &result); + void cb_opcode(const uavcan::ServiceCallResult &result); + void cb_restart(const uavcan::ServiceCallResult &result); + + uavcan::ServiceClient _param_getset_client; + uavcan::ServiceClient _param_opcode_client; + uavcan::ServiceClient _param_restartnode_client; + void param_count(uavcan::NodeID node_id); + void param_opcode(uavcan::NodeID node_id); + + uint8_t get_next_active_node_id(uint8_t base); + uint8_t get_next_dirty_node_id(uint8_t base); + void set_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] |= 1 << (node_id & 31); } + void clear_node_params_dirty(uint8_t node_id) { _param_dirty_bitmap[node_id >> 5] &= ~(1 << (node_id & 31)); } + bool are_node_params_dirty(uint8_t node_id) const { return bool((_param_dirty_bitmap[node_id >> 5] >> (node_id & 31)) & 1); } + bool _mutex_inited; volatile bool _check_fw; + // ESC enumeration + bool _esc_enumeration_active; + uint8_t _esc_enumeration_ids[uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize]; + uint8_t _esc_enumeration_index; + uint8_t _esc_set_index; + uint8_t _esc_count; + + typedef uavcan::MethodBinder &)> EnumerationBeginCallback; + typedef uavcan::MethodBinder&)> + EnumerationIndicationCallback; + void cb_enumeration_begin(const uavcan::ServiceCallResult &result); + void cb_enumeration_indication(const uavcan::ReceivedDataStructure &msg); + void cb_enumeration_getset(const uavcan::ServiceCallResult &result); + void cb_enumeration_save(const uavcan::ServiceCallResult &result); + + uavcan::Publisher _beep_pub; + uavcan::Subscriber _enumeration_indication_sub; + uavcan::ServiceClient _enumeration_client; + uavcan::ServiceClient _enumeration_getset_client; + uavcan::ServiceClient _enumeration_save_client; + + void unpackFwFromROMFS(const char* sd_path, const char* romfs_path); + int copyFw(const char* dst, const char* src); }; diff --git a/src/platforms/px4_log.h b/src/platforms/px4_log.h index 5b7a65eeb6..55bf3a55bc 100644 --- a/src/platforms/px4_log.h +++ b/src/platforms/px4_log.h @@ -130,7 +130,7 @@ __EXPORT extern int __px4_log_level_current; __END_DECLS // __px4_log_level_current will be initialized to PX4_LOG_LEVEL_AT_RUN_TIME -#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_WARN +#define PX4_LOG_LEVEL_AT_RUN_TIME _PX4_LOG_LEVEL_ERROR /**************************************************************************** * Implementation of log section formatting based on printf @@ -357,8 +357,8 @@ __END_DECLS /**************************************************************************** * Non-verbose settings for a Release build to minimize strings in build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) #define PX4_WARN(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) @@ -366,9 +366,9 @@ __END_DECLS /**************************************************************************** * Medium verbose settings for a default build ****************************************************************************/ -#define PX4_PANIC(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) -#define PX4_ERR(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) -#define PX4_WARN(FMT, ...) __px4_log_file_and_line(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) +#define PX4_PANIC(FMT, ...) __px4_log(_PX4_LOG_LEVEL_PANIC, FMT, ##__VA_ARGS__) +#define PX4_ERR(FMT, ...) __px4_log(_PX4_LOG_LEVEL_ERROR, FMT, ##__VA_ARGS__) +#define PX4_WARN(FMT, ...) __px4_log(_PX4_LOG_LEVEL_WARN, FMT, ##__VA_ARGS__) #define PX4_DEBUG(FMT, ...) __px4_log_omit(_PX4_LOG_LEVEL_DEBUG, FMT, ##__VA_ARGS__) #endif