From 018ca6b49d2dd6fe2327c4d65a96021885ba2748 Mon Sep 17 00:00:00 2001 From: Eric Katzfey <53063038+katzfey@users.noreply.github.com> Date: Wed, 19 Apr 2023 08:21:02 -0700 Subject: [PATCH] VOXL2 board support updates (#21426) --- boards/modalai/voxl2-slpi/default.px4board | 36 ++-- boards/modalai/voxl2-slpi/src/board_config.h | 2 +- .../drivers/mavlink_rc_in/mavlink_rc_in.cpp | 30 ++- boards/modalai/voxl2-slpi/src/i2c.cpp | 1 + boards/modalai/voxl2/default.px4board | 36 ++-- boards/modalai/voxl2/scripts/install-voxl.sh | 116 ++++++++++- boards/modalai/voxl2/src/board_config.h | 2 + boards/modalai/voxl2/target/voxl-px4 | 140 ++++++++++++-- .../voxl-px4-fake-imu-calibration.config | 37 ++++ .../voxl-px4-set-default-parameters.config | 182 ++++++++++++++++++ boards/modalai/voxl2/target/voxl-px4-start | 164 ++++++++++++++++ boards/modalai/voxl2/target/voxl-px4.config | 21 -- platforms/qurt/src/px4/tasks.cpp | 15 +- platforms/qurt/unresolved_symbols.c | 11 +- src/drivers/actuators/modal_io/modal_io.cpp | 32 +-- src/modules/commander/worker_thread.cpp | 4 +- src/modules/muorb/apps/muorb_main.cpp | 67 ++----- 17 files changed, 755 insertions(+), 141 deletions(-) mode change 100644 => 100755 boards/modalai/voxl2/target/voxl-px4 create mode 100755 boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config create mode 100755 boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config create mode 100755 boards/modalai/voxl2/target/voxl-px4-start delete mode 100755 boards/modalai/voxl2/target/voxl-px4.config diff --git a/boards/modalai/voxl2-slpi/default.px4board b/boards/modalai/voxl2-slpi/default.px4board index 6f3e20440e..f6eae5c82f 100644 --- a/boards/modalai/voxl2-slpi/default.px4board +++ b/boards/modalai/voxl2-slpi/default.px4board @@ -1,18 +1,28 @@ CONFIG_PLATFORM_QURT=y CONFIG_BOARD_TOOLCHAIN="qurt" -CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y -CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y -CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y -CONFIG_DRIVERS_QSHELL_QURT=y -CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_EKF2=y -CONFIG_MODULES_LAND_DETECTOR=y -CONFIG_MODULES_MC_ATT_CONTROL=y -CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y -CONFIG_MODULES_MC_POS_CONTROL=y -CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_BOARD_ROOTFSDIR="/" CONFIG_MODULES_MUORB_SLPI=y -CONFIG_MODULES_SENSORS=y -CONFIG_SYSTEMCMDS_PARAM=y CONFIG_SYSTEMCMDS_UORB=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y +CONFIG_DRIVERS_QSHELL_QURT=y +CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_DRIVERS_BAROMETER_INVENSENSE_ICP101XX=y +CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y +CONFIG_DRIVERS_MAGNETOMETER_ISENTEK_IST8310=y +CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y +CONFIG_DRIVERS_GPS=y +CONFIG_DRIVERS_RC_CRSF_RC=y +CONFIG_MODULES_MANUAL_CONTROL=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_SENSORS=y +CONFIG_MODULES_EKF2=y +CONFIG_MODULES_MC_POS_CONTROL=y +CONFIG_MODULES_MC_ATT_CONTROL=y +CONFIG_MODULES_MC_RATE_CONTROL=y +CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y +CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y +CONFIG_MODULES_LAND_DETECTOR=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_COMMANDER=y +CONFIG_PARAM_CLIENT=y diff --git a/boards/modalai/voxl2-slpi/src/board_config.h b/boards/modalai/voxl2-slpi/src/board_config.h index 6f0e71b30b..2aa91b4b36 100644 --- a/boards/modalai/voxl2-slpi/src/board_config.h +++ b/boards/modalai/voxl2-slpi/src/board_config.h @@ -45,7 +45,7 @@ * I2C buses */ #define CONFIG_I2C 1 -#define PX4_NUMBER_I2C_BUSES 3 +#define PX4_NUMBER_I2C_BUSES 4 /* * SPI buses diff --git a/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp b/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp index d1f1bdde1e..709f7e2519 100644 --- a/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp +++ b/boards/modalai/voxl2-slpi/src/drivers/mavlink_rc_in/mavlink_rc_in.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include "uORB/uORBManager.hpp" #include #include @@ -77,6 +78,7 @@ std::string port = "7"; uint32_t baudrate = 115200; uORB::PublicationMulti _rc_pub{ORB_ID(input_rc)}; +uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status)}; perf_counter_t _perf_rx_rate = nullptr; @@ -97,6 +99,7 @@ void task_main(int argc, char *argv[]); void handle_message_dsp(mavlink_message_t *msg); void handle_message_rc_channels_override_dsp(mavlink_message_t *msg); +void handle_message_radio_status_dsp(mavlink_message_t *msg); void handle_message_dsp(mavlink_message_t *msg) { @@ -117,6 +120,10 @@ void handle_message_dsp(mavlink_message_t *msg) handle_message_rc_channels_override_dsp(msg); break; + case MAVLINK_MSG_ID_RADIO_STATUS: + handle_message_radio_status_dsp(msg); + break; + default: break; } @@ -300,12 +307,33 @@ void task_main(int argc, char *argv[]) } } +void handle_message_radio_status_dsp(mavlink_message_t *msg) +{ + if (debug) { PX4_INFO("Radio status msg received"); } + + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + radio_status_s status{}; + + status.timestamp = hrt_absolute_time(); + status.rssi = rstatus.rssi; + status.remote_rssi = rstatus.remrssi; + status.txbuf = rstatus.txbuf; + status.noise = rstatus.noise; + status.remote_noise = rstatus.remnoise; + status.rxerrors = rstatus.rxerrors; + status.fix = rstatus.fixed; + + _radio_status_pub.publish(status); +} + void handle_message_rc_channels_override_dsp(mavlink_message_t *msg) { mavlink_rc_channels_override_t man; mavlink_msg_rc_channels_override_decode(msg, &man); - // if (debug) PX4_INFO("RC channels override msg received"); + if (debug) { PX4_INFO("RC channels override msg received"); } // Check target if (man.target_system != 0) { diff --git a/boards/modalai/voxl2-slpi/src/i2c.cpp b/boards/modalai/voxl2-slpi/src/i2c.cpp index 6befbedb1a..da40791c66 100644 --- a/boards/modalai/voxl2-slpi/src/i2c.cpp +++ b/boards/modalai/voxl2-slpi/src/i2c.cpp @@ -38,5 +38,6 @@ constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = { initI2CBusExternal(1), initI2CBusExternal(2), + initI2CBusExternal(4), initI2CBusInternal(5) }; diff --git a/boards/modalai/voxl2/default.px4board b/boards/modalai/voxl2/default.px4board index c0a2902904..8ba67ef454 100644 --- a/boards/modalai/voxl2/default.px4board +++ b/boards/modalai/voxl2/default.px4board @@ -2,19 +2,25 @@ CONFIG_PLATFORM_POSIX=y CONFIG_BOARD_LINUX_TARGET=y CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu" CONFIG_BOARD_ROOTFSDIR="/data/px4" -CONFIG_DRIVERS_QSHELL_POSIX=y -CONFIG_MODULES_COMMANDER=y -CONFIG_MODULES_DATAMAN=y -CONFIG_MODULES_FLIGHT_MODE_MANAGER=y -CONFIG_MODULES_LOGGER=y -CONFIG_MODULES_MAVLINK=y -CONFIG_MODULES_MUORB_APPS=y -CONFIG_MODULES_NAVIGATOR=y -CONFIG_MODULES_RC_UPDATE=y -CONFIG_SYSTEMCMDS_BSONDUMP=y -CONFIG_SYSTEMCMDS_PARAM=y -CONFIG_SYSTEMCMDS_PERF=y -CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y -CONFIG_SYSTEMCMDS_UORB=y -CONFIG_SYSTEMCMDS_VER=y CONFIG_ORB_COMMUNICATOR=y +CONFIG_PARAM_SERVER=y +CONFIG_DRIVERS_QSHELL_POSIX=y +CONFIG_DRIVERS_OSD_MSP_OSD=y +CONFIG_DRIVERS_ACTUATORS_MODAL_IO=y +CONFIG_SYSTEMCMDS_PERF=y +CONFIG_SYSTEMCMDS_UORB=y +CONFIG_SYSTEMCMDS_PARAM=y +CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y +CONFIG_SYSTEMCMDS_BSONDUMP=y +CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y +CONFIG_SYSTEMCMDS_VER=y +CONFIG_MODULES_LOGGER=y +CONFIG_MODULES_RC_UPDATE=y +CONFIG_MODULES_DATAMAN=y +CONFIG_MODULES_NAVIGATOR=y +CONFIG_MODULES_COMMANDER=y +CONFIG_MODULES_FLIGHT_MODE_MANAGER=y +CONFIG_MODULES_MAVLINK=y +CONFIG_MODULES_CONTROL_ALLOCATOR=y +CONFIG_MODULES_MUORB_APPS=y + diff --git a/boards/modalai/voxl2/scripts/install-voxl.sh b/boards/modalai/voxl2/scripts/install-voxl.sh index db746fffee..37f2ede020 100755 --- a/boards/modalai/voxl2/scripts/install-voxl.sh +++ b/boards/modalai/voxl2/scripts/install-voxl.sh @@ -9,11 +9,125 @@ adb push build/modalai_voxl2_default/bin/px4 /usr/bin # Push scripts to voxl2 adb push build/modalai_voxl2_default/bin/px4-alias.sh /usr/bin adb push boards/modalai/voxl2/target/voxl-px4 /usr/bin +adb push boards/modalai/voxl2/target/voxl-px4-start /usr/bin adb shell chmod a+x /usr/bin/px4-alias.sh adb shell chmod a+x /usr/bin/voxl-px4 +adb shell chmod a+x /usr/bin/voxl-px4-start # Push configuration file adb shell mkdir -p /etc/modalai -adb push boards/modalai/voxl2/target/voxl-px4.config /etc/modalai +adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai +adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai + +# Make sure to setup all of the needed px4 aliases. +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-accelsim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-attitude_estimator_q" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-barosim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-batt_smbus" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-bottle_drop" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-camera_feedback" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-camera_trigger" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-cdev_test" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-cm8jl65" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-commander" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-commander_tests" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-control_allocator" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-controllib_test" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-dataman" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ekf2" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-esc_calib" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ets_airspeed" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ex_fixedwing_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-fw_att_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-fw_pos_control_l1" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-gnd_att_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-gnd_pos_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-gps" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-gpssim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-gyrosim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-hello" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-hrt_test" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-land_detector" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-landing_target_estimator" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-led_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-leddar_one" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-linux_sbus" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-listener" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ll40ls" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-load_mon" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-local_position_estimator" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-logger" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-manual_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mavlink" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mavlink_bridge" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mavlink_tests" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mb12xx" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_att_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mc_pos_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-measairspeedsim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-mixer" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-motor_ramp" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-modalai_gps_timer" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms4525_airspeed" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ms5525_airspeed" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-msp_osd" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-muorb" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-muorb_test" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-navigator" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-param" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-perf" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-pga460" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-position_estimator_inav" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-pwm" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-pwm_out_sim" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-px4io" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-px4_mavlink_debug" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-px4_simple_app" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-qshell" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rc_tests" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rc_update" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-reboot" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rgbled" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rover_steering_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sd_bench" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sdp3x_airspeed" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-segway" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-send_event" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sensors" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sf0x" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sf0x_tests" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sf1xx" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-shutdown" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-sih" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-simulator" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-spektrum_rc" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-srf02" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-teraranger" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-tests" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-tfmini" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-top" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-tune_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ulanding_radar" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uorb" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uorb_tests" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-ver" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-vl53lxx" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-vmount" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-vtol_att_control" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-wind_estimator" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-rc_controller" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-uart_esc_driver" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-flight_mode_manager" +adb shell "cd /usr/bin; /bin/ln -f -s px4 px4-imu_server" + +# Make sure any required directories exist +adb shell "/bin/mkdir -p /data/px4/param" +adb shell "/bin/mkdir -p /data/px4/etc/extras" + +# Push the json files for the component metadata +adb push build/modalai_voxl2_default/actuators.json.xz /data/px4/etc/extras +adb push build/modalai_voxl2_default/component_general.json.xz /data/px4/etc/extras +adb push build/modalai_voxl2_default/parameters.json.xz /data/px4/etc/extras +adb push build/modalai_voxl2_default/events/all_events.json.xz /data/px4/etc/extras adb shell sync diff --git a/boards/modalai/voxl2/src/board_config.h b/boards/modalai/voxl2/src/board_config.h index c44de17796..9fe0f35807 100644 --- a/boards/modalai/voxl2/src/board_config.h +++ b/boards/modalai/voxl2/src/board_config.h @@ -50,3 +50,5 @@ #define BOARD_OVERRIDE_UUID "MODALAIVOXL20000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_VOXL2 + +#define MODAL_IO_DEFAULT_PORT "2" diff --git a/boards/modalai/voxl2/target/voxl-px4 b/boards/modalai/voxl2/target/voxl-px4 old mode 100644 new mode 100755 index 6f536d3f81..38560b47a0 --- a/boards/modalai/voxl2/target/voxl-px4 +++ b/boards/modalai/voxl2/target/voxl-px4 @@ -1,39 +1,147 @@ #!/bin/bash +CONFIG_FILE="/etc/modalai/voxl-px4.conf" + +GPS=NONE +RC=SPEKTRUM +IMU_ROTATION=NONE +OSD=DISABLE +DAEMON_MODE=DISABLE +SENSOR_CAL=ACTUAL +EXTRA_STEPS=() + +# Try to source configuration variables from a file. +# This will override anything already set in the environment. +if [ -f "$CONFIG_FILE" ]; then + echo "[INFO] Reading from $CONFIG_FILE" + source $CONFIG_FILE +else + echo "[INFO] $CONFIG_FILE not found, using defaults" +fi + # Make sure that the SLPI DSP test signature is there otherwise px4 cannot run # on the DSP if /bin/ls /usr/lib/rfsa/adsp/testsig-*.so &> /dev/null; then /bin/echo "Found DSP signature file" else - /bin/echo "Error, could not find DSP signature file" + /bin/echo "[WARNING] Could not find DSP signature file" # Look for the DSP signature generation script if [ -f /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh ]; then - /bin/echo "Attempting to generate the DSP signature file" + /bin/echo "[INFO] Attempting to generate the DSP signature file" # Automatically generate the test signature so that px4 can run on SLPI DSP /share/modalai/qrb5165-slpi-test-sig/generate-test-sig.sh else - /bin/echo "Could not find the DSP signature file generation script" + /bin/echo "[ERROR] Could not find the DSP signature file generation script" exit 0 fi fi -# Make sure to setup all of the needed px4 aliases. -cd /usr/bin -/bin/ln -f -s px4 px4-muorb -/bin/ln -f -s px4 px4-qshell -cd - +print_usage() { + echo -e "\nUsage: voxl-px4 [-b (Specify Holybro GPS unit)]" + echo " [-c delete configuration file and exit]" + echo " [-d start px4 in daemon mode]" + echo " [-f (Use fake rc input instead of from a real transmitter)]" + echo " [-i (Set IMU_ROTATION to ROTATION_YAW_180)]" + echo " [-m (Specify Matek GPS unit)]" + echo " [-o (Start OSD module on the apps processor)]" + echo " [-r (Specify TBS Crossfire RC receiver, MAVLINK)]" + echo " [-w (Specify TBS Crossfire RC receiver, raw)]" + echo " [-z (Use fake sensor calibration values)]" + echo " [-h (show help)]" -DAEMON=" " -S="OFF" + exit 1 +} -while getopts "ds" flag +print_config_settings(){ + echo -e "\n*************************" + echo "GPS=$GPS" + echo "RC=$RC" + echo "IMU_ROTATION=$IMU_ROTATION" + echo "OSD=$OSD" + echo "DAEMON_MODE=$DAEMON_MODE" + echo "SENSOR_CAL=$SENSOR_CAL" + echo "EXTRA STEPS:" + for i in "${EXTRA_STEPS[@]}" + do + echo -e "\t$i" + done + echo -e "*************************\n" +} + +while getopts "bcdhifmorwz" flag do case "${flag}" in - # Use -d to put PX4 into daemon mode - d) DAEMON="-d";; - # Use -s to run self tests - s) S="ON";; + b) + echo "[INFO] Holybro GPS selected" + GPS=HOLYBRO + ;; + c) + echo "[INFO] Wiping old config file" + if [ -f "$CONFIG_FILE" ]; then + rm -rf ${CONFIG_FILE} + fi + exit 0 + ;; + d) + echo "[INFO] Enabling daemon mode" + DAEMON_MODE=ENABLE + ;; + h) + print_usage + ;; + i) + echo "[INFO] Setting IMU_ROTATION to 4: ROTATION_YAW_180" + IMU_ROTATION=4 + ;; + f) + echo "[INFO] Setting RC to FAKE_RC_INPUT" + RC=FAKE_RC_INPUT + ;; + m) + echo "[INFO] Matek GPS selected" + GPS=MATEK + ;; + o) + echo "[INFO] OSD module selected" + OSD=ENABLE + ;; + r) + echo "[INFO] TBS Crossfire RC receiver, MAVLINK selected" + RC=CRSF_MAV + ;; + w) + echo "[INFO] TBS Crossfire RC receiver, raw selected" + RC=CRSF_RAW + ;; + z) + echo "[INFO] Fake sensor calibration values selected" + SENSOR_CAL=FAKE + ;; + *) + print_usage + ;; esac done -TESTMODE=$S px4 $DAEMON -s /etc/modalai/voxl-px4.config +if [ $DAEMON_MODE == "DISABLE" ]; then + DAEMON=" " +else + echo "[INFO] Daemon mode enabled" + DAEMON="-d" +fi + +if [ ! -f /data/px4/param/parameters ]; then + echo "[INFO] Setting default parameters for PX4 on voxl" + px4 $DAEMON -s /etc/modalai/voxl-px4-set-default-parameters.config + /bin/sync +fi + +if [ $SENSOR_CAL == "FAKE" ]; then + /bin/echo "[INFO] Setting up fake sensor calibration values" + px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config + /bin/sync +fi + +print_config_settings + +GPS=$GPS RC=$RC OSD=$OSD IMU_ROTATION=$IMU_ROTATION EXTRA_STEPS=$EXTRA_STEPS px4 $DAEMON -s /usr/bin/voxl-px4-start diff --git a/boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config b/boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config new file mode 100755 index 0000000000..8d270a56f6 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config @@ -0,0 +1,37 @@ + +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +# uorb start + +param select /data/px4/param/parameters + +# Load in all of the current parameters that have been saved in the file +param load + +# Fake Accel calibration parameters for testing +param set CAL_ACC0_ID 2490378 +param set CAL_ACC0_PRIO 75 +param set CAL_ACC0_ROT -1 +param set CAL_ACC0_XOFF -0.005 +param set CAL_ACC0_XSCALE 1.008 +param set CAL_ACC0_YOFF 0.008 +param set CAL_ACC0_YSCALE 1.0008 +param set CAL_ACC0_ZOFF -0.1 +param set CAL_ACC0_ZSCALE 1.008 + +# Fake gyro calibration parameters for testing +param set CAL_GYRO0_ID 2490378 +param set CAL_GYRO0_PRIO 75 +param set CAL_GYRO0_ROT 0 +param set CAL_GYRO0_XOFF -0.0008 +param set CAL_GYRO0_YOFF -0.03 +param set CAL_GYRO0_ZOFF 0.002 + +param save + +sleep 2 + +/usr/bin/px4-shutdown diff --git a/boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config b/boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config new file mode 100755 index 0000000000..729b2d21dc --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config @@ -0,0 +1,182 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +param select /data/px4/param/parameters + +# Make sure we are running at 800Hz on IMU +param set IMU_GYRO_RATEMAX 800 + +# EKF2 Parameters +param set EKF2_IMU_POS_X 0.027 +param set EKF2_IMU_POS_Y 0.009 +param set EKF2_IMU_POS_Z -0.019 +param set EKF2_EV_DELAY 5 +param set EKF2_AID_MASK 280 +param set EKF2_ABL_LIM 0.8 +param set EKF2_TAU_POS 0.25 +param set EKF2_TAU_VEL 0.25 + +param set MC_AIRMODE 0 + +param set MC_YAW_P 2.0 +param set MC_YAWRATE_P 0.15 +param set MC_YAWRATE_I 0.1 +param set MC_YAWRATE_D 0.0 +param set MC_YAWRATE_K 1.0 + +param set MC_PITCH_P 5.5 +param set MC_PITCHRATE_P 0.08 +param set MC_PITCHRATE_I 0.2 +param set MC_PITCHRATE_D 0.0013 +param set MC_PITCHRATE_K 1.0 + +param set MC_ROLL_P 5.5 +param set MC_ROLLRATE_P 0.08 +param set MC_ROLLRATE_I 0.2 +param set MC_ROLLRATE_D 0.0013 +param set MC_ROLLRATE_K 1.0 + +param set MPC_VELD_LP 5.0 + +# tweak MPC_THR_MIN to prevent roll/pitch losing control +# authority under rapid downward acceleration +param set MPC_THR_MAX 0.75 +param set MPC_THR_MIN 0.08 +param set MPC_THR_HOVER 0.42 +param set MPC_MANTHR_MIN 0.05 + +# default position mode with a little expo, smooth mode is terrible +param set MPC_POS_MODE 0 +param set MPC_YAW_EXPO 0.20 +param set MPC_XY_MAN_EXPO 0.20 +param set MPC_Z_MAN_EXPO 0.20 + +# max velocities +param set MPC_VEL_MANUAL 5.0 +param set MPC_XY_VEL_MAX 5.0 +param set MPC_XY_CRUISE 5.0 +param set MPC_Z_VEL_MAX_DN 1.5 +param set MPC_Z_VEL_MAX_UP 4.0 +param set MPC_LAND_SPEED 1.0 + +# Horizontal position PID +param set MPC_XY_P 0.95 +param set MPC_XY_VEL_P_ACC 3.00 +param set MPC_XY_VEL_I_ACC 0.10 +param set MPC_XY_VEL_D_ACC 0.00 + +# Vertical position PID +# PX4 Defaults +param set MPC_Z_P 1.0 +param set MPC_Z_VEL_P_ACC 8.0 +param set MPC_Z_VEL_I_ACC 2.0 +param set MPC_Z_VEL_D_ACC 0.0 + +param set MPC_TKO_RAMP_T 1.50 +param set MPC_TKO_SPEED 1.50 + +# Set the ESC outputs to function as motors +param set MODAL_IO_FUNC1 101 +param set MODAL_IO_FUNC2 103 +param set MODAL_IO_FUNC3 104 +param set MODAL_IO_FUNC4 102 + +param set MODAL_IO_SDIR1 0 +param set MODAL_IO_SDIR2 0 +param set MODAL_IO_SDIR3 0 +param set MODAL_IO_SDIR4 0 + +param set MODAL_IO_CONFIG 1 +param set MODAL_IO_REV 0 +param set MODAL_IO_MODE 0 +param set MODAL_IO_BAUD 2000000 +param set MODAL_IO_RPM_MAX 10500 +param set MODAL_IO_RPM_MIN 1000 + +# Some parameters for control allocation +param set CA_ROTOR_COUNT 4 +param set CA_R_REV 0 +param set CA_AIRFRAME 0 +param set CA_ROTOR_COUNT 4 +param set CA_ROTOR0_PX 0.15 +param set CA_ROTOR0_PY 0.15 +param set CA_ROTOR1_PX -0.15 +param set CA_ROTOR1_PY -0.15 +param set CA_ROTOR2_PX 0.15 +param set CA_ROTOR2_PY -0.15 +param set CA_ROTOR2_KM -0.05 +param set CA_ROTOR3_PX -0.15 +param set CA_ROTOR3_PY 0.15 +param set CA_ROTOR3_KM -0.05 + +# Some parameter settings to disable / ignore certain preflight checks + +# No GPS driver yet so disable it +param set SYS_HAS_GPS 0 + +# Allow arming wihtout a magnetometer (Need magnetometer driver) +param set SYS_HAS_MAG 0 +param set EKF2_MAG_TYPE 5 + +# Allow arming without battery check (Need voxlpm driver) +param set CBRK_SUPPLY_CHK 894281 + +# Allow arming without an SD card +param set COM_ARM_SDCARD 0 + +# Allow arming wihtout CPU load information +param set COM_CPU_MAX 0.0 + +# Disable auto disarm. This is number of seconds to wait for takeoff +# after arming. If no takeoff happens then it will disarm. A negative +# value disables this. +param set COM_DISARM_PRFLT -1 + +# This parameter doesn't exist anymore. Need to see what the new method is. +# param set MAV_BROADCAST 0 + +# Doesn't work without setting this to Quadcopter +param set MAV_TYPE 2 + +# Parameters that we don't use but QGC complains about if they aren't there +param set SYS_AUTOSTART 4001 + +# Default RC channel mappings +param set RC_MAP_ACRO_SW 0 +param set RC_MAP_ARM_SW 0 +param set RC_MAP_AUX1 0 +param set RC_MAP_AUX2 0 +param set RC_MAP_AUX3 0 +param set RC_MAP_AUX4 0 +param set RC_MAP_AUX5 0 +param set RC_MAP_AUX6 0 +param set RC_MAP_FAILSAFE 0 +param set RC_MAP_FLAPS 0 +param set RC_MAP_FLTMODE 6 +param set RC_MAP_GEAR_SW 0 +param set RC_MAP_KILL_SW 7 +param set RC_MAP_LOITER_SW 0 +param set RC_MAP_MAN_SW 0 +param set RC_MAP_MODE_SW 0 +param set RC_MAP_OFFB_SW 0 +param set RC_MAP_PARAM1 0 +param set RC_MAP_PARAM2 0 +param set RC_MAP_PARAM3 0 +param set RC_MAP_PITCH 2 +param set RC_MAP_POSCTL_SW 0 +param set RC_MAP_RATT_SW 0 +param set RC_MAP_RETURN_SW 0 +param set RC_MAP_ROLL 1 +param set RC_MAP_STAB_SW 0 +param set RC_MAP_THROTTLE 3 +param set RC_MAP_TRANS_SW 0 +param set RC_MAP_YAW 4 + +param save + +sleep 2 + +# Need px4-shutdown otherwise Linux system shutdown is called +px4-shutdown diff --git a/boards/modalai/voxl2/target/voxl-px4-start b/boards/modalai/voxl2/target/voxl-px4-start new file mode 100755 index 0000000000..2611de9853 --- /dev/null +++ b/boards/modalai/voxl2/target/voxl-px4-start @@ -0,0 +1,164 @@ +#!/bin/sh +# PX4 commands need the 'px4-' prefix in bash. +# (px4-alias.sh is expected to be in the PATH) +. px4-alias.sh + +echo -e "\n*************************" +echo "GPS: $GPS" +echo "RC: $RC" +echo "IMU ROTATION: $IMU_ROTATION" +echo "OSD: $OSD" +echo "EXTRA STEPS:" +for i in "${EXTRA_STEPS[@]}" +do + echo -e "\t$i" +done +echo -e "*************************\n" + +# In order to just exit after starting the uorb / muorb modules define +# the environment variable MINIMAL_PX4. (e.g. export MINIMAL_PX4=1) +# This is useful for testing / debug where you may want to start drivers +# and modules manually from the px4 command shell +if [ ! -z $MINIMAL_PX4 ]; then + /bin/echo "Running minimal script" + exit 0 +fi + +# Sleep a little here. A lot happens when the uorb and muorb start +# and we need to make sure that it all completes successfully to avoid +# any possible race conditions. +/bin/sleep 1 + +param select /data/px4/param/parameters + +# Load in all of the parameters that have been saved in the file +param load + +# Start logging and use timestamps for log files when possible. +# Add the "-e" option to start logging immediately. Default is +# to log only when armed. Caution must be used with the "-e" option +# because if power is removed without stopping the logger gracefully then +# the log file may be corrupted. +logger start -t + +/bin/sleep 1 + +# IMU (accelerometer / gyroscope) +if [ "$IMU_ROTATION" != "NONE" ]; then + /bin/echo "Starting IMU driver with rotation: $IMU_ROTATION" + qshell icm42688p start -s -R $IMU_ROTATION +else + qshell icm42688p start -s +fi + + +/bin/sleep 1 + +# Start Invensense ICP 101xx barometer built on to VOXL 2 +qshell icp101xx start -I -b 5 + +# Magnetometer +if [ "$GPS" == "MATEK" ]; then + # Use this line for the magnetometer in the Matek Systems M8Q-5883 module + /bin/echo "Starting Mateksys M8Q-5883 magnetometer" + qshell qmc5883l start -R 10 -X -b 1 +elif [ "$GPS" == "HOLYBRO" ]; then + # Use this line for the magnetometer in the Holybro GPS module + /bin/echo "Starting Holybro magnetometer" + qshell ist8310 start -R 10 -X -b 1 +fi + +# GPS +if [ "$GPS" == "MATEK" ]; then + # Use this gps start line instead for Matek Systems M8Q-5883 module + /bin/echo "Starting Mateksys M8Q-5883 GPS" + qshell gps start +elif [ "$GPS" == "HOLYBRO" ]; then + # Only the newer Holybro unit is supported on M0054 + /bin/echo "Starting Holybro GPS" + qshell gps start -b 115200 +fi + +# LED driver for the Pixhawk 4 GPS module +if [ "$GPS" == "HOLYBRO" ]; then + /bin/echo "Starting Holybro LED driver" + qshell rgbled_ncp5623c start -X -b 1 -f 400 -a 56 +fi + +# We do not change the value of SYS_AUTOCONFIG but if it does not +# show up as used then it is not reported to QGC and we get a +# missing parameter error. +param touch SYS_AUTOCONFIG + +# ESC driver +qshell modal_io start + +/bin/sleep 1 + +if [ "$RC" == "FAKE_RC_INPUT" ]; then + /bin/echo "Starting fake RC driver" + qshell rc_controller start +elif [ "$RC" == "CRSF_RAW" ]; then + /bin/echo "Starting CRSF RC driver" + qshell crsf_rc start -d 7 +elif [ "$RC" == "CRSF_MAV" ]; then + /bin/echo "Starting TBS crossfire RC - MAV Mode" + qshell mavlink_rc_in start -m -p 7 -b 115200 +elif [ "$RC" == "SPEKTRUM" ]; then + /bin/echo "Starting Spektrum RC" + qshell spektrum_rc start +fi + +/bin/sleep 1 + +# APM power monitor +qshell voxlpm start -X -b 2 + +# Start all of the processing modules on DSP +qshell sensors start +qshell ekf2 start +qshell mc_pos_control start +qshell mc_att_control start +qshell mc_rate_control start +qshell mc_hover_thrust_estimator start +qshell land_detector start multicopter +qshell manual_control start +qshell control_allocator start +qshell rc_update start +qshell commander start +qshell commander mode manual + +/bin/sleep 1 + +# Start all of the processing modules on the applications processor +dataman start +navigator start + +# This is needed for altitude and position hold modes +flight_mode_manager start + +mavlink start -x -u 14556 -o 14557 -r 100000 -n lo -m onboard + +# slow down some of the fastest streams in onboard mode +mavlink stream -u 14556 -s HIGHRES_IMU -r 10 +mavlink stream -u 14556 -s ATTITUDE -r 10 +mavlink stream -u 14556 -s ATTITUDE_QUATERNION -r 10 +# speed up rc_channels +mavlink stream -u 14556 -s RC_CHANNELS -r 50 + +/bin/sleep 1 + +mavlink boot_complete + +# Optional MSP OSD driver for DJI goggles +# This is only supported on M0054 (with M0125 accessory board) +if [ "$OSD" == "ENABLE" ]; then + /bin/echo "Starting OSD driver" + msp_osd start -d /dev/ttyHS1 +fi + +# Start optional EXTRA_STEPS +for i in "${EXTRA_STEPS[@]}" +do + $i +done diff --git a/boards/modalai/voxl2/target/voxl-px4.config b/boards/modalai/voxl2/target/voxl-px4.config deleted file mode 100755 index e84e24275f..0000000000 --- a/boards/modalai/voxl2/target/voxl-px4.config +++ /dev/null @@ -1,21 +0,0 @@ -#!/bin/sh -# PX4 commands need the 'px4-' prefix in bash. -# (px4-alias.sh is expected to be in the PATH) -. px4-alias.sh - -if [ $TESTMODE = "ON" ]; then - /bin/echo "Running self tests" - muorb test - MUORB_TEST_STATUS=$? - if [ $MUORB_TEST_STATUS -ne 0 ]; then - /bin/echo "muorb test failed" - shutdown - exit 0 - else - /bin/echo "muorb test passed" - fi -fi - -muorb start - -qshell icm42688p start -s diff --git a/platforms/qurt/src/px4/tasks.cpp b/platforms/qurt/src/px4/tasks.cpp index 13bac3f711..712c6c377c 100644 --- a/platforms/qurt/src/px4/tasks.cpp +++ b/platforms/qurt/src/px4/tasks.cpp @@ -93,8 +93,13 @@ extern "C" { if (name == NULL) { return -1; } - memcpy(attr->name, name, PTHREAD_NAME_LEN); - attr->name[PTHREAD_NAME_LEN - 1] = 0; + size_t name_len = strlen(name); + + if (name_len > PX4_TASK_MAX_NAME_LENGTH) { name_len = PX4_TASK_MAX_NAME_LENGTH; } + + memcpy(attr->name, name, name_len); + attr->name[name_len] = 0; + return 0; } @@ -424,14 +429,20 @@ int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts) int px4_prctl(int option, const char *arg2, px4_task_t pid) { int rv = -1; + + if (option != PR_SET_NAME) { return rv; } + pthread_mutex_lock(&task_mutex); for (int i = 0; i < PX4_MAX_TASKS; i++) { if (taskmap[i].isused && taskmap[i].tid == (pthread_t) pid) { rv = pthread_attr_setthreadname(&taskmap[i].attr, arg2); + pthread_mutex_unlock(&task_mutex); return rv; } } + pthread_mutex_unlock(&task_mutex); + return rv; } diff --git a/platforms/qurt/unresolved_symbols.c b/platforms/qurt/unresolved_symbols.c index 5511c7879a..0d5ef43d58 100644 --- a/platforms/qurt/unresolved_symbols.c +++ b/platforms/qurt/unresolved_symbols.c @@ -1,30 +1,33 @@ #include - -void qurt_free(void *ptr); +#include +#include __attribute__((visibility("default"))) void free(void *ptr) { qurt_free(ptr); - // ptr = 0; + ptr = 0; } __attribute__((visibility("default"))) void *malloc(size_t size) { - return (void *) 0; + return qurt_malloc(size); } __attribute__((visibility("default"))) void *calloc(size_t nmemb, size_t size) { + PX4_ERR("Undefined calloc called"); return (void *) 0; } __attribute__((visibility("default"))) void *realloc(void *ptr, size_t size) { + PX4_ERR("Undefined realloc called"); return (void *) 0; } __attribute__((visibility("default"))) int nanosleep(const struct timespec *req, struct timespec *rem) { + PX4_ERR("Undefined nanosleep called"); return -1; } diff --git a/src/drivers/actuators/modal_io/modal_io.cpp b/src/drivers/actuators/modal_io/modal_io.cpp index 11bc98e771..6a9c6e0a24 100644 --- a/src/drivers/actuators/modal_io/modal_io.cpp +++ b/src/drivers/actuators/modal_io/modal_io.cpp @@ -31,6 +31,8 @@ * ****************************************************************************/ +#include + #include #include "modal_io.hpp" @@ -255,7 +257,9 @@ int ModalIo::task_spawn(int argc, char *argv[]) PX4_ERR("alloc failed"); } - delete instance; + // This will cause a crash on SLPI DSP + // delete instance; + _object.store(nullptr); _task_id = -1; @@ -322,7 +326,7 @@ int ModalIo::parse_response(uint8_t *buf, uint8_t len, bool print_feedback) uint32_t voltage = fb.voltage; int32_t current = fb.current * 8; int32_t temperature = fb.temperature / 100; - PX4_INFO("[%lld] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, motor_idx + 1, + PX4_INFO("[%" PRId64 "] ID_RAW=%d ID=%d, RPM=%5d, PWR=%3d%%, V=%5dmV, I=%+5dmA, T=%+3dC", tnow, (int)id, motor_idx + 1, (int)rpm, (int)power, (int)voltage, (int)current, (int)temperature); } @@ -1426,21 +1430,21 @@ int ModalIo::print_status() PX4_INFO(""); - PX4_INFO("Params: MODAL_IO_CONFIG: %li", _parameters.config); - PX4_INFO("Params: MODAL_IO_BAUD: %li", _parameters.baud_rate); + PX4_INFO("Params: MODAL_IO_CONFIG: %" PRId32, _parameters.config); + PX4_INFO("Params: MODAL_IO_BAUD: %" PRId32, _parameters.baud_rate); - PX4_INFO("Params: MODAL_IO_FUNC1: %li", _parameters.function_map[0]); - PX4_INFO("Params: MODAL_IO_FUNC2: %li", _parameters.function_map[1]); - PX4_INFO("Params: MODAL_IO_FUNC3: %li", _parameters.function_map[2]); - PX4_INFO("Params: MODAL_IO_FUNC4: %li", _parameters.function_map[3]); + PX4_INFO("Params: MODAL_IO_FUNC1: %" PRId32, _parameters.function_map[0]); + PX4_INFO("Params: MODAL_IO_FUNC2: %" PRId32, _parameters.function_map[1]); + PX4_INFO("Params: MODAL_IO_FUNC3: %" PRId32, _parameters.function_map[2]); + PX4_INFO("Params: MODAL_IO_FUNC4: %" PRId32, _parameters.function_map[3]); - PX4_INFO("Params: MODAL_IO_SDIR1: %li", _parameters.direction_map[0]); - PX4_INFO("Params: MODAL_IO_SDIR2: %li", _parameters.direction_map[1]); - PX4_INFO("Params: MODAL_IO_SDIR3: %li", _parameters.direction_map[2]); - PX4_INFO("Params: MODAL_IO_SDIR4: %li", _parameters.direction_map[3]); + PX4_INFO("Params: MODAL_IO_SDIR1: %" PRId32, _parameters.direction_map[0]); + PX4_INFO("Params: MODAL_IO_SDIR2: %" PRId32, _parameters.direction_map[1]); + PX4_INFO("Params: MODAL_IO_SDIR3: %" PRId32, _parameters.direction_map[2]); + PX4_INFO("Params: MODAL_IO_SDIR4: %" PRId32, _parameters.direction_map[3]); - PX4_INFO("Params: MODAL_IO_RPM_MIN: %li", _parameters.rpm_min); - PX4_INFO("Params: MODAL_IO_RPM_MAX: %li", _parameters.rpm_max); + PX4_INFO("Params: MODAL_IO_RPM_MIN: %" PRId32, _parameters.rpm_min); + PX4_INFO("Params: MODAL_IO_RPM_MAX: %" PRId32, _parameters.rpm_max); PX4_INFO(""); diff --git a/src/modules/commander/worker_thread.cpp b/src/modules/commander/worker_thread.cpp index 70a3ebb33e..501c483e91 100644 --- a/src/modules/commander/worker_thread.cpp +++ b/src/modules/commander/worker_thread.cpp @@ -75,15 +75,13 @@ void WorkerThread::startTask(Request request) pthread_attr_init(&low_prio_attr); pthread_attr_setstacksize(&low_prio_attr, PX4_STACK_ADJUSTED(4804)); -#ifndef __PX4_QURT - // This is not supported by QURT (yet). struct sched_param param; pthread_attr_getschedparam(&low_prio_attr, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; pthread_attr_setschedparam(&low_prio_attr, ¶m); -#endif + int ret = pthread_create(&_thread_handle, &low_prio_attr, &threadEntryTrampoline, this); pthread_attr_destroy(&low_prio_attr); diff --git a/src/modules/muorb/apps/muorb_main.cpp b/src/modules/muorb/apps/muorb_main.cpp index 6ad21b4a53..d3c4d78cec 100644 --- a/src/modules/muorb/apps/muorb_main.cpp +++ b/src/modules/muorb/apps/muorb_main.cpp @@ -35,11 +35,9 @@ #include "uORBAppsProtobufChannel.hpp" #include "uORB/uORBManager.hpp" -extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); } - -static void usage() -{ - PX4_INFO("Usage: muorb 'start', 'test', 'stop', 'status'"); +extern "C" { + __EXPORT int muorb_main(int argc, char *argv[]); + __EXPORT int muorb_init(); } static bool enable_debug = false; @@ -47,52 +45,21 @@ static bool enable_debug = false; int muorb_main(int argc, char *argv[]) { - if (argc < 2) { - usage(); - return -EINVAL; + return muorb_init(); +} + +int +muorb_init() +{ + uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); + + PX4_INFO("Got muorb init command"); + + if (channel && channel->Initialize(enable_debug)) { + uORB::Manager::get_instance()->set_uorb_communicator(channel); + + if (channel->Test()) { return OK; } } - // TODO: Add an optional start parameter to control debug messages - if (!strcmp(argv[1], "start")) { - // Register the protobuf channel with UORB. - uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); - - PX4_INFO("Got muorb start command"); - - if (channel && channel->Initialize(enable_debug)) { - uORB::Manager::get_instance()->set_uorb_communicator(channel); - return OK; - } - - } else if (!strcmp(argv[1], "test")) { - uORB::AppsProtobufChannel *channel = uORB::AppsProtobufChannel::GetInstance(); - - PX4_INFO("Got muorb test command"); - - if (channel && channel->Initialize(enable_debug)) { - uORB::Manager::get_instance()->set_uorb_communicator(channel); - - if (channel->Test()) { return OK; } - } - - } else if (!strcmp(argv[1], "stop")) { - if (uORB::AppsProtobufChannel::isInstance() == false) { - PX4_WARN("muorb not running"); - } - - return OK; - - } else if (!strcmp(argv[1], "status")) { - if (uORB::AppsProtobufChannel::isInstance()) { - PX4_INFO("muorb initialized"); - - } else { - PX4_INFO("muorb not running"); - } - - return OK; - } - - usage(); return -EINVAL; }