From de4f594937e5ef4fc8abcb965e04509f74928c7c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 9 Jan 2020 11:00:40 -0500 Subject: [PATCH] DriverFramework purge The bulk of this change was tightly coupled and needed to be deleted in one pass. Some of the smaller changes were things that broke as a result of the initial purge and subsequently fixed by further eradicating unnecessary platform differences. Finally, I deleted any dead code I came across in the related files I touched while going through everything. - DriverFramework (src/lib/DriverFramework submodule) completely removed - added dspal submodule in qurt platform (was brought in via DriverFramework) - all df wrapper drivers removed - all boards using df wrapper drivers updated to use in tree equivalents - unused empty arch/board.h on posix and qurt removed - unused IOCTLs removed (pub block, priv, etc) - Integrator delete methods only used from df wrapper drivers - commander: sensor calibration use "NuttX version" everywhere for now - sensors: update to px4_{open, read, close} instead of DevMgr wrapper (adc open for analog differential pressure) - battery_status: update to px4_{open, read, close} instead of DevMgr wrapper (adc open for analog differential pressure) - cdev cleanup conflicting typedefs and names with actual OS (pollevent_t, etc) - load_mon and top remove from linux boards (unused) - delete unused PX4_MAIN_FUNCTION - delete unused getreg32 macro - delete unused SIOCDEVPRIVATE define - named each platform tasks consistently - posix list_devices and list_topics removed (list_files now shows all virtual files) --- .gitmodules | 7 +- CMakeLists.txt | 20 - Documentation/Doxyfile.in | 3 +- Tools/astyle/files_to_check_code_style.sh | 2 +- boards/aerotenna/ocpoc/default.cmake | 6 +- boards/atlflight/eagle/default.cmake | 7 +- boards/atlflight/eagle/qurt.cmake | 7 +- boards/atlflight/excelsior/default.cmake | 7 +- boards/atlflight/excelsior/qurt.cmake | 5 - boards/beaglebone/blue/default.cmake | 6 +- boards/beaglebone/blue/src/init.cpp | 4 - boards/emlid/navio2/cmake/upload.cmake | 2 +- boards/emlid/navio2/default.cmake | 9 +- boards/px4/raspberrypi/default.cmake | 4 +- boards/px4/sitl/default.cmake | 4 +- boards/px4/sitl/rtps.cmake | 4 +- boards/px4/sitl/test.cmake | 4 +- cmake/px4_add_board.cmake | 16 - cmake/px4_add_common_flags.cmake | 1 - cmake/px4_add_module.cmake | 2 +- platforms/common/apps.cpp.in | 51 +- .../include/px4_platform_common/defines.h | 17 - .../include/px4_platform_common/posix.h | 30 +- platforms/common/shutdown.cpp | 1 + platforms/nuttx/cmake/px4_impl_os.cmake | 1 - platforms/nuttx/src/px4/common/CMakeLists.txt | 2 +- .../common/include/px4_platform/micro_hal.h | 1 - .../common/{px4_nuttx_tasks.c => tasks.cpp} | 10 +- platforms/posix/CMakeLists.txt | 5 - platforms/posix/cmake/px4_impl_os.cmake | 36 +- platforms/posix/include/arch/board/board.h | 0 platforms/posix/src/px4/common/CMakeLists.txt | 2 +- platforms/posix/src/px4/common/main.cpp | 3 - .../posix/src/px4/common/px4_posix_impl.cpp | 1 - .../common/{px4_posix_tasks.cpp => tasks.cpp} | 0 .../src/px4/common/test_stubs/CMakeLists.txt | 1 - .../src/px4/common/test_stubs/stub_devmgr.cpp | 10 - .../src/px4/common/test_stubs/stub_devmgr.h | 8 - platforms/qurt/CMakeLists.txt | 8 +- platforms/qurt/cmake/px4_impl_os.cmake | 12 +- platforms/qurt/dspal | 1 + platforms/qurt/include/arch/board/board.h | 0 platforms/qurt/include/poll.h | 40 - platforms/qurt/include/sys/ioctl.h | 3 - platforms/qurt/src/px4/common/CMakeLists.txt | 2 +- platforms/qurt/src/px4/common/commands_hil.c | 2 - platforms/qurt/src/px4/common/main.cpp | 2 - .../common/{px4_qurt_tasks.cpp => tasks.cpp} | 0 posix-configs/bbblue/px4.config | 2 +- posix-configs/bbblue/px4_fw.config | 2 +- posix-configs/eagle/200qx/px4.config | 14 +- posix-configs/eagle/210qc/px4.config | 12 +- posix-configs/eagle/flight/px4.config | 5 +- posix-configs/eagle/hil/px4.config | 32 +- posix-configs/eagle/init/rcS | 108 +-- posix-configs/excelsior/px4.config | 61 +- posix-configs/ocpoc/px4.config | 3 +- posix-configs/rpi/px4.config | 8 +- posix-configs/rpi/px4_fw.config | 8 +- posix-configs/rpi/px4_test.config | 8 +- .../df_ak8963_wrapper/CMakeLists.txt | 45 - .../df_ak8963_wrapper/df_ak8963_wrapper.cpp | 459 --------- .../df_isl29501_wrapper/CMakeLists.txt | 45 - .../df_isl29501_wrapper.cpp | 378 -------- .../df_lsm9ds1_wrapper/CMakeLists.txt | 45 - .../df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp | 854 ----------------- .../df_ltc2946_wrapper/CMakeLists.txt | 46 - .../df_ltc2946_wrapper/df_ltc2946_wrapper.cpp | 257 ----- .../df_mpu6050_wrapper/CMakeLists.txt | 45 - .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp | 687 -------------- .../df_mpu9250_wrapper/CMakeLists.txt | 45 - .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 897 ------------------ .../df_trone_wrapper/CMakeLists.txt | 46 - .../df_trone_wrapper/df_trone_wrapper.cpp | 362 ------- src/drivers/drv_device.h | 16 +- src/drivers/linux_pwm_out/bbblue_pwm_rc.cpp | 4 +- src/drivers/linux_pwm_out/linux_pwm_out.cpp | 2 +- src/drivers/magnetometer/ist8310/ist8310.cpp | 3 - .../magnetometer/lis3mdl/lis3mdl_i2c.cpp | 2 - .../magnetometer/lis3mdl/lis3mdl_spi.cpp | 2 - .../magnetometer/qmc5883/qmc5883_i2c.cpp | 2 - .../magnetometer/qmc5883/qmc5883_spi.cpp | 2 - .../magnetometer/rm3100/rm3100_i2c.cpp | 2 +- .../magnetometer/rm3100/rm3100_spi.cpp | 2 - src/drivers/px4io/px4io.cpp | 2 +- src/drivers/qshell/qurt/qshell.cpp | 1 - .../qurt/fc_addon/mpu_spi/CMakeLists.txt | 56 -- .../qurt/fc_addon/mpu_spi/mpu9x50_main.cpp | 641 ------------- .../qurt/fc_addon/mpu_spi/mpu9x50_params.c | 86 -- .../qurt/fc_addon/rc_receiver/CMakeLists.txt | 55 -- .../fc_addon/rc_receiver/rc_receiver_main.cpp | 347 ------- .../fc_addon/rc_receiver/rc_receiver_params.c | 53 -- .../qurt/fc_addon/uart_esc/CMakeLists.txt | 56 -- .../qurt/fc_addon/uart_esc/uart_esc_main.cpp | 521 ---------- .../qurt/fc_addon/uart_esc/uart_esc_params.c | 139 --- src/drivers/qurt/tests/hello/CMakeLists.txt | 41 - .../qurt/tests/hello/hello_example.cpp | 61 -- src/drivers/qurt/tests/hello/hello_example.h | 54 -- src/drivers/qurt/tests/hello/hello_main.cpp | 56 -- .../qurt/tests/hello/hello_start_qurt.cpp | 101 -- src/drivers/qurt/tests/muorb/CMakeLists.txt | 40 - .../qurt/tests/muorb/muorb_test_example.cpp | 215 ----- .../qurt/tests/muorb/muorb_test_example.h | 59 -- .../qurt/tests/muorb/muorb_test_main.cpp | 57 -- .../tests/muorb/muorb_test_start_qurt.cpp | 115 --- src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/drivers/uavcan/uavcan_main.cpp | 2 +- src/drivers/uavcan/uavcan_servers.cpp | 2 +- src/drivers/uavcannode/led.cpp | 2 +- src/examples/bottle_drop/bottle_drop.cpp | 2 +- src/lib/DriverFramework | 1 - src/lib/cdev/CDev.cpp | 25 +- src/lib/cdev/CDev.hpp | 12 +- src/lib/cdev/posix/cdev_platform.cpp | 74 +- src/lib/drivers/barometer/PX4Barometer.cpp | 2 - src/lib/drivers/device/CDev.cpp | 26 +- src/lib/drivers/device/CDev.hpp | 2 +- src/lib/drivers/device/integrator.cpp | 44 - src/lib/drivers/device/integrator.h | 42 - src/lib/drivers/device/nuttx/SPI.hpp | 2 + .../drivers/magnetometer/PX4Magnetometer.cpp | 1 - src/lib/systemlib/CMakeLists.txt | 10 +- src/lib/systemlib/cpuload.c | 2 +- src/modules/commander/CMakeLists.txt | 1 - .../commander/accelerometer_calibration.cpp | 10 +- .../commander/airspeed_calibration.cpp | 1 - src/modules/commander/baro_calibration.cpp | 1 - .../commander/calibration_routines.cpp | 1 - src/modules/commander/esc_calibration.cpp | 30 +- src/modules/commander/gyro_calibration.cpp | 7 +- src/modules/commander/mag_calibration.cpp | 20 +- .../temperature_calibration/polyfit.hpp | 1 - src/modules/mavlink/mavlink_receiver.cpp | 2 + src/modules/sensors/sensors.cpp | 17 +- src/modules/sensors/voted_sensors_update.cpp | 196 ++-- src/modules/sensors/voted_sensors_update.h | 33 - src/modules/simulator/simulator.cpp | 31 +- src/modules/simulator/simulator.h | 8 +- src/modules/simulator/simulator_mavlink.cpp | 25 +- src/modules/uORB/uORBDeviceMaster.cpp | 6 +- src/modules/uORB/uORBDeviceNode.cpp | 4 +- src/modules/uORB/uORBDeviceNode.hpp | 4 +- src/modules/uORB/uORBMain.cpp | 2 +- .../uORB/uORB_tests/uORBTest_UnitTest.cpp | 1 - .../uORB/uORB_tests/uORB_tests_main.cpp | 7 - src/systemcmds/config/config.c | 61 +- src/systemcmds/esc_calib/esc_calib.c | 2 +- src/systemcmds/motor_ramp/motor_ramp.cpp | 2 +- src/systemcmds/param/param.cpp | 2 - src/systemcmds/pwm/pwm.cpp | 2 - src/systemcmds/shutdown/shutdown.c | 2 - src/systemcmds/tests/test_dataman.c | 2 - src/systemcmds/tests/test_led.c | 2 - src/systemcmds/tests/test_ppm_loopback.c | 1 - src/systemcmds/tests/test_rc.c | 1 - src/systemcmds/tests/test_servo.c | 1 - src/systemcmds/tests/test_sleep.c | 2 - src/systemcmds/tests/test_time.c | 2 - src/systemcmds/tests/test_uart_baudchange.c | 2 - src/systemcmds/tests/test_uart_break.c | 2 - src/systemcmds/tests/test_uart_console.c | 2 - src/systemcmds/tests/test_uart_loopback.c | 2 - src/systemcmds/tests/test_uart_send.c | 2 - src/systemcmds/usb_connected/usb_connected.c | 2 +- 165 files changed, 285 insertions(+), 8108 deletions(-) rename platforms/nuttx/src/px4/common/{px4_nuttx_tasks.c => tasks.cpp} (98%) delete mode 100644 platforms/posix/include/arch/board/board.h rename platforms/posix/src/px4/common/{px4_posix_tasks.cpp => tasks.cpp} (100%) delete mode 100644 platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp delete mode 100644 platforms/posix/src/px4/common/test_stubs/stub_devmgr.h create mode 160000 platforms/qurt/dspal delete mode 100644 platforms/qurt/include/arch/board/board.h delete mode 100644 platforms/qurt/include/poll.h delete mode 100644 platforms/qurt/include/sys/ioctl.h rename platforms/qurt/src/px4/common/{px4_qurt_tasks.cpp => tasks.cpp} (100%) delete mode 100644 src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp delete mode 100644 src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp delete mode 100644 src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt delete mode 100644 src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp delete mode 100644 src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c delete mode 100644 src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt delete mode 100644 src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp delete mode 100644 src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c delete mode 100644 src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt delete mode 100644 src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp delete mode 100644 src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c delete mode 100644 src/drivers/qurt/tests/hello/CMakeLists.txt delete mode 100644 src/drivers/qurt/tests/hello/hello_example.cpp delete mode 100644 src/drivers/qurt/tests/hello/hello_example.h delete mode 100644 src/drivers/qurt/tests/hello/hello_main.cpp delete mode 100644 src/drivers/qurt/tests/hello/hello_start_qurt.cpp delete mode 100644 src/drivers/qurt/tests/muorb/CMakeLists.txt delete mode 100644 src/drivers/qurt/tests/muorb/muorb_test_example.cpp delete mode 100644 src/drivers/qurt/tests/muorb/muorb_test_example.h delete mode 100644 src/drivers/qurt/tests/muorb/muorb_test_main.cpp delete mode 100644 src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp delete mode 160000 src/lib/DriverFramework diff --git a/.gitmodules b/.gitmodules index 6cc7d2ee81..5b5649a57c 100644 --- a/.gitmodules +++ b/.gitmodules @@ -26,10 +26,6 @@ path = src/lib/matrix url = https://github.com/PX4/Matrix.git branch = master -[submodule "src/lib/DriverFramework"] - path = src/lib/DriverFramework - url = https://github.com/PX4/DriverFramework.git - branch = master [submodule "src/lib/ecl"] path = src/lib/ecl url = https://github.com/PX4/ecl.git @@ -58,3 +54,6 @@ path = cmake/configs/uavcan_board_ident url = https://github.com/PX4/uavcan_board_ident.git branch = master +[submodule "platforms/qurt/dspal"] + path = platforms/qurt/dspal + url = https://github.com/ATLFlight/dspal.git diff --git a/CMakeLists.txt b/CMakeLists.txt index c8ce5e25fb..c445634437 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,7 +138,6 @@ set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration") include(px4_add_module) set(config_module_list) -set(config_df_driver_list) include(px4_config) include(px4_add_board) @@ -311,25 +310,6 @@ add_subdirectory(msg EXCLUDE_FROM_ALL) px4_generate_airframes_xml(BOARD ${PX4_BOARD}) -#============================================================================= -# DriverFramework -# -px4_add_git_submodule(TARGET git_driverframework PATH "src/lib/DriverFramework") -set(OS ${PX4_PLATFORM}) -add_subdirectory(src/lib/DriverFramework/framework) - -# List the DriverFramework drivers -if (DEFINED config_df_driver_list) - message("DF Drivers: ${config_df_driver_list}") -endif() - -set(df_driver_libs) -foreach(driver ${config_df_driver_list}) - add_subdirectory(src/lib/DriverFramework/drivers/${driver}) - list(APPEND df_driver_libs df_${driver}) - message("Adding DF driver: ${driver}") -endforeach() - #============================================================================= # external projects # diff --git a/Documentation/Doxyfile.in b/Documentation/Doxyfile.in index 63815d3393..f6d12430c7 100644 --- a/Documentation/Doxyfile.in +++ b/Documentation/Doxyfile.in @@ -828,8 +828,7 @@ RECURSIVE = YES # Note that relative paths are relative to the directory from which doxygen is # run. -EXCLUDE = @CMAKE_SOURCE_DIR@/src/lib/DriverFramework \ - @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \ +EXCLUDE = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \ @CMAKE_SOURCE_DIR@/src/modules/micrortps_bridge/micro-CDR \ @CMAKE_SOURCE_DIR@/src/examples \ @CMAKE_SOURCE_DIR@/src/templates diff --git a/Tools/astyle/files_to_check_code_style.sh b/Tools/astyle/files_to_check_code_style.sh index a381cf3638..3dc1582aec 100755 --- a/Tools/astyle/files_to_check_code_style.sh +++ b/Tools/astyle/files_to_check_code_style.sh @@ -10,9 +10,9 @@ fi exec find boards msg src platforms \ -path msg/templates/urtps -prune -o \ -path platforms/nuttx/NuttX -prune -o \ + -path platforms/qurt/dspal -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \ -path src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis -prune -o \ - -path src/lib/DriverFramework -prune -o \ -path src/lib/ecl -prune -o \ -path src/lib/matrix -prune -o \ -path src/lib/systemlib/uthash -prune -o \ diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index b20902efee..a076265e5a 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -34,8 +34,6 @@ px4_add_board( pwm_out_sim rc_input #telemetry # all available telemetry drivers - DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers - mpu9250 MODULES airspeed_selector attitude_estimator_q @@ -49,7 +47,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -77,7 +75,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake index 35118c1834..3870f2a656 100644 --- a/boards/atlflight/eagle/default.cmake +++ b/boards/atlflight/eagle/default.cmake @@ -32,9 +32,6 @@ add_compile_options($<$:-std=gnu++11>) add_definitions( -D__PX4_POSIX_EAGLE -D__PX4_LINUX - - # For DriverFramework - -D__DF_LINUX ) px4_add_board( @@ -71,7 +68,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -108,7 +105,7 @@ px4_add_board( sd_bench shutdown #tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/atlflight/eagle/qurt.cmake b/boards/atlflight/eagle/qurt.cmake index 3e271a25f6..921e360d3c 100644 --- a/boards/atlflight/eagle/qurt.cmake +++ b/boards/atlflight/eagle/qurt.cmake @@ -47,15 +47,10 @@ px4_add_board( barometer/bmp280 gps imu/mpu9250 - magnetometer/hmc5883 + #magnetometer/hmc5883 qshell/qurt snapdragon_pwm_out spektrum_rc - DF_DRIVERS - isl29501 - ltc2946 - mpu9250 - trone MODULES airspeed_selector attitude_estimator_q diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake index 312f7633ce..c5701519ca 100644 --- a/boards/atlflight/excelsior/default.cmake +++ b/boards/atlflight/excelsior/default.cmake @@ -32,9 +32,6 @@ add_compile_options($<$:-std=gnu++11>) add_definitions( -D__PX4_POSIX_EXCELSIOR -D__PX4_LINUX - - # For DriverFramework - -D__DF_LINUX ) px4_add_board( @@ -71,7 +68,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -108,7 +105,7 @@ px4_add_board( sd_bench shutdown #tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/atlflight/excelsior/qurt.cmake b/boards/atlflight/excelsior/qurt.cmake index 2039e65a85..420cd7d73d 100644 --- a/boards/atlflight/excelsior/qurt.cmake +++ b/boards/atlflight/excelsior/qurt.cmake @@ -51,11 +51,6 @@ px4_add_board( qshell/qurt snapdragon_pwm_out spektrum_rc - DF_DRIVERS - isl29501 - ltc2946 - mpu9250 - trone MODULES airspeed_selector attitude_estimator_q diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index cb67c801e1..0b444c9746 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -26,8 +26,6 @@ px4_add_board( pwm_out_sim rc_input #telemetry # all available telemetry drivers - DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers - mpu9250 MODULES airspeed_selector attitude_estimator_q @@ -41,7 +39,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -70,7 +68,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/beaglebone/blue/src/init.cpp b/boards/beaglebone/blue/src/init.cpp index a24df2ec68..7a3f8dbab2 100644 --- a/boards/beaglebone/blue/src/init.cpp +++ b/boards/beaglebone/blue/src/init.cpp @@ -49,7 +49,6 @@ int rc_init(void) #ifdef __RC_V0_3 return rc_initialize(); #else -#ifdef __DF_BBBLUE if (rc_get_state() == RUNNING) { return 0; } @@ -111,7 +110,6 @@ int rc_init(void) //i2c, barometer and mpu will be initialized later rc_set_state(RUNNING); -#endif return 0; #endif @@ -123,7 +121,6 @@ void rc_cleaning(void) #ifdef __RC_V0_3 rc_cleanup(); return ; #else -#ifdef __DF_BBBLUE if (rc_get_state() == EXITING) { return; } @@ -136,5 +133,4 @@ void rc_cleaning(void) rc_remove_pid_file(); #endif -#endif } diff --git a/boards/emlid/navio2/cmake/upload.cmake b/boards/emlid/navio2/cmake/upload.cmake index faf5f9df1d..94567474a6 100644 --- a/boards/emlid/navio2/cmake/upload.cmake +++ b/boards/emlid/navio2/cmake/upload.cmake @@ -31,7 +31,7 @@ # ############################################################################ -if($ENV{AUTOPILOT_HOST}) +if(DEFINED ENV{AUTOPILOT_HOST}) set(AUTOPILOT_HOST $ENV{AUTOPILOT_HOST}) else() set(AUTOPILOT_HOST "navio") diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index f8400a57e8..32358546fc 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -28,11 +28,6 @@ px4_add_board( pwm_out_sim rc_input #telemetry # all available telemetry drivers - DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers - isl29501 - lsm9ds1 - mpu9250 - trone MODULES airspeed_selector attitude_estimator_q @@ -46,7 +41,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -75,7 +70,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 90b384a5d1..4fdb116679 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -39,7 +39,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -68,7 +68,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/px4/sitl/default.cmake b/boards/px4/sitl/default.cmake index 298538be92..c7eafcfd5c 100644 --- a/boards/px4/sitl/default.cmake +++ b/boards/px4/sitl/default.cmake @@ -31,7 +31,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -65,7 +65,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/px4/sitl/rtps.cmake b/boards/px4/sitl/rtps.cmake index 5c45701912..52b85ecb88 100644 --- a/boards/px4/sitl/rtps.cmake +++ b/boards/px4/sitl/rtps.cmake @@ -32,7 +32,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -67,7 +67,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/boards/px4/sitl/test.cmake b/boards/px4/sitl/test.cmake index 6147192344..36de8c309c 100644 --- a/boards/px4/sitl/test.cmake +++ b/boards/px4/sitl/test.cmake @@ -31,7 +31,7 @@ px4_add_board( fw_pos_control_l1 land_detector landing_target_estimator - load_mon + #load_mon local_position_estimator logger mavlink @@ -65,7 +65,7 @@ px4_add_board( sd_bench shutdown tests # tests and test runner - top + #top topic_listener tune_control ver diff --git a/cmake/px4_add_board.cmake b/cmake/px4_add_board.cmake index 021f58c61e..3d9de012be 100644 --- a/cmake/px4_add_board.cmake +++ b/cmake/px4_add_board.cmake @@ -55,7 +55,6 @@ # [ SYSTEMCMDS ] # [ EXAMPLES ] # [ SERIAL_PORTS ] -# [ DF_DRIVERS ] # [ CONSTRAINED_FLASH ] # [ TESTING ] # ) @@ -77,7 +76,6 @@ # SYSTEMCMDS : list of system commands to build for this board (relative to src/systemcmds) # EXAMPLES : list of example modules to build for this board (relative to src/examples) # SERIAL_PORTS : mapping of user configurable serial ports and param facing name -# DF_DRIVERS : list of DriverFramework device drivers (includes DriverFramework driver and wrapper) # CONSTRAINED_FLASH : flag to enable constrained flash options (eg limit init script status text) # TESTING : flag to enable automatic inclusion of PX4 testing modules # @@ -148,7 +146,6 @@ function(px4_add_board) SYSTEMCMDS EXAMPLES SERIAL_PORTS - DF_DRIVERS OPTIONS BUILD_BOOTLOADER CONSTRAINED_FLASH @@ -264,19 +261,6 @@ function(px4_add_board) endforeach() endif() - # DriverFramework drivers - if(DF_DRIVERS) - set(config_df_driver_list) - foreach(driver ${DF_DRIVERS}) - list(APPEND config_df_driver_list ${driver}) - - if(EXISTS "${PX4_SOURCE_DIR}/src/drivers/driver_framework_wrapper/df_${driver}_wrapper") - list(APPEND config_module_list drivers/driver_framework_wrapper/df_${driver}_wrapper) - endif() - endforeach() - set(config_df_driver_list ${config_df_driver_list} PARENT_SCOPE) - endif() - # add board config directory src to build modules file(RELATIVE_PATH board_support_src_rel ${PX4_SOURCE_DIR}/src ${PX4_BOARD_DIR}) list(APPEND config_module_list ${board_support_src_rel}/src) diff --git a/cmake/px4_add_common_flags.cmake b/cmake/px4_add_common_flags.cmake index 523669564e..7ab11c195a 100644 --- a/cmake/px4_add_common_flags.cmake +++ b/cmake/px4_add_common_flags.cmake @@ -177,7 +177,6 @@ function(px4_add_common_flags) ${PX4_SOURCE_DIR}/src ${PX4_SOURCE_DIR}/src/include ${PX4_SOURCE_DIR}/src/lib - ${PX4_SOURCE_DIR}/src/lib/DriverFramework/framework/include ${PX4_SOURCE_DIR}/src/lib/matrix ${PX4_SOURCE_DIR}/src/modules ) diff --git a/cmake/px4_add_module.cmake b/cmake/px4_add_module.cmake index 080f0c68b5..ba3a74d9ec 100644 --- a/cmake/px4_add_module.cmake +++ b/cmake/px4_add_module.cmake @@ -132,7 +132,7 @@ function(px4_add_module) endforeach() endif() - elseif(DYNAMIC AND MAIN AND (${OS} STREQUAL "posix")) + elseif(DYNAMIC AND MAIN AND (${PX4_PLATFORM} STREQUAL "posix")) add_library(${MODULE} SHARED ${SRCS}) target_compile_definitions(${MODULE} PRIVATE ${MAIN}_main=px4_module_main) set_target_properties(${MODULE} PROPERTIES diff --git a/platforms/common/apps.cpp.in b/platforms/common/apps.cpp.in index 0a78d210cd..a56e63d516 100644 --- a/platforms/common/apps.cpp.in +++ b/platforms/common/apps.cpp.in @@ -19,25 +19,17 @@ ${builtin_apps_decl_string} int shutdown_main(int argc, char *argv[]); int list_tasks_main(int argc, char *argv[]); int list_files_main(int argc, char *argv[]); -int list_devices_main(int argc, char *argv[]); -int list_topics_main(int argc, char *argv[]); int sleep_main(int argc, char *argv[]); -int wait_for_topic(int argc, char *argv[]); } -extern void px4_show_devices(void); - void init_app_map(apps_map_type &apps) { -${builtin_apps_string} + ${builtin_apps_string} apps["shutdown"] = shutdown_main; apps["list_tasks"] = list_tasks_main; apps["list_files"] = list_files_main; - apps["list_devices"] = list_devices_main; - apps["list_topics"] = list_topics_main; apps["sleep"] = sleep_main; - apps["wait_for_topic"] = wait_for_topic; } void list_builtins(apps_map_type &apps) @@ -60,18 +52,6 @@ int list_tasks_main(int argc, char *argv[]) return 0; } -int list_devices_main(int argc, char *argv[]) -{ - px4_show_devices(); - return 0; -} - -int list_topics_main(int argc, char *argv[]) -{ - px4_show_topics(); - return 0; -} - int list_files_main(int argc, char *argv[]) { px4_show_files(); @@ -90,32 +70,3 @@ int sleep_main(int argc, char *argv[]) px4_usleep(usecs); return 0; } - -#include "uORB/uORB.h" - -int wait_for_topic(int argc, char *argv[]) -{ - if (argc < 2 || argc > 3) { - printf("Usage: wait_for_topic [timeout_sec]\n"); - return 1; - } - - struct orb_metadata meta = { .o_name = argv[1], - .o_size = 0, - .o_size_no_padding = 0, - .o_fields = nullptr }; - - unsigned int timeout = (argc == 3) ? (unsigned int)atoi(argv[2]) : 0; - unsigned int elapsed = 0; - - printf("Waiting for topic %s timeout %u\n", argv[1], timeout); - - while (orb_exists(&meta, 0) != 0 && (timeout && (elapsed < timeout))) - { - px4_sleep(1); - elapsed += 1; - } - - return 0; -} - diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h index 817f836438..7038130b2f 100644 --- a/platforms/common/include/px4_platform_common/defines.h +++ b/platforms/common/include/px4_platform_common/defines.h @@ -54,19 +54,6 @@ constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); } constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } #endif /* __cplusplus */ -#if defined(__PX4_NUTTX) || defined(__PX4_POSIX) -/**************************************************************************** - * Building for NuttX or POSIX. - ****************************************************************************/ - -/* Main entry point */ -#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[]) - -#else // defined(__PX4_NUTTX) || defined(__PX4_POSIX) -/****************************************************************************/ -#error "No target OS defined" -#endif - #if defined(__PX4_NUTTX) /**************************************************************************** * NuttX specific defines. @@ -99,9 +86,6 @@ constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } // NuttX _IOC is equivalent to Linux _IO #define _PX4_IOC(x,y) _IO(x,y) -/* FIXME - Used to satisfy build */ -#define getreg32(a) (*(volatile uint32_t *)(a)) - #define USEC_PER_TICK (1000000/PX4_TICKS_PER_SEC) #define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK) @@ -111,7 +95,6 @@ constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); } # include "dspal_math.h" # define PX4_ROOTFSDIR "." # define PX4_TICKS_PER_SEC 1000L -# define SIOCDEVPRIVATE 999999 #else // __PX4_QURT diff --git a/platforms/common/include/px4_platform_common/posix.h b/platforms/common/include/px4_platform_common/posix.h index ecac16050e..8395bca00d 100644 --- a/platforms/common/include/px4_platform_common/posix.h +++ b/platforms/common/include/px4_platform_common/posix.h @@ -44,7 +44,6 @@ #include #include #include -#include #include #if defined(__PX4_QURT) @@ -60,7 +59,10 @@ #ifdef __PX4_NUTTX +#include + typedef struct pollfd px4_pollfd_struct_t; +typedef pollevent_t px4_pollevent_t; #if defined(__cplusplus) #define _GLOBAL :: @@ -73,7 +75,6 @@ typedef struct pollfd px4_pollfd_struct_t; #define px4_write _GLOBAL write #define px4_read _GLOBAL read #define px4_poll _GLOBAL poll -#define px4_fsync _GLOBAL fsync #define px4_access _GLOBAL access #define px4_getpid _GLOBAL getpid @@ -83,34 +84,31 @@ typedef struct pollfd px4_pollfd_struct_t; #define PX4_STACK_OVERHEAD (1024 * 11) -/** - * Terminates the calling process immediately. - * @return 0 on success, 1 on error - */ -#define px4_exit(status) ({return status;}) - __BEGIN_DECLS -typedef short pollevent_t; +typedef short px4_pollevent_t; typedef struct { /* This part of the struct is POSIX-like */ int fd; /* The descriptor being polled */ - pollevent_t events; /* The input event flags */ - pollevent_t revents; /* The output event flags */ + px4_pollevent_t events; /* The input event flags */ + px4_pollevent_t revents; /* The output event flags */ /* Required for PX4 compatibility */ px4_sem_t *sem; /* Pointer to semaphore used to post output event */ void *priv; /* For use by drivers */ } px4_pollfd_struct_t; +#ifndef POLLIN +#define POLLIN (0x01) +#endif + __EXPORT int px4_open(const char *path, int flags, ...); __EXPORT int px4_close(int fd); __EXPORT ssize_t px4_read(int fd, void *buffer, size_t buflen); __EXPORT ssize_t px4_write(int fd, const void *buffer, size_t buflen); __EXPORT int px4_ioctl(int fd, int cmd, unsigned long arg); -__EXPORT int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout); -__EXPORT int px4_fsync(int fd); +__EXPORT int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout); __EXPORT int px4_access(const char *pathname, int mode); __EXPORT px4_task_t px4_getpid(void); @@ -127,13 +125,7 @@ __END_DECLS #define PX4_STACK_ADJUSTED(_s) (_s * (__SIZEOF_POINTER__ >> 2) + PX4_STACK_OVERHEAD) __BEGIN_DECLS -extern int px4_errno; -__EXPORT void px4_show_devices(void); __EXPORT void px4_show_files(void); -__EXPORT const char *px4_get_device_names(unsigned int *handle); - -__EXPORT void px4_show_topics(void); -__EXPORT const char *px4_get_topic_names(unsigned int *handle); __END_DECLS diff --git a/platforms/common/shutdown.cpp b/platforms/common/shutdown.cpp index 3a80adb0d0..0c3cd7cffd 100644 --- a/platforms/common/shutdown.cpp +++ b/platforms/common/shutdown.cpp @@ -94,6 +94,7 @@ int px4_register_shutdown_hook(shutdown_hook_t hook) { return -EINVAL; } + int px4_unregister_shutdown_hook(shutdown_hook_t hook) { return -EINVAL; diff --git a/platforms/nuttx/cmake/px4_impl_os.cmake b/platforms/nuttx/cmake/px4_impl_os.cmake index 0e4a9441b3..a9ce19a321 100644 --- a/platforms/nuttx/cmake/px4_impl_os.cmake +++ b/platforms/nuttx/cmake/px4_impl_os.cmake @@ -69,7 +69,6 @@ function(px4_os_add_flags) add_definitions( -D__PX4_NUTTX - -D__DF_NUTTX -D_SYS_CDEFS_H_ # skip toolchain's -D_SYS_REENT_H_ # skip toolchain's diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index 4f37cc064c..124b1a1ae6 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -40,7 +40,7 @@ if (NOT ${PX4_BOARD} MATCHES "px4_io") board_fat_dma_alloc.c console_buffer.cpp gpio.c - px4_nuttx_tasks.c + tasks.cpp px4_nuttx_impl.cpp px4_init.cpp ) diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h b/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h index 7d2ee5b256..96615af30d 100644 --- a/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h +++ b/platforms/nuttx/src/px4/common/include/px4_platform/micro_hal.h @@ -34,7 +34,6 @@ __BEGIN_DECLS -#include /* For historical reasons (NuttX STM32 numbering) PX4 bus numbering is 1 based * All PX4 code, including, board code is written to assuming 1 based numbering. diff --git a/platforms/nuttx/src/px4/common/px4_nuttx_tasks.c b/platforms/nuttx/src/px4/common/tasks.cpp similarity index 98% rename from platforms/nuttx/src/px4/common/px4_nuttx_tasks.c rename to platforms/nuttx/src/px4/common/tasks.cpp index 3364c5c589..d50646ffb0 100644 --- a/platforms/nuttx/src/px4/common/px4_nuttx_tasks.c +++ b/platforms/nuttx/src/px4/common/tasks.cpp @@ -33,11 +33,13 @@ ****************************************************************************/ /** - * @file px4_nuttx_tasks.c + * @file tasks.cpp * Implementation of existing task API for NuttX */ -#include +#include +#include +#include #include #include @@ -50,10 +52,6 @@ #include #include -#include -#include -#include - void px4_systemreset(bool to_bootloader) { diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 52224e7941..1987d8d6ce 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -42,7 +42,6 @@ if (("${PX4_BOARD}" MATCHES "atlflight_eagle") OR ("${PX4_BOARD}" MATCHES "atlfl px4_layer parameters ${module_libraries} - ${df_driver_libs} ${FASTRPC_ARM_LIBS} pthread m rt -Wl,--end-group @@ -57,8 +56,6 @@ else() target_link_libraries(px4 PRIVATE ${module_libraries} - ${df_driver_libs} - df_driver_framework pthread m # horrible circular dependencies that need to be teased apart @@ -109,8 +106,6 @@ endif() if("${PX4_BOARD}" MATCHES "beaglebone_blue") target_link_libraries(px4 PRIVATE robotics_cape) - add_dependencies(df_driver_framework librobotcontrol) - add_dependencies(df_mpu9250 librobotcontrol) elseif("${PX4_BOARD}" MATCHES "emlid_navio2") target_link_libraries(px4 PRIVATE atomic) diff --git a/platforms/posix/cmake/px4_impl_os.cmake b/platforms/posix/cmake/px4_impl_os.cmake index 6b4cd9088a..eac23a788e 100644 --- a/platforms/posix/cmake/px4_impl_os.cmake +++ b/platforms/posix/cmake/px4_impl_os.cmake @@ -223,10 +223,7 @@ function(px4_os_add_flags) if ("${PX4_BOARD}" MATCHES "sitl") if(UNIX AND APPLE) - add_definitions( - -D__PX4_DARWIN - -D__DF_DARWIN - ) + add_definitions(-D__PX4_DARWIN) if (CMAKE_CXX_COMPILER_VERSION VERSION_LESS 8.0) message(FATAL_ERROR "PX4 Firmware requires XCode 8 or newer on Mac OS. Version installed on this system: ${CMAKE_CXX_COMPILER_VERSION}") @@ -251,46 +248,21 @@ function(px4_os_add_flags) -U__CUSTOM_FILE_IO__ ) else() - add_definitions( - -D__PX4_LINUX - -D__DF_LINUX - ) + add_definitions(-D__PX4_LINUX) endif() elseif (("${PX4_BOARD}" MATCHES "navio2") OR ("${PX4_BOARD}" MATCHES "raspberrypi")) - #TODO: move to board support - - add_definitions( - -D__PX4_LINUX - - # For DriverFramework - -D__DF_LINUX - -D__DF_RPI - ) + add_definitions(-D__PX4_LINUX) elseif ("${PX4_BOARD}" MATCHES "aerotenna_ocpoc") - #TODO: move to board support - - add_definitions( - -D__PX4_LINUX - -D__PX4_POSIX_OCPOC # TODO: remove - - # For DriverFramework - -D__DF_LINUX - -D__DF_OCPOC - ) + add_definitions(-D__PX4_LINUX) elseif ("${PX4_BOARD}" MATCHES "beaglebone_blue") #TODO: move to board support add_definitions( -D__PX4_LINUX - -D__PX4_POSIX_BBBLUE # TODO: remove - - # For DriverFramework - -D__DF_LINUX - -D__DF_BBBLUE -DRC_AUTOPILOT_EXT # Enable extensions in Robotics Cape Library, TODO: remove ) diff --git a/platforms/posix/include/arch/board/board.h b/platforms/posix/include/arch/board/board.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/platforms/posix/src/px4/common/CMakeLists.txt b/platforms/posix/src/px4/common/CMakeLists.txt index 71ed5b6c68..07457652f3 100644 --- a/platforms/posix/src/px4/common/CMakeLists.txt +++ b/platforms/posix/src/px4/common/CMakeLists.txt @@ -50,7 +50,7 @@ endif() add_library(px4_layer px4_posix_impl.cpp - px4_posix_tasks.cpp + tasks.cpp px4_sem.cpp px4_init.cpp lib_crc32.c diff --git a/platforms/posix/src/px4/common/main.cpp b/platforms/posix/src/px4/common/main.cpp index 7ed6a1a883..30e185a2cd 100644 --- a/platforms/posix/src/px4/common/main.cpp +++ b/platforms/posix/src/px4/common/main.cpp @@ -71,7 +71,6 @@ #include #include "apps.h" -#include "DriverFramework.hpp" #include "px4_daemon/client.h" #include "px4_daemon/server.h" #include "px4_daemon/pxh.h" @@ -281,8 +280,6 @@ int main(int argc, char **argv) return ret; } - DriverFramework::Framework::initialize(); - px4::init_once(); px4::init(argc, argv, "px4"); diff --git a/platforms/posix/src/px4/common/px4_posix_impl.cpp b/platforms/posix/src/px4/common/px4_posix_impl.cpp index 1daa817dae..385c1a2445 100644 --- a/platforms/posix/src/px4/common/px4_posix_impl.cpp +++ b/platforms/posix/src/px4/common/px4_posix_impl.cpp @@ -68,7 +68,6 @@ void init_once(); void init_once() { _shell_task_id = pthread_self(); - //printf("[init] shell id: %lu\n", (unsigned long)_shell_task_id); work_queues_init(); hrt_work_queue_init(); diff --git a/platforms/posix/src/px4/common/px4_posix_tasks.cpp b/platforms/posix/src/px4/common/tasks.cpp similarity index 100% rename from platforms/posix/src/px4/common/px4_posix_tasks.cpp rename to platforms/posix/src/px4/common/tasks.cpp diff --git a/platforms/posix/src/px4/common/test_stubs/CMakeLists.txt b/platforms/posix/src/px4/common/test_stubs/CMakeLists.txt index 1950cb8c8a..8b8c0ad9b7 100644 --- a/platforms/posix/src/px4/common/test_stubs/CMakeLists.txt +++ b/platforms/posix/src/px4/common/test_stubs/CMakeLists.txt @@ -33,7 +33,6 @@ set(SRCS stub_daemon.cpp - stub_devmgr.cpp stub_parameter.cpp ) diff --git a/platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp b/platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp deleted file mode 100644 index 374712f7ba..0000000000 --- a/platforms/posix/src/px4/common/test_stubs/stub_devmgr.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "stub_devmgr.h" - - -namespace DriverFramework -{ -int DevMgr::getNextDeviceName(unsigned int &index, const char **instancename) -{ - return stub_getNextDeviceName_callback(index, instancename); -} -} diff --git a/platforms/posix/src/px4/common/test_stubs/stub_devmgr.h b/platforms/posix/src/px4/common/test_stubs/stub_devmgr.h deleted file mode 100644 index 15840c889f..0000000000 --- a/platforms/posix/src/px4/common/test_stubs/stub_devmgr.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include "DevMgr.hpp" - -#include - -std::function stub_getNextDeviceName_callback = [](unsigned int &, -const char **) {return 0;}; diff --git a/platforms/qurt/CMakeLists.txt b/platforms/qurt/CMakeLists.txt index 44dc4b1093..a7bf297126 100644 --- a/platforms/qurt/CMakeLists.txt +++ b/platforms/qurt/CMakeLists.txt @@ -1,5 +1,7 @@ -list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/boards/atlflight/cmake_hexagon") +px4_add_git_submodule(TARGET git_dspal PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal") + +list(APPEND CMAKE_MODULE_PATH "${PX4_SOURCE_DIR}/platforms/qurt/dspal/cmake_hexagon") include(toolchain/Toolchain-qurt) include(fastrpc) include(qurt_lib) @@ -33,7 +35,7 @@ if ("${QURT_ENABLE_STUBS}" STREQUAL "1") ${PX4_BINARY_DIR}/platforms/qurt/px4muorb_skel.c ) - target_link_libraries(px4 PRIVATE ${module_libraries} ${df_driver_libs}) + target_link_libraries(px4 PRIVATE ${module_libraries}) else() # Generate the DSP lib and stubs but not the apps side executable @@ -45,8 +47,6 @@ else() LINK_LIBS modules__muorb__adsp ${module_libraries} - ${df_driver_libs} - df_driver_framework m ) diff --git a/platforms/qurt/cmake/px4_impl_os.cmake b/platforms/qurt/cmake/px4_impl_os.cmake index a3fa032bf8..054ab22c65 100644 --- a/platforms/qurt/cmake/px4_impl_os.cmake +++ b/platforms/qurt/cmake/px4_impl_os.cmake @@ -108,24 +108,20 @@ endfunction() # function(px4_os_add_flags) - set(DSPAL_ROOT src/lib/DriverFramework/dspal) + set(DSPAL_ROOT platforms/qurt/dspal) include_directories( ${DSPAL_ROOT}/include - ${DSPAL_ROOT}/mpu_spi/inc ${DSPAL_ROOT}/sys ${DSPAL_ROOT}/sys/sys - ${DSPAL_ROOT}/uart_esc/inc platforms/posix/include platforms/qurt/include - ) + ) add_definitions( -D__PX4_POSIX -D__PX4_QURT - - -D__DF_QURT # For DriverFramework - ) + ) add_compile_options( -fPIC @@ -139,8 +135,6 @@ function(px4_os_add_flags) set(CMAKE_SHARED_LIBRARY_LINK_C_FLAGS) set(CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS) - set(DF_TARGET "qurt" CACHE STRING "DriverFramework target" FORCE) - endfunction() #============================================================================= diff --git a/platforms/qurt/dspal b/platforms/qurt/dspal new file mode 160000 index 0000000000..0322a4e345 --- /dev/null +++ b/platforms/qurt/dspal @@ -0,0 +1 @@ +Subproject commit 0322a4e345e48ea28cb1cee14a33033cdaf0b16a diff --git a/platforms/qurt/include/arch/board/board.h b/platforms/qurt/include/arch/board/board.h deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/platforms/qurt/include/poll.h b/platforms/qurt/include/poll.h deleted file mode 100644 index 59f1809e2a..0000000000 --- a/platforms/qurt/include/poll.h +++ /dev/null @@ -1,40 +0,0 @@ -/**************************************************************************** - * include/poll.h - * - * Copyright (C) 2008-2009 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt - * - * 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 NuttX nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -typedef unsigned int nfds_t; - -#define POLLIN (0x01) diff --git a/platforms/qurt/include/sys/ioctl.h b/platforms/qurt/include/sys/ioctl.h deleted file mode 100644 index 0d08f75231..0000000000 --- a/platforms/qurt/include/sys/ioctl.h +++ /dev/null @@ -1,3 +0,0 @@ -#pragma once - -#define _IO(x,y) (x+y) diff --git a/platforms/qurt/src/px4/common/CMakeLists.txt b/platforms/qurt/src/px4/common/CMakeLists.txt index 667826082f..c64e4765d1 100644 --- a/platforms/qurt/src/px4/common/CMakeLists.txt +++ b/platforms/qurt/src/px4/common/CMakeLists.txt @@ -38,7 +38,7 @@ include_directories(${HEXAGON_8074_INCLUDES}) set(QURT_LAYER_SRCS px4_qurt_impl.cpp - px4_qurt_tasks.cpp + tasks.cpp lib_crc32.c drv_hrt.cpp qurt_stubs.c diff --git a/platforms/qurt/src/px4/common/commands_hil.c b/platforms/qurt/src/px4/common/commands_hil.c index 682707d08b..4a5a4f22a7 100644 --- a/platforms/qurt/src/px4/common/commands_hil.c +++ b/platforms/qurt/src/px4/common/commands_hil.c @@ -93,10 +93,8 @@ const char *get_commands() "param set MAV_TYPE 2\n" "mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n" - "list_devices\n" "list_files\n" "list_tasks\n" - "list_topics\n" "sleep 10\n" "list_tasks\n" "sleep 10\n" diff --git a/platforms/qurt/src/px4/common/main.cpp b/platforms/qurt/src/px4/common/main.cpp index 174a6f48f8..5d69ae9491 100644 --- a/platforms/qurt/src/px4/common/main.cpp +++ b/platforms/qurt/src/px4/common/main.cpp @@ -52,7 +52,6 @@ #include "get_commands.h" #include "apps.h" -#include "DriverFramework.hpp" #define MAX_ARGS 8 // max number of whitespace separated args after app name @@ -196,7 +195,6 @@ int dspal_entry(int argc, char *argv[]) PX4_INFO("In dspal_entry"); apps_map_type apps; init_app_map(apps); - DriverFramework::Framework::initialize(); px4::init_once(); px4::init(argc, (char **)argv, "px4"); process_commands(apps, get_commands()); diff --git a/platforms/qurt/src/px4/common/px4_qurt_tasks.cpp b/platforms/qurt/src/px4/common/tasks.cpp similarity index 100% rename from platforms/qurt/src/px4/common/px4_qurt_tasks.cpp rename to platforms/qurt/src/px4/common/tasks.cpp diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index 366de5ad6f..7bd79d6a72 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -50,7 +50,7 @@ dataman start bmp280 -I start -df_mpu9250_wrapper start +mpu9250 start # options: -R rotation gps start -d /dev/ttyS2 -s -p ubx diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 086ba00492..e4c656f20d 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -50,7 +50,7 @@ dataman start bmp280 -I start -df_mpu9250_wrapper start +mpu9250 start # options: -R rotation gps start -d /dev/ttyS2 -s -p ubx diff --git a/posix-configs/eagle/200qx/px4.config b/posix-configs/eagle/200qx/px4.config index 781a166b96..9eb017d110 100644 --- a/posix-configs/eagle/200qx/px4.config +++ b/posix-configs/eagle/200qx/px4.config @@ -14,17 +14,11 @@ param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.08 param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.001 -param set ATT_W_MAG 0.00 param set SENS_BOARD_ROT 0 -param set RC_RECEIVER_TYPE 1 -param set UART_ESC_MODEL 2 -param set UART_ESC_BAUD 250000 -param set UART_ESC_MOTOR1 4 -param set UART_ESC_MOTOR2 2 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 + + sleep 1 -df_mpu9250_wrapper start +mpu9250 start bmp280 start gps start -d /dev/tty-4 rc_update start @@ -38,7 +32,5 @@ mc_rate_control start uart_esc start -D /dev/tty-2 spektrum_rc start -d /dev/tty-1 sleep 1 -list_devices list_files list_tasks -list_topics diff --git a/posix-configs/eagle/210qc/px4.config b/posix-configs/eagle/210qc/px4.config index 781a166b96..36c69c9149 100644 --- a/posix-configs/eagle/210qc/px4.config +++ b/posix-configs/eagle/210qc/px4.config @@ -16,15 +16,9 @@ param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.001 param set ATT_W_MAG 0.00 param set SENS_BOARD_ROT 0 -param set RC_RECEIVER_TYPE 1 -param set UART_ESC_MODEL 2 -param set UART_ESC_BAUD 250000 -param set UART_ESC_MOTOR1 4 -param set UART_ESC_MOTOR2 2 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 + sleep 1 -df_mpu9250_wrapper start +mpu9250 start bmp280 start gps start -d /dev/tty-4 rc_update start @@ -38,7 +32,5 @@ mc_rate_control start uart_esc start -D /dev/tty-2 spektrum_rc start -d /dev/tty-1 sleep 1 -list_devices list_files list_tasks -list_topics diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 6ab46467c8..3bb8c9fa75 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -6,11 +6,8 @@ gps start -d /dev/tty-4 param set MAV_TYPE 2 sleep 1 hmc5883 start -df_mpu9250_wrapper start_without_mag +mpu9250 start_without_mag bmp280 start -df_trone_wrapper start -#df_ltc2946_wrapper start # (currently not working on all boards...) -#df_isl29501_wrapper start sleep 1 rc_update start sensors start diff --git a/posix-configs/eagle/hil/px4.config b/posix-configs/eagle/hil/px4.config index c3a723552d..539ff4350d 100644 --- a/posix-configs/eagle/hil/px4.config +++ b/posix-configs/eagle/hil/px4.config @@ -1,42 +1,16 @@ uorb start qshell start -param set CAL_GYRO0_ID 2293760 -param set CAL_ACC0_ID 1310720 -param set CAL_ACC1_ID 1376256 -param set CAL_MAG0_ID 196608 + param set SYS_AUTOSTART 4001 param set SYS_AUTOCONFIG 1 param set MAV_TYPE 2 -param set RC1_MAX 2015 -param set RC1_MIN 996 -param set RC1_TRIM 1502 -param set RC1_REV -1 -param set RC2_MAX 2016 -param set RC2_MIN 995 -param set RC2_TRIM 1500 -param set RC3_MAX 2003 -param set RC3_MIN 992 -param set RC3_TRIM 992 -param set RC4_MAX 2011 -param set RC4_MIN 997 -param set RC4_TRIM 1504 -param set RC4_REV -1 -param set RC6_MAX 2016 -param set RC6_MIN 992 -param set RC6_TRIM 1504 -param set RC_CHAN_CNT 8 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 7 -param set RC_MAP_RETURN_SW 8 param set MC_YAW_P 1.5 param set MC_PITCH_P 3.0 param set MC_ROLL_P 3.0 param set MC_YAWRATE_P 0.2 param set MC_PITCHRATE_P 0.03 param set MC_ROLLRATE_P 0.03 -param set ATT_W_ACC 0.0002 -param set ATT_W_MAG 0.002 -param set ATT_W_GYRO_BIAS 0.05 + sleep 1 rc_update start commander start -hil @@ -49,10 +23,8 @@ land_detector start multicopter sleep 1 pwm_out_sim start mixer load /dev/pwm_output0 /startup/quad_x.main.mix -list_devices list_files list_tasks -list_topics sleep 10 list_tasks sleep 10 diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index 2e99309d78..4018151392 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -20,15 +20,10 @@ then qshell gps start -d /dev/tty-4 qshell hmc5883 start - qshell df_trone_wrapper start - #qshell df_isl29501_wrapper start # Qualcomm 200qx microheli platform elif param compare SYS_AUTOSTART 4200 then - param set CAL_GYRO0_ID 100 - param set CAL_ACC0_ID 100 - param set CAL_MAG0_ID 101 param set MAV_TYPE 2 param set MC_YAW_P 7.0 param set MC_YAWRATE_P 0.08 @@ -42,56 +37,11 @@ then param set MC_ROLLRATE_P 0.08 param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.0001 - param set RC_MAP_THROTTLE 1 - param set RC_MAP_ROLL 2 - param set RC_MAP_PITCH 3 - param set RC_MAP_YAW 4 - param set RC_MAP_MODE_SW 5 - param set RC_MAP_POSCTL_SW 6 - param set RC1_MAX 1900 - param set RC1_MIN 1099 - param set RC1_TRIM 1099 - param set RC1_REV 1 - param set RC2_MAX 1900 - param set RC2_MIN 1099 - param set RC2_TRIM 1500 - param set RC2_REV -1 - param set RC3_MAX 1900 - param set RC3_MIN 1099 - param set RC3_TRIM 1500 - param set RC3_REV 1 - param set RC4_MAX 1900 - param set RC4_MIN 1099 - param set RC4_TRIM 1500 - param set RC4_REV -1 - param set RC5_MAX 1900 - param set RC5_MIN 1099 - param set RC5_TRIM 1500 - param set RC5_REV 1 - param set RC6_MAX 1900 - param set RC6_MIN 1099 - param set RC6_TRIM 1099 - param set RC6_REV 1 - param set ATT_W_MAG 0.00 - param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 0 - param set RC_RECEIVER_TYPE 1 - param set UART_ESC_MODEL 2 - param set UART_ESC_BAUDRTE 250000 - param set UART_ESC_MOTOR1 4 - param set UART_ESC_MOTOR2 2 - param set UART_ESC_MOTOR3 1 - param set UART_ESC_MOTOR4 3 - - qshell uart_esc start -D /dev/tty-2 - qshell rc_receiver start -D /dev/tty-1 # Qualcomm internal 210qc platform elif param compare SYS_AUTOSTART 4210 then - param set CAL_GYRO0_ID 100 - param set CAL_ACC0_ID 100 - param set CAL_MAG0_ID 101 param set MAV_TYPE 2 param set MC_YAW_P 12 param set MC_YAWRATE_P 0.08 @@ -105,64 +55,8 @@ then param set MC_ROLLRATE_P 0.08 param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.001 - param set RC_MAP_THROTTLE 1 - param set RC_MAP_ROLL 2 - param set RC_MAP_PITCH 3 - param set RC_MAP_YAW 4 - param set RC_MAP_MODE_SW 5 - param set RC_MAP_POSCTL_SW 6 - param set RC1_MAX 1900 - param set RC1_MIN 1099 - param set RC1_TRIM 1099 - param set RC1_REV 1 - param set RC2_MAX 1900 - param set RC2_MIN 1099 - param set RC2_TRIM 1500 - param set RC2_REV -1 - param set RC3_MAX 1900 - param set RC3_MIN 1099 - param set RC3_TRIM 1500 - param set RC3_REV 1 - param set RC4_MAX 1900 - param set RC4_MIN 1099 - param set RC4_TRIM 1500 - param set RC4_REV -1 - param set RC5_MAX 1900 - param set RC5_MIN 1099 - param set RC5_TRIM 1500 - param set RC5_REV 1 - param set RC6_MAX 1900 - param set RC6_MIN 1099 - param set RC6_TRIM 1099 - param set RC6_REV 1 - param set ATT_W_MAG 0.00 - param set PE_MAG_NOISE 1.0f param set SENS_BOARD_ROT 0 - param set CAL_GYRO0_XOFF -0.0028 - param set CAL_GYRO0_YOFF -0.0047 - param set CAL_GYRO0_ZOFF -0.0034 - param set CAL_MAG0_XOFF 0.0000 - param set CAL_MAG0_YOFF 0.0000 - param set CAL_MAG0_ZOFF 0.0000 - param set CAL_MAG0_XSCALE 1.0000 - param set CAL_MAG0_YSCALE 1.0000 - param set CAL_MAG0_ZSCALE 1.0000 - param set CAL_ACC0_XOFF -0.0941 - param set CAL_ACC0_YOFF 0.1168 - param set CAL_ACC0_ZOFF -0.0398 - param set CAL_ACC0_XSCALE 1.0004 - param set CAL_ACC0_YSCALE 0.9974 - param set CAL_ACC0_ZSCALE 0.9951 - param set RC_RECEIVER_TYPE 1 - param set UART_ESC_MODEL 2 - param set UART_ESC_BAUDRTE 250000 - param set UART_ESC_MOTOR1 4 - param set UART_ESC_MOTOR2 2 - param set UART_ESC_MOTOR3 1 - param set UART_ESC_MOTOR4 3 - uart_esc start -D /dev/tty-2 - rc_receiver start -D /dev/tty-1 else echo "NO AIRFRAME CHOSEN!" echo "Please set parameter to select airframe:" @@ -171,7 +65,7 @@ else fi -qshell df_mpu9250_wrapper start +qshell mpu9250 start qshell bmp280 start qshell rc_update start qshell sensors start diff --git a/posix-configs/excelsior/px4.config b/posix-configs/excelsior/px4.config index 6ae7889daf..45f7f0866e 100644 --- a/posix-configs/excelsior/px4.config +++ b/posix-configs/excelsior/px4.config @@ -1,8 +1,5 @@ uorb start qshell start -param set CAL_GYRO0_ID 100 -param set CAL_ACC0_ID 100 -param set CAL_MAG0_ID 101 param set SYS_AUTOSTART 4001 param set MAV_TYPE 2 param set MC_YAW_P 12 @@ -17,62 +14,10 @@ param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.08 param set MC_ROLLRATE_I 0.0 param set MC_ROLLRATE_D 0.001 -param set RC_MAP_THROTTLE 1 -param set RC_MAP_ROLL 2 -param set RC_MAP_PITCH 3 -param set RC_MAP_YAW 4 -param set RC_MAP_MODE_SW 5 -param set RC_MAP_POSCTL_SW 6 -param set RC1_MAX 1900 -param set RC1_MIN 1099 -param set RC1_TRIM 1099 -param set RC1_REV 1 -param set RC2_MAX 1900 -param set RC2_MIN 1099 -param set RC2_TRIM 1500 -param set RC2_REV -1 -param set RC3_MAX 1900 -param set RC3_MIN 1099 -param set RC3_TRIM 1500 -param set RC3_REV 1 -param set RC4_MAX 1900 -param set RC4_MIN 1099 -param set RC4_TRIM 1500 -param set RC4_REV -1 -param set RC5_MAX 1900 -param set RC5_MIN 1099 -param set RC5_TRIM 1500 -param set RC5_REV 1 -param set RC6_MAX 1900 -param set RC6_MIN 1099 -param set RC6_TRIM 1099 -param set RC6_REV 1 -param set ATT_W_MAG 0.00 param set SENS_BOARD_ROT 4 -param set CAL_GYRO0_XOFF -0.0028 -param set CAL_GYRO0_YOFF -0.0047 -param set CAL_GYRO0_ZOFF -0.0034 -param set CAL_MAG0_XOFF 0.0000 -param set CAL_MAG0_YOFF 0.0000 -param set CAL_MAG0_ZOFF 0.0000 -param set CAL_MAG0_XSCALE 1.0000 -param set CAL_MAG0_YSCALE 1.0000 -param set CAL_MAG0_ZSCALE 1.0000 -param set CAL_ACC0_XOFF -0.0941 -param set CAL_ACC0_YOFF 0.1168 -param set CAL_ACC0_ZOFF -0.0398 -param set CAL_ACC0_XSCALE 1.0004 -param set CAL_ACC0_YSCALE 0.9974 -param set CAL_ACC0_ZSCALE 0.9951 -param set RC_RECEIVER_TYPE 1 -param set UART_ESC_MODEL 2 -param set UART_ESC_BAUD 250000 -param set UART_ESC_MOTOR1 4 -param set UART_ESC_MOTOR2 2 -param set UART_ESC_MOTOR3 1 -param set UART_ESC_MOTOR4 3 + sleep 1 -df_mpu9250_wrapper start +mpu9250 start bmp280 start rc_update start sensors start @@ -85,7 +30,5 @@ mc_rate_control start uart_esc start -D /dev/tty-1 spektrum_rc start -d /dev/tty-101 sleep 1 -list_devices list_files list_tasks -list_topics diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index 2baf757dc3..95e5006610 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -15,9 +15,10 @@ param set MAV_BROADCAST 1 param set MAV_TYPE 2 param set MAV_SYS_ID 1 -df_mpu9250_wrapper start_without_mag -R 10 +mpu9250 -R 10 start hmc5883 start ms5611 start + rgbled start -b 1 adc start diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index e372bbb85c..1894ced585 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -19,11 +19,11 @@ param set BAT_CNT_V_CURR 0.001 dataman start -df_lsm9ds1_wrapper start -R 4 -#lsm9ds1 start -R 4 -#lsm9ds1_mag start -R 4 -#df_mpu9250_wrapper start -R 10 +mpu9250 -R 8 start +lsm9ds1 -R 4 start +lsm9ds1_mag -R 4 start ms5611 start + navio_rgbled start adc start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index 1429c7bc11..e2e139df56 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -19,11 +19,11 @@ param set BAT_CNT_V_CURR 0.001 dataman start -df_lsm9ds1_wrapper start -R 4 -#lsm9ds1 start -R 4 -#lsm9ds1_mag start -R 4 -#df_mpu9250_wrapper start -R 10 +mpu9250 -R 8 start +lsm9ds1 -R 4 start +lsm9ds1_mag -R 4 start ms5611 start + navio_rgbled start adc start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 8c6e43be80..f552b3610d 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -19,11 +19,11 @@ param set BAT_CNT_V_CURR 0.001 dataman start -df_lsm9ds1_wrapper start -R 4 -#lsm9ds1 start -R 4 -#lsm9ds1_mag start -R 4 -#df_mpu9250_wrapper start -R 10 +mpu9250 -R 8 start +lsm9ds1 -R 4 start +lsm9ds1_mag -R 4 start ms5611 start + navio_rgbled start adc start diff --git a/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt deleted file mode 100644 index dc9db8bd96..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_ak8963_wrapper - MAIN df_ak8963_wrapper - SRCS - df_ak8963_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_ak8963 - ) diff --git a/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp deleted file mode 100644 index b3581dbdee..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ak8963_wrapper/df_ak8963_wrapper.cpp +++ /dev/null @@ -1,459 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -/** - * @file df_ak8963_wrapper.cpp - * Lightweight driver to access the AK8963 of the DriverFramework. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include - -#include - -#include -#include - - -extern "C" { __EXPORT int df_ak8963_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfAK8963Wrapper : public AK8963 -{ -public: - DfAK8963Wrapper(enum Rotation rotation); - ~DfAK8963Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - - void print_calibration(); - -private: - int _publish(struct mag_sensor_data &data); - - void _update_mag_calibration(); - - orb_advert_t _mag_topic; - - int _param_update_sub; - - struct mag_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _mag_calibration; - - matrix::Dcmf _rotation_matrix; - - int _mag_orb_class_instance; - - perf_counter_t _mag_sample_perf; - -}; - -DfAK8963Wrapper::DfAK8963Wrapper(enum Rotation rotation) : - AK8963(MAG_DEVICE_PATH), - _mag_topic(nullptr), - _param_update_sub(-1), - _mag_calibration{}, - _mag_orb_class_instance(-1), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read")) -{ - // Set sane default calibration values - _mag_calibration.x_scale = 1.0f; - _mag_calibration.y_scale = 1.0f; - _mag_calibration.z_scale = 1.0f; - _mag_calibration.x_offset = 0.0f; - _mag_calibration.y_offset = 0.0f; - _mag_calibration.z_offset = 0.0f; - - // Get sensor rotation matrix - _rotation_matrix = get_rot_matrix(rotation); -} - -DfAK8963Wrapper::~DfAK8963Wrapper() -{ - perf_free(_mag_sample_perf); -} - -int DfAK8963Wrapper::start() -{ - /* Subscribe to param update topic. */ - if (_param_update_sub < 0) { - _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - } - - /* Init device and start sensor. */ - int ret = init(); - - if (ret != 0) { - PX4_ERR("AK8963 init fail: %d", ret); - return ret; - } - - ret = AK8963::start(); - - if (ret != 0) { - PX4_ERR("AK8963 start fail: %d", ret); - return ret; - } - - /* Force getting the calibration values. */ - _update_mag_calibration(); - - return 0; -} - -int DfAK8963Wrapper::stop() -{ - /* Stop sensor. */ - int ret = AK8963::stop(); - - if (ret != 0) { - PX4_ERR("AK8963 stop fail: %d", ret); - return ret; - } - - return 0; -} - -void DfAK8963Wrapper::_update_mag_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_MAG%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_MAG%u_XSCALE", i); - res = param_get(param_find(str), &_mag_calibration.x_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - res = param_get(param_find(str), &_mag_calibration.y_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - res = param_get(param_find(str), &_mag_calibration.z_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_XOFF", i); - res = param_get(param_find(str), &_mag_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YOFF", i); - res = param_get(param_find(str), &_mag_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZOFF", i); - res = param_get(param_find(str), &_mag_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - } -} - -void DfAK8963Wrapper::print_calibration() -{ - PX4_INFO("calibration x_offset: %f", (double)_mag_calibration.x_offset); - PX4_INFO("calibration x_scale: %f", (double)_mag_calibration.x_scale); - PX4_INFO("calibration y_offset: %f", (double)_mag_calibration.y_offset); - PX4_INFO("calibration y_scale: %f", (double)_mag_calibration.y_scale); - PX4_INFO("calibration z_offset: %f", (double)_mag_calibration.z_offset); - PX4_INFO("calibration z_scale: %f", (double)_mag_calibration.z_scale); -} - -int DfAK8963Wrapper::_publish(struct mag_sensor_data &data) -{ - /* Check if calibration values are still up-to-date. */ - bool updated; - orb_check(_param_update_sub, &updated); - - if (updated) { - parameter_update_s parameter_update; - orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); - - _update_mag_calibration(); - } - - /* Publish mag first. */ - perf_begin(_mag_sample_perf); - - sensor_mag_s mag_report = {}; - mag_report.timestamp = hrt_absolute_time(); - mag_report.is_external = true; - - // TODO: remove these (or get the values) - mag_report.x_raw = 0; - mag_report.y_raw = 0; - mag_report.z_raw = 0; - - matrix::Vector3f mag_val(data.field_x_ga, data.field_y_ga, data.field_z_ga); - - // apply sensor rotation on the accel measurement - mag_val = _rotation_matrix * mag_val; - - mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale; - mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale; - mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale; - - - // TODO: get these right - //mag_report.scaling = -1.0f; - - mag_report.device_id = m_id.dev_id; - - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { - - if (_mag_topic == nullptr) { - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, - &_mag_orb_class_instance, ORB_PRIO_HIGH); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); - } - - } - - perf_end(_mag_sample_perf); - - return 0; -}; - - -namespace df_ak8963_wrapper -{ - -DfAK8963Wrapper *g_dev = nullptr; - -int start(enum Rotation rotation); -int stop(); -int info(); -void usage(); - -int start(enum Rotation rotation) -{ - g_dev = new DfAK8963Wrapper(rotation); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfAK8963Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfAK8963Wrapper start failed"); - return ret; - } - - // Open the MAG sensor - DevHandle h; - DevMgr::getHandle(MAG_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - MAG_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_DEBUG("state @ %p", g_dev); - - g_dev->print_calibration(); - - return 0; -} - -void -usage() -{ - PX4_INFO("Usage: df_ak8963_wrapper 'start', 'info', 'stop'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace df_ak8963_wrapper - - -int -df_ak8963_wrapper_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret = 0; - int myoptind = 1; - const char *myoptarg = NULL; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - default: - df_ak8963_wrapper::usage(); - return 0; - } - } - - if (argc <= 1) { - df_ak8963_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_ak8963_wrapper::start(rotation); - } - - else if (!strcmp(verb, "stop")) { - ret = df_ak8963_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_ak8963_wrapper::info(); - } - - else { - df_ak8963_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt deleted file mode 100644 index b48e85238c..0000000000 --- a/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_isl29501_wrapper - MAIN df_isl29501_wrapper - SRCS - df_isl29501_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_isl29501 - ) diff --git a/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp deleted file mode 100644 index 6b1ea42060..0000000000 --- a/src/drivers/driver_framework_wrapper/df_isl29501_wrapper/df_isl29501_wrapper.cpp +++ /dev/null @@ -1,378 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -/** - * @file df_isl29501_wrapper.cpp - * Driver to access the ISL29501 of the DriverFramework. - * - * @author Zach Lovett - * @author Siddharth B Purohit - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include - - -extern "C" { __EXPORT int df_isl29501_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfISL29501Wrapper : public ISL29501 -{ -public: - DfISL29501Wrapper(); - ~DfISL29501Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - -private: - int _publish(struct range_sensor_data &data); - - orb_advert_t _range_topic; - - int _orb_class_instance; - - // perf_counter_t _range_sample_perf; - -}; - -DfISL29501Wrapper::DfISL29501Wrapper(/*enum Rotation rotation*/) : - ISL29501(ISL_DEVICE_PATH), - _range_topic(nullptr), - _orb_class_instance(-1) -{ -} - -DfISL29501Wrapper::~DfISL29501Wrapper() -{ -} - -int DfISL29501Wrapper::start() -{ - int ret; - ret = ISL29501::init(); - - if (ret != 0) { - PX4_ERR("ISL init fail: %d", ret); - return ret; - } - - ret = ISL29501::start(); - - if (ret != 0) { - PX4_ERR("ISL start fail: %d", ret); - return ret; - } - - return 0; -} - -int DfISL29501Wrapper::stop() -{ - /* Stop sensor. */ - int ret = ISL29501::stop(); - - if (ret != 0) { - PX4_ERR("ISL stop fail: %d", ret); - return ret; - } - - return 0; -} - -int DfISL29501Wrapper::_publish(struct range_sensor_data &data) -{ - struct distance_sensor_s d; - - memset(&d, 0, sizeof(d)); - - d.timestamp = hrt_absolute_time(); - - d.min_distance = float(ISL_MIN_DISTANCE); /* m */ - - d.max_distance = float(ISL_MAX_DISTANCE); /* m */ - - d.current_distance = float(data.dist); - - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - - d.id = 0; // TODO set proper ID - - d.variance = 0.0f; - - d.signal_quality = -1; - - if (_range_topic == nullptr) { - _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(distance_sensor), _range_topic, &d); - } - - return 0; -}; - - -namespace df_isl29501_wrapper -{ - -DfISL29501Wrapper *g_dev = nullptr; - -int start(); -int stop(); -int info(); -int probe(); -int calibration(); -void usage(); - -int start() -{ - PX4_ERR("start"); - g_dev = new DfISL29501Wrapper(); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfISL29501Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfISL29501Wrapper start failed"); - return ret; - } - - // Open the range sensor - DevHandle h; - DevMgr::getHandle(ISL_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - ISL_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - -/** - * Who am i - */ -int -probe() -{ - int ret; - - if (g_dev == nullptr) { - ret = start(); - - if (ret) { - PX4_ERR("Failed to start"); - return ret; - } - } - - ret = g_dev->probe(); - - if (ret) { - PX4_ERR("Failed to probe"); - return ret; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - -/** - * Calibration - * runs calibration routine for ISL29501 - * TODO: implement calibration user interface and parameter system to store calib - * Note: Currently only serves debugging purpose, user is required to manually - * set offset inside code. - */ -int -calibration() -{ - int ret; - - if (g_dev == nullptr) { - ret = start(); - - if (ret) { - PX4_ERR("Failed to start"); - return ret; - } - } - - ret = g_dev->calibration(); - - if (ret) { - PX4_ERR("Failed to calibrate"); - return ret; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - - -void -usage() -{ - PX4_WARN("Usage: df_isl_wrapper 'start', 'info', 'stop', 'calib', 'probe'"); -} - -} // namespace df_isl_wrapper - - -int -df_isl29501_wrapper_main(int argc, char *argv[]) -{ - int ret = 0; - int myoptind = 1; - - if (argc <= 1) { - df_isl29501_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_isl29501_wrapper::start(/*rotation*/); - } - - else if (!strcmp(verb, "stop")) { - ret = df_isl29501_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_isl29501_wrapper::info(); - } - - else if (!strcmp(verb, "probe")) { - ret = df_isl29501_wrapper::probe(); - } - - else if (!strcmp(verb, "calib")) { - df_isl29501_wrapper::calibration(); - return 1; - } - - else { - df_isl29501_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt deleted file mode 100644 index 329b4cd21a..0000000000 --- a/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_lsm9ds1_wrapper - MAIN df_lsm9ds1_wrapper - SRCS - df_lsm9ds1_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_lsm9ds1 - ) diff --git a/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp deleted file mode 100644 index 276d390eae..0000000000 --- a/src/drivers/driver_framework_wrapper/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp +++ /dev/null @@ -1,854 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -/** - * @file df_lsm9ds1_wrapper.cpp - * Lightweight driver to access the MPU9250 of the DriverFramework. - * - * @author Miguel Arroyo - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include - -// We don't want to auto publish, therefore set this to 0. -#define LSM9DS1_NEVER_AUTOPUBLISH_US 0 - - -extern "C" { __EXPORT int df_lsm9ds1_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfLsm9ds1Wrapper : public LSM9DS1 -{ -public: - DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation); - ~DfLsm9ds1Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - - /** - * Print some debug info. - */ - void info(); - -private: - int _publish(struct imu_sensor_data &data); - - void _update_accel_calibration(); - void _update_gyro_calibration(); - void _update_mag_calibration(); - - orb_advert_t _accel_topic; - orb_advert_t _gyro_topic; - orb_advert_t _mag_topic; - orb_advert_t _mavlink_log_pub; - - int _param_update_sub; - - struct accel_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _accel_calibration; - - struct gyro_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _gyro_calibration; - - struct mag_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _mag_calibration; - - matrix::Dcmf _rotation_matrix; - int _accel_orb_class_instance; - int _gyro_orb_class_instance; - int _mag_orb_class_instance; - - Integrator _accel_int; - Integrator _gyro_int; - - unsigned _publish_count; - - perf_counter_t _read_counter; - perf_counter_t _error_counter; - perf_counter_t _fifo_overflow_counter; - perf_counter_t _fifo_corruption_counter; - perf_counter_t _gyro_range_hit_counter; - perf_counter_t _accel_range_hit_counter; - perf_counter_t _publish_perf; - - hrt_abstime _last_accel_range_hit_time; - uint64_t _last_accel_range_hit_count; - - bool _mag_enabled; -}; - -DfLsm9ds1Wrapper::DfLsm9ds1Wrapper(bool mag_enabled, enum Rotation rotation) : - LSM9DS1(IMU_DEVICE_ACC_GYRO, IMU_DEVICE_MAG), - _accel_topic(nullptr), - _gyro_topic(nullptr), - _mag_topic(nullptr), - _mavlink_log_pub(nullptr), - _param_update_sub(-1), - _accel_calibration{}, - _gyro_calibration{}, - _mag_calibration{}, - _accel_orb_class_instance(-1), - _gyro_orb_class_instance(-1), - _mag_orb_class_instance(-1), - _accel_int(LSM9DS1_NEVER_AUTOPUBLISH_US, false), - _gyro_int(LSM9DS1_NEVER_AUTOPUBLISH_US, true), - _publish_count(0), - _read_counter(perf_alloc(PC_COUNT, "lsm9ds1_reads")), - _error_counter(perf_alloc(PC_COUNT, "lsm9ds1_errors")), - _fifo_overflow_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_overflows")), - _fifo_corruption_counter(perf_alloc(PC_COUNT, "lsm9ds1_fifo_corruptions")), - _gyro_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_gyro_range_hits")), - _accel_range_hit_counter(perf_alloc(PC_COUNT, "lsm9ds1_accel_range_hits")), - _publish_perf(perf_alloc(PC_ELAPSED, "lsm9ds1_publish")), - _last_accel_range_hit_time(0), - _last_accel_range_hit_count(0), - _mag_enabled(mag_enabled) -{ - // Set sane default calibration values - _accel_calibration.x_scale = 1.0f; - _accel_calibration.y_scale = 1.0f; - _accel_calibration.z_scale = 1.0f; - _accel_calibration.x_offset = 0.0f; - _accel_calibration.y_offset = 0.0f; - _accel_calibration.z_offset = 0.0f; - - _gyro_calibration.x_offset = 0.0f; - _gyro_calibration.y_offset = 0.0f; - _gyro_calibration.z_offset = 0.0f; - - if (_mag_enabled) { - _mag_calibration.x_scale = 1.0f; - _mag_calibration.y_scale = 1.0f; - _mag_calibration.z_scale = 1.0f; - _mag_calibration.x_offset = 0.0f; - _mag_calibration.y_offset = 0.0f; - _mag_calibration.z_offset = 0.0f; - } - - // Get sensor rotation matrix - _rotation_matrix = get_rot_matrix(rotation); -} - -DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper() -{ - perf_free(_read_counter); - perf_free(_error_counter); - perf_free(_fifo_overflow_counter); - perf_free(_fifo_corruption_counter); - perf_free(_gyro_range_hit_counter); - perf_free(_accel_range_hit_counter); - - perf_free(_publish_perf); -} - -int DfLsm9ds1Wrapper::start() -{ - // TODO: don't publish garbage here - sensor_accel_s accel_report = {}; - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_accel_topic == nullptr) { - PX4_ERR("sensor_accel advert fail"); - return -1; - } - - // TODO: don't publish garbage here - sensor_gyro_s gyro_report = {}; - _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, - &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_gyro_topic == nullptr) { - PX4_ERR("sensor_gyro advert fail"); - return -1; - } - - if (_mag_enabled) { - sensor_mag_s mag_report = {}; - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, - &_mag_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_mag_topic == nullptr) { - PX4_ERR("sensor_mag advert fail"); - return -1; - } - } - - /* Subscribe to param update topic. */ - if (_param_update_sub < 0) { - _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - } - - /* Init device and start sensor. */ - int ret = init(); - - if (ret != 0) { - PX4_ERR("LSM9DS1 init fail: %d", ret); - return ret; - } - - ret = LSM9DS1::start(); - - if (ret != 0) { - PX4_ERR("LSM9DS1 start fail: %d", ret); - return ret; - } - - PX4_DEBUG("LSM9DS1 device id is: %d", m_id.dev_id); - - /* Force getting the calibration values. */ - _update_accel_calibration(); - _update_gyro_calibration(); - _update_mag_calibration(); - - return 0; -} - -int DfLsm9ds1Wrapper::stop() -{ - /* Stop sensor. */ - int ret = LSM9DS1::stop(); - - if (ret != 0) { - PX4_ERR("LSM9DS1 stop fail: %d", ret); - return ret; - } - - return 0; -} - -void DfLsm9ds1Wrapper::info() -{ - perf_print_counter(_read_counter); - perf_print_counter(_error_counter); - perf_print_counter(_fifo_overflow_counter); - perf_print_counter(_fifo_corruption_counter); - perf_print_counter(_gyro_range_hit_counter); - perf_print_counter(_accel_range_hit_counter); - - perf_print_counter(_publish_perf); -} - -void DfLsm9ds1Wrapper::_update_gyro_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_GYRO%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", i); - res = param_get(param_find(str), &_gyro_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_YOFF", i); - res = param_get(param_find(str), &_gyro_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); - res = param_get(param_find(str), &_gyro_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - _gyro_calibration.x_offset = 0.0f; - _gyro_calibration.y_offset = 0.0f; - _gyro_calibration.z_offset = 0.0f; -} - -void DfLsm9ds1Wrapper::_update_accel_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_ACC%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - res = param_get(param_find(str), &_accel_calibration.x_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - res = param_get(param_find(str), &_accel_calibration.y_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - res = param_get(param_find(str), &_accel_calibration.z_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_XOFF", i); - res = param_get(param_find(str), &_accel_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YOFF", i); - res = param_get(param_find(str), &_accel_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - res = param_get(param_find(str), &_accel_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - // Set sane default calibration values - _accel_calibration.x_scale = 1.0f; - _accel_calibration.y_scale = 1.0f; - _accel_calibration.z_scale = 1.0f; - _accel_calibration.x_offset = 0.0f; - _accel_calibration.y_offset = 0.0f; - _accel_calibration.z_offset = 0.0f; -} - -void DfLsm9ds1Wrapper::_update_mag_calibration() -{ - if (!_mag_enabled) { - return; - } - - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_MAG%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_MAG%u_XSCALE", i); - res = param_get(param_find(str), &_mag_calibration.x_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - res = param_get(param_find(str), &_mag_calibration.y_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - res = param_get(param_find(str), &_mag_calibration.z_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_XOFF", i); - res = param_get(param_find(str), &_mag_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YOFF", i); - res = param_get(param_find(str), &_mag_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZOFF", i); - res = param_get(param_find(str), &_mag_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - // Set sane default calibration values - _mag_calibration.x_scale = 1.0f; - _mag_calibration.y_scale = 1.0f; - _mag_calibration.z_scale = 1.0f; - _mag_calibration.x_offset = 0.0f; - _mag_calibration.y_offset = 0.0f; - _mag_calibration.z_offset = 0.0f; -} - -int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) -{ - /* Check if calibration values are still up-to-date. */ - bool updated; - orb_check(_param_update_sub, &updated); - - if (updated) { - parameter_update_s parameter_update; - orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); - - _update_accel_calibration(); - _update_gyro_calibration(); - } - - matrix::Vector3f vec_integrated_unused; - uint32_t integral_dt_unused; - matrix::Vector3f accel_val(data.accel_m_s2_x, - data.accel_m_s2_y, - data.accel_m_s2_z); - // apply sensor rotation on the accel measurement - accel_val = _rotation_matrix * accel_val; - // Apply calibration after rotation - accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale; - accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale; - accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale; - _accel_int.put_with_interval(data.fifo_sample_interval_us, - accel_val, - vec_integrated_unused, - integral_dt_unused); - matrix::Vector3f gyro_val(data.gyro_rad_s_x, - data.gyro_rad_s_y, - data.gyro_rad_s_z); - // apply sensor rotation on the gyro measurement - gyro_val = _rotation_matrix * gyro_val; - // Apply calibration after rotation - gyro_val(0) = gyro_val(0) - _gyro_calibration.x_offset; - gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset; - gyro_val(2) = gyro_val(2) - _gyro_calibration.z_offset; - _gyro_int.put_with_interval(data.fifo_sample_interval_us, - gyro_val, - vec_integrated_unused, - integral_dt_unused); - - // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz. - // Therefore, only publish every forth time. - ++_publish_count; - - if (_publish_count < 4) { - return 0; - } - - _publish_count = 0; - - // Update all the counters. - perf_set_count(_read_counter, data.read_counter); - perf_set_count(_error_counter, data.error_counter); - perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); - perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); - perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); - perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); - - perf_begin(_publish_perf); - - sensor_accel_s accel_report = {}; - sensor_gyro_s gyro_report = {}; - sensor_mag_s mag_report = {}; - - accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); - mag_report.timestamp = accel_report.timestamp; - mag_report.is_external = false; - - // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.device_id = m_id.dev_id; - - accel_report.scaling = -1.0f; - accel_report.device_id = m_id.dev_id; - - if (_mag_enabled) { - mag_report.scaling = -1.0f; - mag_report.device_id = m_id.dev_id; - } - - // TODO: remove these (or get the values) - gyro_report.x_raw = 0; - gyro_report.y_raw = 0; - gyro_report.z_raw = 0; - - accel_report.x_raw = 0; - accel_report.y_raw = 0; - accel_report.z_raw = 0; - - if (_mag_enabled) { - mag_report.x_raw = 0; - mag_report.y_raw = 0; - mag_report.z_raw = 0; - } - - matrix::Vector3f gyro_val_filt; - matrix::Vector3f accel_val_filt; - - // Read and reset. - matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); - matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); - - // Use the filtered (by integration) values to get smoother / less noisy data. - gyro_report.x = gyro_val_filt(0); - gyro_report.y = gyro_val_filt(1); - gyro_report.z = gyro_val_filt(2); - - accel_report.x = accel_val_filt(0); - accel_report.y = accel_val_filt(1); - accel_report.z = accel_val_filt(2); - - if (_mag_enabled) { - matrix::Vector3f mag_val(data.mag_ga_x, - data.mag_ga_y, - data.mag_ga_z); - mag_val = _rotation_matrix * mag_val; - mag_val(0) = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale; - mag_val(1) = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale; - mag_val(2) = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale; - - mag_report.x = mag_val(0); - mag_report.y = mag_val(1); - mag_report.z = mag_val(2); - } - - gyro_report.x_integral = gyro_val_integ(0); - gyro_report.y_integral = gyro_val_integ(1); - gyro_report.z_integral = gyro_val_integ(2); - - accel_report.x_integral = accel_val_integ(0); - accel_report.y_integral = accel_val_integ(1); - accel_report.z_integral = accel_val_integ(2); - - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { - - - if (_gyro_topic != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); - } - - if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } - - if (_mag_topic != nullptr) { - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); - } - - // Report if there are high vibrations, every 10 times it happens. - const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10); - - // Report every 5s. - const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000); - - if (threshold_reached && due_to_report) { - mavlink_log_critical(&_mavlink_log_pub, - "High accelerations, range exceeded %llu times", - data.accel_range_hit_counter); - - _last_accel_range_hit_time = hrt_absolute_time(); - _last_accel_range_hit_count = data.accel_range_hit_counter; - } - } - - perf_end(_publish_perf); - - // TODO: check the return codes of this function - return 0; -}; - - -namespace df_lsm9ds1_wrapper -{ - -DfLsm9ds1Wrapper *g_dev = nullptr; - -int start(bool mag_enabled, enum Rotation rotation); -int stop(); -int info(); -void usage(); - -int start(bool mag_enabled, enum Rotation rotation) -{ - g_dev = new DfLsm9ds1Wrapper(mag_enabled, rotation); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfLsm9ds1Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfLsm9ds1Wrapper start failed"); - return ret; - } - - // Open the IMU sensor - DevHandle h; - DevMgr::getHandle(IMU_DEVICE_ACC_GYRO, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - IMU_DEVICE_ACC_GYRO, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - DevMgr::getHandle(IMU_DEVICE_MAG, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - IMU_DEVICE_MAG, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_INFO("state @ %p", g_dev); - g_dev->info(); - - return 0; -} - -void -usage() -{ - PX4_INFO("Usage: df_lsm9ds1_wrapper 'start', 'info', 'stop', 'start_without_mag'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace df_lsm9ds1_wrapper - - -int -df_lsm9ds1_wrapper_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret = 0; - int myoptind = 1; - const char *myoptarg = NULL; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - default: - df_lsm9ds1_wrapper::usage(); - return 0; - } - } - - if (argc <= 1) { - df_lsm9ds1_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - if (!strcmp(verb, "start_without_mag")) { - ret = df_lsm9ds1_wrapper::start(false, rotation); - } - - else if (!strcmp(verb, "start")) { - ret = df_lsm9ds1_wrapper::start(true, rotation); - } - - else if (!strcmp(verb, "stop")) { - ret = df_lsm9ds1_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_lsm9ds1_wrapper::info(); - } - - else { - df_lsm9ds1_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt deleted file mode 100644 index 5f56c75d41..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_ltc2946_wrapper - MAIN df_ltc2946_wrapper - SRCS - df_ltc2946_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_ltc2946 - battery - ) diff --git a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp deleted file mode 100644 index 41ff1e138e..0000000000 --- a/src/drivers/driver_framework_wrapper/df_ltc2946_wrapper/df_ltc2946_wrapper.cpp +++ /dev/null @@ -1,257 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2018 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. - * - ****************************************************************************/ - -/** - * @file df_ltc2946_wrapper.cpp - * Driver to access the LTC2946 of the DriverFramework. - * - * @author Christoph Tobler - */ - - -#include -#include -#include -#include -#include - -#include - -#include -#include -#include - - -extern "C" { __EXPORT int df_ltc2946_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfLtc2946Wrapper : public LTC2946 -{ -public: - DfLtc2946Wrapper(); - ~DfLtc2946Wrapper(); - - int start(); - int stop(); - -private: - int _publish(const struct ltc2946_sensor_data &data); - - Battery _battery{1, nullptr}; - - int _actuator_ctrl_0_sub{-1}; - int _vcontrol_mode_sub{-1}; - -}; - -DfLtc2946Wrapper::DfLtc2946Wrapper() : - LTC2946(LTC2946_DEVICE_PATH) -{ - _battery.reset(); - - // subscriptions - _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); - _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); -} - -DfLtc2946Wrapper::~DfLtc2946Wrapper() -{ - orb_unsubscribe(_actuator_ctrl_0_sub); - orb_unsubscribe(_vcontrol_mode_sub); -} - -int DfLtc2946Wrapper::start() -{ - int ret; - - /* Init device and start sensor. */ - ret = init(); - - // - if (ret != 0) { - PX4_ERR("LTC2946 init fail: %d", ret); - return ret; - } - - ret = LTC2946::start(); - - if (ret != 0) { - PX4_ERR("LTC2946 start fail: %d", ret); - return ret; - } - - return 0; -} - -int DfLtc2946Wrapper::stop() -{ - /* Stop sensor. */ - int ret = LTC2946::stop(); - - if (ret != 0) { - PX4_ERR("LTC2946 stop fail: %d", ret); - return ret; - } - - return 0; -} - -int DfLtc2946Wrapper::_publish(const struct ltc2946_sensor_data &data) -{ - hrt_abstime t = hrt_absolute_time(); - bool connected = data.battery_voltage_V > BOARD_ADC_OPEN_CIRCUIT_V; - - actuator_controls_s ctrl; - orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl); - vehicle_control_mode_s vcontrol_mode; - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); - - _battery.updateBatteryStatus(t, data.battery_voltage_V, data.battery_current_A, - connected, true, 1, - ctrl.control[actuator_controls_s::INDEX_THROTTLE], - true); - return 0; -} - - -namespace df_ltc2946_wrapper -{ - -DfLtc2946Wrapper *g_dev = nullptr; - -int start(); -int stop(); -int info(); -void usage(); - -int start() -{ - PX4_WARN("starting LTC2946"); - g_dev = new DfLtc2946Wrapper(); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfLtc2946Wrapper object"); - return -1; - - } else { - PX4_INFO("started LTC2946"); - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfLtc2946Wrapper start failed"); - return ret; - } - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - -void -usage() -{ - PX4_WARN("Usage: df_ltc2946_wrapper 'start', 'info', 'stop'"); -} - -} // namespace df_ltc2946_wrapper - - -int -df_ltc2946_wrapper_main(int argc, char *argv[]) -{ - int ret = 0; - - if (argc <= 1) { - df_ltc2946_wrapper::usage(); - return 1; - } - - const char *verb = argv[1]; - - - if (!strcmp(verb, "start")) { - ret = df_ltc2946_wrapper::start(); - } - - else if (!strcmp(verb, "stop")) { - ret = df_ltc2946_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_ltc2946_wrapper::info(); - } - - else { - df_ltc2946_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt deleted file mode 100644 index 3ec392d854..0000000000 --- a/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_mpu6050_wrapper - MAIN df_mpu6050_wrapper - SRCS - df_mpu6050_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_mpu6050 - ) diff --git a/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp deleted file mode 100644 index bc3654fb0a..0000000000 --- a/src/drivers/driver_framework_wrapper/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp +++ /dev/null @@ -1,687 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -/** - * @file df_mpu6050_wrapper.cpp - * Lightweight driver to access the MPU6050 of the DriverFramework. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#include - -#include -#include - -// We don't want to auto publish, therefore set this to 0. -#define MPU6050_NEVER_AUTOPUBLISH_US 0 - - -extern "C" { __EXPORT int df_mpu6050_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfMPU6050Wrapper : public MPU6050 -{ -public: - DfMPU6050Wrapper(enum Rotation rotation); - ~DfMPU6050Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - - /** - * Print some debug info. - */ - void info(); - -private: - int _publish(struct imu_sensor_data &data); - - void _update_accel_calibration(); - void _update_gyro_calibration(); - - orb_advert_t _accel_topic; - orb_advert_t _gyro_topic; - - orb_advert_t _mavlink_log_pub; - - int _param_update_sub; - - struct accel_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _accel_calibration; - - struct gyro_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _gyro_calibration; - - matrix::Dcmf _rotation_matrix; - - int _accel_orb_class_instance; - int _gyro_orb_class_instance; - - Integrator _accel_int; - Integrator _gyro_int; - - unsigned _publish_count; - - perf_counter_t _read_counter; - perf_counter_t _error_counter; - perf_counter_t _fifo_overflow_counter; - perf_counter_t _fifo_corruption_counter; - perf_counter_t _gyro_range_hit_counter; - perf_counter_t _accel_range_hit_counter; - perf_counter_t _publish_perf; - - hrt_abstime _last_accel_range_hit_time; - uint64_t _last_accel_range_hit_count; - -}; - -DfMPU6050Wrapper::DfMPU6050Wrapper(enum Rotation rotation) : - MPU6050(IMU_DEVICE_PATH), - _accel_topic(nullptr), - _gyro_topic(nullptr), - _mavlink_log_pub(nullptr), - _param_update_sub(-1), - _accel_calibration{}, - _gyro_calibration{}, - _accel_orb_class_instance(-1), - _gyro_orb_class_instance(-1), - _accel_int(MPU6050_NEVER_AUTOPUBLISH_US, false), - _gyro_int(MPU6050_NEVER_AUTOPUBLISH_US, true), - _publish_count(0), - _read_counter(perf_alloc(PC_COUNT, "mpu6050_reads")), - _error_counter(perf_alloc(PC_COUNT, "mpu6050_errors")), - _fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_overflows")), - _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu6050_fifo_corruptions")), - _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_gyro_range_hits")), - _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu6050_accel_range_hits")), - _publish_perf(perf_alloc(PC_ELAPSED, "mpu6050_publish")), - _last_accel_range_hit_time(0), - _last_accel_range_hit_count(0) -{ - // Set sane default calibration values - _accel_calibration.x_scale = 1.0f; - _accel_calibration.y_scale = 1.0f; - _accel_calibration.z_scale = 1.0f; - _accel_calibration.x_offset = 0.0f; - _accel_calibration.y_offset = 0.0f; - _accel_calibration.z_offset = 0.0f; - - _gyro_calibration.x_offset = 0.0f; - _gyro_calibration.y_offset = 0.0f; - _gyro_calibration.z_offset = 0.0f; - - // Get sensor rotation matrix - _rotation_matrix = get_rot_matrix(rotation); -} - -DfMPU6050Wrapper::~DfMPU6050Wrapper() -{ - perf_free(_read_counter); - perf_free(_error_counter); - perf_free(_fifo_overflow_counter); - perf_free(_fifo_corruption_counter); - perf_free(_gyro_range_hit_counter); - perf_free(_accel_range_hit_counter); - - perf_free(_publish_perf); -} - -int DfMPU6050Wrapper::start() -{ - /* Subscribe to param update topic. */ - if (_param_update_sub < 0) { - _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - } - - /* Init device and start sensor. */ - int ret = init(); - - if (ret != 0) { - PX4_ERR("MPU6050 init fail: %d", ret); - return ret; - } - - ret = MPU6050::start(); - - if (ret != 0) { - PX4_ERR("MPU6050 start fail: %d", ret); - return ret; - } - - PX4_DEBUG("MPU6050 device id is: %d", m_id.dev_id); - - /* Force getting the calibration values. */ - _update_accel_calibration(); - _update_gyro_calibration(); - - return 0; -} - -int DfMPU6050Wrapper::stop() -{ - /* Stop sensor. */ - int ret = MPU6050::stop(); - - if (ret != 0) { - PX4_ERR("MPU6050 stop fail: %d", ret); - return ret; - } - - return 0; -} - -void DfMPU6050Wrapper::info() -{ - perf_print_counter(_read_counter); - perf_print_counter(_error_counter); - perf_print_counter(_fifo_overflow_counter); - perf_print_counter(_fifo_corruption_counter); - perf_print_counter(_gyro_range_hit_counter); - perf_print_counter(_accel_range_hit_counter); - - perf_print_counter(_publish_perf); -} - -void DfMPU6050Wrapper::_update_gyro_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_GYRO%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", i); - res = param_get(param_find(str), &_gyro_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_YOFF", i); - res = param_get(param_find(str), &_gyro_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); - res = param_get(param_find(str), &_gyro_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - _gyro_calibration.x_offset = 0.0f; - _gyro_calibration.y_offset = 0.0f; - _gyro_calibration.z_offset = 0.0f; -} - -void DfMPU6050Wrapper::_update_accel_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_ACC%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - res = param_get(param_find(str), &_accel_calibration.x_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - res = param_get(param_find(str), &_accel_calibration.y_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - res = param_get(param_find(str), &_accel_calibration.z_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_XOFF", i); - res = param_get(param_find(str), &_accel_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YOFF", i); - res = param_get(param_find(str), &_accel_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - res = param_get(param_find(str), &_accel_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - // Set sane default calibration values - _accel_calibration.x_scale = 1.0f; - _accel_calibration.y_scale = 1.0f; - _accel_calibration.z_scale = 1.0f; - _accel_calibration.x_offset = 0.0f; - _accel_calibration.y_offset = 0.0f; - _accel_calibration.z_offset = 0.0f; -} - -int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) -{ - /* Check if calibration values are still up-to-date. */ - bool updated; - orb_check(_param_update_sub, &updated); - - if (updated) { - parameter_update_s parameter_update; - orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); - - _update_accel_calibration(); - _update_gyro_calibration(); - } - - uint64_t now = hrt_absolute_time(); - - matrix::Vector3f vec_integrated_unused; - uint32_t integral_dt_unused; - - matrix::Vector3f accel_val(data.accel_m_s2_x, data.accel_m_s2_y, data.accel_m_s2_z); - - // apply sensor rotation on the accel measurement - accel_val = _rotation_matrix * accel_val; - - accel_val(0) = (accel_val(0) - _accel_calibration.x_offset) * _accel_calibration.x_scale; - accel_val(1) = (accel_val(1) - _accel_calibration.y_offset) * _accel_calibration.y_scale; - accel_val(2) = (accel_val(2) - _accel_calibration.z_offset) * _accel_calibration.z_scale; - - _accel_int.put(now, - accel_val, - vec_integrated_unused, - integral_dt_unused); - - matrix::Vector3f gyro_val(data.gyro_rad_s_x, data.gyro_rad_s_y, data.gyro_rad_s_z); - - // apply sensor rotation on the gyro measurement - gyro_val = _rotation_matrix * gyro_val; - - gyro_val(0) = gyro_val(0) - _gyro_calibration.x_offset; - gyro_val(1) = gyro_val(1) - _gyro_calibration.y_offset; - gyro_val(2) = gyro_val(2) - _gyro_calibration.z_offset; - - _gyro_int.put(now, - gyro_val, - vec_integrated_unused, - integral_dt_unused); - - // If we are not receiving the last sample from the FIFO buffer yet, let's stop here - // and wait for more packets. - if (!data.is_last_fifo_sample) { - return 0; - } - - // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz. - // Therefore, only publish every forth time. - ++_publish_count; - - if (_publish_count < 4) { - return 0; - } - - _publish_count = 0; - - // Update all the counters. - perf_set_count(_read_counter, data.read_counter); - perf_set_count(_error_counter, data.error_counter); - perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); - perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); - perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); - perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); - - perf_begin(_publish_perf); - - sensor_accel_s accel_report = {}; - sensor_gyro_s gyro_report = {}; - - accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); - - // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.device_id = m_id.dev_id; - - accel_report.scaling = -1.0f; - accel_report.device_id = m_id.dev_id; - - // TODO: remove these (or get the values) - gyro_report.x_raw = 0; - gyro_report.y_raw = 0; - gyro_report.z_raw = 0; - - accel_report.x_raw = 0; - accel_report.y_raw = 0; - accel_report.z_raw = 0; - - matrix::Vector3f gyro_val_filt; - matrix::Vector3f accel_val_filt; - - // Read and reset. - matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); - matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); - - // Use the filtered (by integration) values to get smoother / less noisy data. - gyro_report.x = gyro_val_filt(0); - gyro_report.y = gyro_val_filt(1); - gyro_report.z = gyro_val_filt(2); - - accel_report.x = accel_val_filt(0); - accel_report.y = accel_val_filt(1); - accel_report.z = accel_val_filt(2); - - gyro_report.x_integral = gyro_val_integ(0); - gyro_report.y_integral = gyro_val_integ(1); - gyro_report.z_integral = gyro_val_integ(2); - - accel_report.x_integral = accel_val_integ(0); - accel_report.y_integral = accel_val_integ(1); - accel_report.z_integral = accel_val_integ(2); - - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { - - if (_gyro_topic == nullptr) { - _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, - &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); - } - - if (_accel_topic == nullptr) { - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } - - // Report if there are high vibrations, every 10 times it happens. - const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10); - - // Report every 5s. - const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000); - - if (threshold_reached && due_to_report) { - mavlink_log_critical(&_mavlink_log_pub, - "High accelerations, range exceeded %llu times", - data.accel_range_hit_counter); - - _last_accel_range_hit_time = hrt_absolute_time(); - _last_accel_range_hit_count = data.accel_range_hit_counter; - } - } - - perf_end(_publish_perf); - - // TODO: check the return codes of this function - return 0; -}; - - -namespace df_mpu6050_wrapper -{ - -DfMPU6050Wrapper *g_dev = nullptr; - -int start(enum Rotation rotation); -int stop(); -int info(); -void usage(); - -int start(enum Rotation rotation) -{ - g_dev = new DfMPU6050Wrapper(rotation); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfMPU6050Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfMPU6050Wrapper start failed"); - return ret; - } - - // Open the IMU sensor - DevHandle h; - DevMgr::getHandle(IMU_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - IMU_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_INFO("state @ %p", g_dev); - g_dev->info(); - - return 0; -} - -void -usage() -{ - PX4_INFO("Usage: df_mpu6050_wrapper 'start', 'info', 'stop'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace df_mpu6050_wrapper - - -int -df_mpu6050_wrapper_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret = 0; - int myoptind = 1; - const char *myoptarg = NULL; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - default: - df_mpu6050_wrapper::usage(); - return 0; - } - } - - if (argc <= 1) { - df_mpu6050_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - if (!strcmp(verb, "start")) { - ret = df_mpu6050_wrapper::start(rotation); - } - - else if (!strcmp(verb, "stop")) { - ret = df_mpu6050_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_mpu6050_wrapper::info(); - } - - else { - df_mpu6050_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt deleted file mode 100644 index f5f87eb754..0000000000 --- a/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_mpu9250_wrapper - MAIN df_mpu9250_wrapper - SRCS - df_mpu9250_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_mpu9250 - ) diff --git a/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp deleted file mode 100644 index 2f24e1dcf0..0000000000 --- a/src/drivers/driver_framework_wrapper/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ /dev/null @@ -1,897 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -/** - * @file df_mpu9250_wrapper.cpp - * Lightweight driver to access the MPU9250 of the DriverFramework. - * - * @author Julian Oes - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include - -#define MPU9250_ACCEL_DEFAULT_RATE 1000 -#define MPU9250_GYRO_DEFAULT_RATE 1000 - -#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define MPU9250_PUB_RATE 280 - - -extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfMpu9250Wrapper : public MPU9250 -{ -public: - DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation); - ~DfMpu9250Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - - /** - * Print some debug info. - */ - void info(); - -private: - int _publish(struct imu_sensor_data &data); - - void _update_accel_calibration(); - void _update_gyro_calibration(); - void _update_mag_calibration(); - - orb_advert_t _accel_topic; - orb_advert_t _gyro_topic; - orb_advert_t _mag_topic; - - orb_advert_t _mavlink_log_pub; - - int _param_update_sub; - - struct accel_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _accel_calibration; - - struct gyro_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _gyro_calibration; - - struct mag_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; - } _mag_calibration; - - int _accel_orb_class_instance; - int _gyro_orb_class_instance; - int _mag_orb_class_instance; - - Integrator _accel_int; - Integrator _gyro_int; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - math::LowPassFilter2p _gyro_filter_x; - math::LowPassFilter2p _gyro_filter_y; - math::LowPassFilter2p _gyro_filter_z; - - perf_counter_t _read_counter; - perf_counter_t _error_counter; - perf_counter_t _fifo_overflow_counter; - perf_counter_t _fifo_corruption_counter; - perf_counter_t _gyro_range_hit_counter; - perf_counter_t _accel_range_hit_counter; - perf_counter_t _mag_fifo_overflow_counter; - perf_counter_t _publish_perf; - - hrt_abstime _last_accel_range_hit_time; - uint64_t _last_accel_range_hit_count; - - bool _mag_enabled; - - enum Rotation _rotation; -}; - -DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled, enum Rotation rotation) : - MPU9250(IMU_DEVICE_PATH, mag_enabled), - _accel_topic(nullptr), - _gyro_topic(nullptr), - _mag_topic(nullptr), - _mavlink_log_pub(nullptr), - _param_update_sub(-1), - _accel_calibration{}, - _gyro_calibration{}, - _mag_calibration{}, - _accel_orb_class_instance(-1), - _gyro_orb_class_instance(-1), - _mag_orb_class_instance(-1), - _accel_int(1000000 / MPU9250_PUB_RATE, false), - _gyro_int(1000000 / MPU9250_PUB_RATE, true), - _accel_filter_x(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(MPU9250_ACCEL_DEFAULT_RATE, MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_x(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_y(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _gyro_filter_z(MPU9250_GYRO_DEFAULT_RATE, MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")), - _error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")), - _fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_overflows")), - _fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")), - _gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")), - _accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")), - _mag_fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_mag_fifo_overflows")), - _publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")), - _last_accel_range_hit_time(0), - _last_accel_range_hit_count(0), - _mag_enabled(mag_enabled), - _rotation(rotation) -{ - // Set sane default calibration values - _accel_calibration.x_scale = 1.0f; - _accel_calibration.y_scale = 1.0f; - _accel_calibration.z_scale = 1.0f; - _accel_calibration.x_offset = 0.0f; - _accel_calibration.y_offset = 0.0f; - _accel_calibration.z_offset = 0.0f; - - _gyro_calibration.x_offset = 0.0f; - _gyro_calibration.y_offset = 0.0f; - _gyro_calibration.z_offset = 0.0f; - - if (_mag_enabled) { - _mag_calibration.x_scale = 1.0f; - _mag_calibration.y_scale = 1.0f; - _mag_calibration.z_scale = 1.0f; - _mag_calibration.x_offset = 0.0f; - _mag_calibration.y_offset = 0.0f; - _mag_calibration.z_offset = 0.0f; - } - - // set software low pass filter for controllers - param_t param_handle = param_find("IMU_ACCEL_CUTOFF"); - float param_val = MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ; - - if (param_handle != PARAM_INVALID && (param_get(param_handle, ¶m_val) == PX4_OK)) { - _accel_filter_x.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val); - _accel_filter_y.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val); - _accel_filter_z.set_cutoff_frequency(MPU9250_ACCEL_DEFAULT_RATE, param_val); - - } else { - PX4_ERR("IMU_ACCEL_CUTOFF param invalid"); - } - - param_handle = param_find("IMU_GYRO_CUTOFF"); - param_val = MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ; - - if (param_handle != PARAM_INVALID && (param_get(param_handle, ¶m_val) == PX4_OK)) { - _gyro_filter_x.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val); - _gyro_filter_y.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val); - _gyro_filter_z.set_cutoff_frequency(MPU9250_GYRO_DEFAULT_RATE, param_val); - - } else { - PX4_ERR("IMU_GYRO_CUTOFF param invalid"); - } -} - -DfMpu9250Wrapper::~DfMpu9250Wrapper() -{ - perf_free(_read_counter); - perf_free(_error_counter); - perf_free(_fifo_overflow_counter); - perf_free(_fifo_corruption_counter); - perf_free(_gyro_range_hit_counter); - perf_free(_accel_range_hit_counter); - - if (_mag_enabled) { - perf_free(_mag_fifo_overflow_counter); - } - - perf_free(_publish_perf); -} - -int DfMpu9250Wrapper::start() -{ - // TODO: don't publish garbage here - sensor_accel_s accel_report = {}; - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, - &_accel_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_accel_topic == nullptr) { - PX4_ERR("sensor_accel advert fail"); - return -1; - } - - // TODO: don't publish garbage here - sensor_gyro_s gyro_report = {}; - _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, - &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); - - if (_gyro_topic == nullptr) { - PX4_ERR("sensor_gyro advert fail"); - return -1; - } - - if (_mag_enabled) { - } - - /* Subscribe to param update topic. */ - if (_param_update_sub < 0) { - _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - } - - /* Init device and start sensor. */ - int ret = init(); - - if (ret != 0) { - PX4_ERR("MPU9250 init fail: %d", ret); - return ret; - } - - ret = MPU9250::start(); - - if (ret != 0) { - PX4_ERR("MPU9250 start fail: %d", ret); - return ret; - } - - PX4_DEBUG("MPU9250 device id is: %d", m_id.dev_id); - - /* Force getting the calibration values. */ - _update_accel_calibration(); - _update_gyro_calibration(); - _update_mag_calibration(); - - return 0; -} - -int DfMpu9250Wrapper::stop() -{ - /* Stop sensor. */ - int ret = MPU9250::stop(); - - if (ret != 0) { - PX4_ERR("MPU9250 stop fail: %d", ret); - return ret; - } - - return 0; -} - -void DfMpu9250Wrapper::info() -{ - perf_print_counter(_read_counter); - perf_print_counter(_error_counter); - perf_print_counter(_fifo_overflow_counter); - perf_print_counter(_fifo_corruption_counter); - perf_print_counter(_gyro_range_hit_counter); - perf_print_counter(_accel_range_hit_counter); - - if (_mag_enabled) { - perf_print_counter(_mag_fifo_overflow_counter); - } - - perf_print_counter(_publish_perf); -} - -void DfMpu9250Wrapper::_update_gyro_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_GYRO%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_GYRO%u_XOFF", i); - res = param_get(param_find(str), &_gyro_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_YOFF", i); - res = param_get(param_find(str), &_gyro_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); - res = param_get(param_find(str), &_gyro_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - _gyro_calibration.x_offset = 0.0f; - _gyro_calibration.y_offset = 0.0f; - _gyro_calibration.z_offset = 0.0f; -} - -void DfMpu9250Wrapper::_update_accel_calibration() -{ - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_ACC%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - res = param_get(param_find(str), &_accel_calibration.x_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - res = param_get(param_find(str), &_accel_calibration.y_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - res = param_get(param_find(str), &_accel_calibration.z_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_XOFF", i); - res = param_get(param_find(str), &_accel_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_YOFF", i); - res = param_get(param_find(str), &_accel_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - res = param_get(param_find(str), &_accel_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - // Set sane default calibration values - _accel_calibration.x_scale = 1.0f; - _accel_calibration.y_scale = 1.0f; - _accel_calibration.z_scale = 1.0f; - _accel_calibration.x_offset = 0.0f; - _accel_calibration.y_offset = 0.0f; - _accel_calibration.z_offset = 0.0f; -} - -void DfMpu9250Wrapper::_update_mag_calibration() -{ - if (!_mag_enabled) { - return; - } - - // TODO: replace magic number - for (unsigned i = 0; i < 3; ++i) { - - // TODO: remove printfs and add error counter - - char str[30]; - (void)sprintf(str, "CAL_MAG%u_ID", i); - int32_t device_id; - int res = param_get(param_find(str), &device_id); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - continue; - } - - if ((uint32_t)device_id != m_id.dev_id) { - continue; - } - - (void)sprintf(str, "CAL_MAG%u_XSCALE", i); - res = param_get(param_find(str), &_mag_calibration.x_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - res = param_get(param_find(str), &_mag_calibration.y_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - res = param_get(param_find(str), &_mag_calibration.z_scale); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_XOFF", i); - res = param_get(param_find(str), &_mag_calibration.x_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_YOFF", i); - res = param_get(param_find(str), &_mag_calibration.y_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - (void)sprintf(str, "CAL_MAG%u_ZOFF", i); - res = param_get(param_find(str), &_mag_calibration.z_offset); - - if (res != OK) { - PX4_ERR("Could not access param %s", str); - } - - // We got calibration values, let's exit. - return; - } - - // Set sane default calibration values - _mag_calibration.x_scale = 1.0f; - _mag_calibration.y_scale = 1.0f; - _mag_calibration.z_scale = 1.0f; - _mag_calibration.x_offset = 0.0f; - _mag_calibration.y_offset = 0.0f; - _mag_calibration.z_offset = 0.0f; -} - -int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) -{ - /* Check if calibration values are still up-to-date. */ - bool updated; - orb_check(_param_update_sub, &updated); - - if (updated) { - parameter_update_s parameter_update; - orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); - - _update_accel_calibration(); - _update_gyro_calibration(); - _update_mag_calibration(); - } - - sensor_accel_s accel_report = {}; - sensor_gyro_s gyro_report = {}; - sensor_mag_s mag_report = {}; - - accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); - - accel_report.error_count = gyro_report.error_count = mag_report.error_count = data.error_counter; - - // ACCEL - - float xraw_f = data.accel_m_s2_x; - float yraw_f = data.accel_m_s2_y; - float zraw_f = data.accel_m_s2_z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - // MPU9250 driver from DriverFramework does not provide any raw values - // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM - accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000]; - accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000]; - accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000]; - - // adjust values according to the calibration - float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale; - float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale; - float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.z_scale; - - accel_report.x = _accel_filter_x.apply(x_in_new); - accel_report.y = _accel_filter_y.apply(y_in_new); - accel_report.z = _accel_filter_z.apply(z_in_new); - - matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); - matrix::Vector3f aval_integrated; - - _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); - - // GYRO - - xraw_f = data.gyro_rad_s_x; - yraw_f = data.gyro_rad_s_y; - zraw_f = data.gyro_rad_s_z; - - // apply user specified rotation - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - // MPU9250 driver from DriverFramework does not provide any raw values - // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM - gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000]; - gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000]; - gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000]; - - // adjust values according to the calibration - float x_gyro_in_new = xraw_f - _gyro_calibration.x_offset; - float y_gyro_in_new = yraw_f - _gyro_calibration.y_offset; - float z_gyro_in_new = zraw_f - _gyro_calibration.z_offset; - - gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new); - gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new); - gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new); - - matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); - matrix::Vector3f gval_integrated; - - bool sensor_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt); - gyro_report.x_integral = gval_integrated(0); - gyro_report.y_integral = gval_integrated(1); - gyro_report.z_integral = gval_integrated(2); - - - // if gyro integrator did not return a sample we can return here - // Note: the accel integrator receives the same timestamp as the gyro integrator - // so we do not need to handle it seperately - if (!sensor_notify) { - return 0; - } - - // Update all the counters. - perf_set_count(_read_counter, data.read_counter); - perf_set_count(_error_counter, data.error_counter); - perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); - perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter); - perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); - perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); - - if (_mag_enabled) { - perf_set_count(_mag_fifo_overflow_counter, data.mag_fifo_overflow_counter); - } - - perf_begin(_publish_perf); - - // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.device_id = m_id.dev_id; - - accel_report.scaling = -1.0f; - accel_report.device_id = m_id.dev_id; - - if (_mag_enabled) { - mag_report.timestamp = accel_report.timestamp; - mag_report.is_external = false; - - mag_report.scaling = -1.0f; - mag_report.device_id = m_id.dev_id; - - xraw_f = data.mag_ga_x; - yraw_f = data.mag_ga_y; - zraw_f = data.mag_ga_z; - - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - // MPU9250 driver from DriverFramework does not provide any raw values - // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM - mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000] - mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000] - mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000] - - mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale; - mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale; - mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale; - } - - // TODO: when is this ever blocked? - if (!(m_pub_blocked) && sensor_notify) { - - if (_gyro_topic != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); - } - - if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } - - if (_mag_enabled) { - - if (_mag_topic == nullptr) { - _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report, - &_mag_orb_class_instance, ORB_PRIO_LOW); - - } else { - orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report); - } - } - - // Report if there are high vibrations, every 10 times it happens. - const bool threshold_reached = (data.accel_range_hit_counter - _last_accel_range_hit_count > 10); - - // Report every 5s. - const bool due_to_report = (hrt_elapsed_time(&_last_accel_range_hit_time) > 5000000); - - if (threshold_reached && due_to_report) { - mavlink_log_critical(&_mavlink_log_pub, - "High accelerations, range exceeded %llu times", - data.accel_range_hit_counter); - - _last_accel_range_hit_time = hrt_absolute_time(); - _last_accel_range_hit_count = data.accel_range_hit_counter; - } - } - - perf_end(_publish_perf); - - // TODO: check the return codes of this function - return 0; -}; - - -namespace df_mpu9250_wrapper -{ - -DfMpu9250Wrapper *g_dev = nullptr; - -int start(bool mag_enabled, enum Rotation rotation); -int stop(); -int info(); -void usage(); - -int start(bool mag_enabled, enum Rotation rotation) -{ - g_dev = new DfMpu9250Wrapper(mag_enabled, rotation); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfMpu9250Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfMpu9250Wrapper start failed"); - return ret; - } - - // Open the IMU sensor - DevHandle h; - DevMgr::getHandle(IMU_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - IMU_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_INFO("state @ %p", g_dev); - g_dev->info(); - - return 0; -} - -void -usage() -{ - PX4_INFO("Usage: df_mpu9250_wrapper 'start', 'start_without_mag', 'info', 'stop'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace df_mpu9250_wrapper - - -int -df_mpu9250_wrapper_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret = 0; - int myoptind = 1; - const char *myoptarg = NULL; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - default: - df_mpu9250_wrapper::usage(); - return 0; - } - } - - if (argc <= 1) { - df_mpu9250_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - if (!strcmp(verb, "start_without_mag")) { - ret = df_mpu9250_wrapper::start(false, rotation); - } - - else if (!strcmp(verb, "start")) { - ret = df_mpu9250_wrapper::start(true, rotation); - } - - else if (!strcmp(verb, "stop")) { - ret = df_mpu9250_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_mpu9250_wrapper::info(); - } - - else { - df_mpu9250_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt deleted file mode 100644 index 2b6d79e821..0000000000 --- a/src/drivers/driver_framework_wrapper/df_trone_wrapper/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -include_directories(../../../lib/DriverFramework/drivers) - -px4_add_module( - MODULE platforms__posix__drivers__df_trone_wrapper - MAIN df_trone_wrapper - SRCS - df_trone_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_trone - ) - diff --git a/src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp deleted file mode 100644 index 5b7c68c23d..0000000000 --- a/src/drivers/driver_framework_wrapper/df_trone_wrapper/df_trone_wrapper.cpp +++ /dev/null @@ -1,362 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 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. - * - ****************************************************************************/ - -/** - * @file df_trone_wrapper.cpp - * Driver to access the TROne of the DriverFramework. - * - * @author Nicolas de Palezieux - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include -#include - -#include - -#include -#include - - -extern "C" { __EXPORT int df_trone_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfTROneWrapper : public TROne -{ -public: - DfTROneWrapper(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING); - ~DfTROneWrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - -private: - int _publish(struct range_sensor_data &data); - - uint8_t _rotation; - - orb_advert_t _range_topic; - - int _orb_class_instance; - - // perf_counter_t _range_sample_perf; - -}; - -DfTROneWrapper::DfTROneWrapper(uint8_t rotation) : - TROne(TRONE_DEVICE_PATH), - _rotation(rotation), - _range_topic(nullptr), - _orb_class_instance(-1) -{ -} - -DfTROneWrapper::~DfTROneWrapper() -{ -} - -int DfTROneWrapper::start() -{ - int ret; - - /* Init device and start sensor. */ - ret = init(); - - if (ret != 0) { - PX4_ERR("TROne init fail: %d", ret); - return ret; - } - - ret = TROne::start(); - - if (ret != 0) { - PX4_ERR("TROne start fail: %d", ret); - return ret; - } - - return 0; -} - -int DfTROneWrapper::stop() -{ - /* Stop sensor. */ - int ret = TROne::stop(); - - if (ret != 0) { - PX4_ERR("TROne stop fail: %d", ret); - return ret; - } - - return 0; -} - -int DfTROneWrapper::_publish(struct range_sensor_data &data) -{ - struct distance_sensor_s d; - - memset(&d, 0, sizeof(d)); - - d.timestamp = hrt_absolute_time(); - - d.min_distance = float(TRONE_MIN_DISTANCE); /* m */ - - d.max_distance = float(TRONE_MAX_DISTANCE); /* m */ - - d.current_distance = float(data.dist); - - d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - - d.id = 0; // TODO set proper ID - - d.orientation = _rotation; - - d.variance = 0.0f; - - d.signal_quality = -1; - - if (_range_topic == nullptr) { - _range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d, - &_orb_class_instance, ORB_PRIO_DEFAULT); - - } else { - orb_publish(ORB_ID(distance_sensor), _range_topic, &d); - } - - return 0; -}; - - -namespace df_trone_wrapper -{ - -DfTROneWrapper *g_dev = nullptr; - -int start(uint8_t rotation); -int stop(); -int info(); -int probe(); -void usage(); - -int start(uint8_t rotation) -{ - PX4_ERR("start"); - g_dev = new DfTROneWrapper(rotation); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfTROneWrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfTROneWrapper start failed"); - return ret; - } - - // Open the range sensor - DevHandle h; - DevMgr::getHandle(TRONE_DEVICE_PATH, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - TRONE_DEVICE_PATH, h.getError()); - return -1; - } - - DevMgr::releaseHandle(h); - - return 0; -} - -int stop() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - int ret = g_dev->stop(); - - if (ret != 0) { - PX4_ERR("driver could not be stopped"); - return ret; - } - - delete g_dev; - g_dev = nullptr; - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - -/** - * Who am i - */ -int -probe() -{ - int ret; - - if (g_dev == nullptr) { - ret = start(distance_sensor_s::ROTATION_DOWNWARD_FACING); - - if (ret) { - PX4_ERR("Failed to start"); - return ret; - } - } - - ret = g_dev->probe(); - - if (ret) { - PX4_ERR("Failed to probe"); - return ret; - } - - PX4_DEBUG("state @ %p", g_dev); - - return 0; -} - - -void -usage() -{ - PX4_WARN("Usage: df_trone_wrapper 'start', 'info', 'stop'"); -} - -} // namespace df_trone_wrapper - - -int -df_trone_wrapper_main(int argc, char *argv[]) -{ - int ch; - int ret = 0; - int myoptind = 1; - const char *myoptarg = NULL; - uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (uint8_t)atoi(myoptarg); - PX4_INFO("Setting distance sensor orientation to %d", (int)rotation); - break; - - default: - df_trone_wrapper::usage(); - return 0; - } - } - - if (argc <= 1) { - df_trone_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_trone_wrapper::start(rotation); - } - - else if (!strcmp(verb, "stop")) { - ret = df_trone_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_trone_wrapper::info(); - } - - else if (!strcmp(verb, "probe")) { - ret = df_trone_wrapper::probe(); - } - - else { - df_trone_wrapper::usage(); - return 1; - } - - return ret; -} diff --git a/src/drivers/drv_device.h b/src/drivers/drv_device.h index 411c30f51f..e25a414304 100644 --- a/src/drivers/drv_device.h +++ b/src/drivers/drv_device.h @@ -46,15 +46,13 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" -#include "DevIOCTL.h" +#define _DEVICEIOCBASE (0x100) +#define _DEVICEIOC(_n) (_PX4_IOC(_DEVICEIOCBASE, _n)) -#ifdef __PX4_POSIX - -#ifndef SIOCDEVPRIVATE -#define SIOCDEVPRIVATE 1 -#endif - -#define DIOC_GETPRIV SIOCDEVPRIVATE -#endif +/** + * Return device ID, to enable matching of configuration parameters + * (such as compass offsets) to specific sensors + */ +#define DEVIOCGDEVICEID _DEVICEIOC(2) #endif /* _DRV_DEVICE_H */ diff --git a/src/drivers/linux_pwm_out/bbblue_pwm_rc.cpp b/src/drivers/linux_pwm_out/bbblue_pwm_rc.cpp index dad99a9310..b1641c02e2 100644 --- a/src/drivers/linux_pwm_out/bbblue_pwm_rc.cpp +++ b/src/drivers/linux_pwm_out/bbblue_pwm_rc.cpp @@ -30,7 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. * ****************************************************************************/ -#ifdef __PX4_POSIX_BBBLUE +#ifdef CONFIG_ARCH_BOARD_BEAGLEBONE_BLUE #include #include @@ -79,4 +79,4 @@ int BBBlueRcPWMOut::send_output_pwm(const uint16_t *pwm, int num_outputs) return ret; } -#endif // __PX4_POSIX_BBBLUE +#endif // CONFIG_ARCH_BOARD_BEAGLEBONE_BLUE diff --git a/src/drivers/linux_pwm_out/linux_pwm_out.cpp b/src/drivers/linux_pwm_out/linux_pwm_out.cpp index bf804c8a9e..1cbff01a58 100644 --- a/src/drivers/linux_pwm_out/linux_pwm_out.cpp +++ b/src/drivers/linux_pwm_out/linux_pwm_out.cpp @@ -235,7 +235,7 @@ void task_main(int argc, char *argv[]) PX4_INFO("Starting PWM output in ocpoc_mmap mode"); pwm_out = new OcpocMmapPWMOut(_max_num_outputs); -#ifdef __DF_BBBLUE +#ifdef CONFIG_ARCH_BOARD_BEAGLEBONE_BLUE } else if (strcmp(_protocol, "bbblue_rc") == 0) { PX4_INFO("Starting PWM output in bbblue_rc mode"); diff --git a/src/drivers/magnetometer/ist8310/ist8310.cpp b/src/drivers/magnetometer/ist8310/ist8310.cpp index 00b37c6b56..474faba7cb 100644 --- a/src/drivers/magnetometer/ist8310/ist8310.cpp +++ b/src/drivers/magnetometer/ist8310/ist8310.cpp @@ -874,9 +874,6 @@ IST8310::collect() /* post a report to the ring */ _reports->force(&new_report); - /* notify anyone waiting for data */ - poll_notify(POLLIN); - /* periodically check the range register and configuration registers. With a bad I2C cable it is possible for the diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp index 01f47e5d57..3a54c168bd 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp @@ -48,8 +48,6 @@ #include #include -#include - #include #include #include diff --git a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp index 7ad3d08d17..a694d4d098 100644 --- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp +++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp @@ -48,8 +48,6 @@ #include #include -#include - #include #include #include diff --git a/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp b/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp index 9664bbbcec..3085ed8ccc 100644 --- a/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp +++ b/src/drivers/magnetometer/qmc5883/qmc5883_i2c.cpp @@ -49,8 +49,6 @@ #include #include -#include - #include #include #include diff --git a/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp b/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp index f7b161b3ac..93478b2003 100644 --- a/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp +++ b/src/drivers/magnetometer/qmc5883/qmc5883_spi.cpp @@ -49,8 +49,6 @@ #include #include -#include - #include #include #include diff --git a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp index 928ba8d380..7c0728de2b 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_i2c.cpp @@ -48,7 +48,7 @@ #include #include -#include + #include #include diff --git a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp index 4b38ffc895..b081703da8 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_spi.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_spi.cpp @@ -48,8 +48,6 @@ #include #include -#include - #include #include #include diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 545cea37bc..5c7a446c0a 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -58,7 +58,7 @@ #include #include -#include + #include #include diff --git a/src/drivers/qshell/qurt/qshell.cpp b/src/drivers/qshell/qurt/qshell.cpp index 5f770842a6..9cd28c64fc 100644 --- a/src/drivers/qshell/qurt/qshell.cpp +++ b/src/drivers/qshell/qurt/qshell.cpp @@ -58,7 +58,6 @@ #include #include -#include "DriverFramework.hpp" #define MAX_ARGS 8 // max number of whitespace separated args after app name diff --git a/src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt b/src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt deleted file mode 100644 index 0441fe1fa6..0000000000 --- a/src/drivers/qurt/fc_addon/mpu_spi/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -############################################################################ -# -# Copyright (C) 2015 Mark Charlebois. 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. -# -############################################################################ - -set(TOOLS_ERROR_MSG - "The FC_ADDON must be installed and the environment variable FC_ADDON must be set" - "(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)") - -if ("$ENV{FC_ADDON}" STREQUAL "") - message(FATAL_ERROR ${TOOLS_ERROR_MSG}) -else() - set(FC_ADDON $ENV{FC_ADDON}) -endif() - -add_library(mpu9x50 SHARED IMPORTED GLOBAL) -set_target_properties(mpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libmpu9x50.so) - -px4_add_module( - MODULE platforms__qurt__fc_addon__mpu_spi - MAIN mpu9x50 - INCLUDES - ${FC_ADDON}/flight_controller/hexagon/inc - SRCS - mpu9x50_main.cpp - DEPENDS - ) -target_link_libraries(platforms__qurt__fc_addon__mpu_spi PRIVATE mpu9x50) diff --git a/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp deleted file mode 100644 index 26fe8d17f0..0000000000 --- a/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp +++ /dev/null @@ -1,641 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include - -#include - -#include -#include - -// TODO-JYW: -// Include references to the driver framework. This will produce a duplicate definition -// of qurt_log. Use a #define for the qurt_log function to avoid redefinition. This code -// change must still be made. Document the latter change to be clear. -// #include -// #include - -#include -#include -#include -#include -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif -#include -#include -#ifdef __cplusplus -} -#endif - -/** driver 'main' command */ -extern "C" { __EXPORT int mpu9x50_main(int argc, char *argv[]); } - -#define MAX_LEN_DEV_PATH 32 - -// TODO - need to load this from parameter file -#define SPI_INT_GPIO 65 // GPIO pin for Data Ready Interrupt -namespace mpu9x50 -{ - -/** SPI device path that mpu9x50 is connected to */ -static char _device[MAX_LEN_DEV_PATH]; - -/** flag indicating if mpu9x50 app is running */ -static bool _is_running = false; - -/** flag indicating if measurement thread should exit */ -static bool _task_should_exit = false; - -/** handle to the task main thread */ -static px4_task_t _task_handle = -1; - -/** IMU measurement data */ -static struct mpu9x50_data _data; - -static orb_advert_t _gyro_pub = nullptr; /**< gyro data publication */ -static int _gyro_orb_class_instance; /**< instance handle for gyro devices */ -static orb_advert_t _accel_pub = nullptr; /**< accelerameter data publication */ -static int _accel_orb_class_instance; /**< instance handle for accel devices */ -static orb_advert_t _mag_pub = nullptr; /**< compass data publication */ -static int _mag_orb_class_instance; /**< instance handle for mag devices */ -static int _params_sub; /**< parameter update subscription */ -static sensor_gyro_s _gyro; /**< gyro report */ -static sensor_accel_s _accel; /**< accel report */ -static sensor_mag_s _mag; /**< mag report */ -static struct gyro_calibration_s _gyro_sc; /**< gyro scale */ -static struct accel_calibration_s _accel_sc; /**< accel scale */ -static struct mag_calibration_s _mag_sc; /**< mag scale */ -static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ; /**< gyro lpf enum value */ -static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ; /**< accel lpf enum value */ -static enum gyro_sample_rate_e _imu_sample_rate = MPU9x50_SAMPLE_RATE_500HZ; /**< IMU sample rate enum */ - -struct { - param_t accel_x_offset; - param_t accel_x_scale; - param_t accel_y_offset; - param_t accel_y_scale; - param_t accel_z_offset; - param_t accel_z_scale; - param_t gyro_x_offset; - param_t gyro_y_offset; - param_t gyro_z_offset; - param_t mag_x_offset; - param_t mag_x_scale; - param_t mag_y_offset; - param_t mag_y_scale; - param_t mag_z_offset; - param_t mag_z_scale; - param_t gyro_lpf_enum; - param_t accel_lpf_enum; - param_t imu_sample_rate_enum; -} _params_handles; /**< parameter handles */ - -bool exit_mreasurement = false; - - -/** Print out the usage information */ -static void usage(); - -/** mpu9x50 stop measurement */ -static void stop(); - -/** task main trampoline function */ -static void task_main_trampoline(int argc, char *argv[]); - -/** mpu9x50 measurement thread primary entry point */ -static void task_main(int argc, char *argv[]); - -/** - * create and advertise all publicatoins - * @return true on success, false otherwise - */ -static bool create_pubs(); - -/** update all sensor reports with the latest sensor reading */ -static void update_reports(); - -/** publish all sensor reports */ -static void publish_reports(); - -/** update all parameters */ -static void parameters_update(); - -/** initialize all parameter handles and load the initial parameter values */ -static void parameters_init(); - -/** poll parameter update */ -static void parameter_update_poll(); - -static int64_t _isr_data_ready_timestamp = 0; - -/** - * MPU9x50 data ready interrupt service routine - * @param[out] context address of the context data provided by user whill - * registering the interrupt servcie routine - */ -static void *data_ready_isr(void *context); - -void *data_ready_isr(void *context) -{ - if (exit_mreasurement) { - return NULL; - } - - _isr_data_ready_timestamp = hrt_absolute_time(); - // send signal to measurement thread - px4_task_kill(_task_handle, SIGRTMIN); - - return NULL; -} - -void parameter_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -void parameters_update() -{ - PX4_DEBUG("mpu9x50_parameters_update"); - float v; - int v_int; - - // accel params - if (param_get(_params_handles.accel_x_offset, &v) == 0) { - _accel_sc.x_offset = v; - PX4_DEBUG("mpu9x50_parameters_update accel_x_offset %f", v); - } - - if (param_get(_params_handles.accel_x_scale, &v) == 0) { - _accel_sc.x_scale = v; - PX4_DEBUG("mpu9x50_parameters_update accel_x_scale %f", v); - } - - if (param_get(_params_handles.accel_y_offset, &v) == 0) { - _accel_sc.y_offset = v; - PX4_DEBUG("mpu9x50_parameters_update accel_y_offset %f", v); - } - - if (param_get(_params_handles.accel_y_scale, &v) == 0) { - _accel_sc.y_scale = v; - PX4_DEBUG("mpu9x50_parameters_update accel_y_scale %f", v); - } - - if (param_get(_params_handles.accel_z_offset, &v) == 0) { - _accel_sc.z_offset = v; - PX4_DEBUG("mpu9x50_parameters_update accel_z_offset %f", v); - } - - if (param_get(_params_handles.accel_z_scale, &v) == 0) { - _accel_sc.z_scale = v; - PX4_DEBUG("mpu9x50_parameters_update accel_z_scale %f", v); - } - - // gyro params - if (param_get(_params_handles.gyro_x_offset, &v) == 0) { - _gyro_sc.x_offset = v; - PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v); - } - - if (param_get(_params_handles.gyro_y_offset, &v) == 0) { - _gyro_sc.y_offset = v; - PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v); - } - - if (param_get(_params_handles.gyro_z_offset, &v) == 0) { - _gyro_sc.z_offset = v; - PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v); - } - - // mag params - if (param_get(_params_handles.mag_x_offset, &v) == 0) { - _mag_sc.x_offset = v; - PX4_DEBUG("mpu9x50_parameters_update mag_x_offset %f", v); - } - - if (param_get(_params_handles.mag_x_scale, &v) == 0) { - _mag_sc.x_scale = v; - PX4_DEBUG("mpu9x50_parameters_update mag_x_scale %f", v); - } - - if (param_get(_params_handles.mag_y_offset, &v) == 0) { - _mag_sc.y_offset = v; - PX4_DEBUG("mpu9x50_parameters_update mag_y_offset %f", v); - } - - if (param_get(_params_handles.mag_y_scale, &v) == 0) { - _mag_sc.y_scale = v; - PX4_DEBUG("mpu9x50_parameters_update mag_y_scale %f", v); - } - - if (param_get(_params_handles.mag_z_offset, &v) == 0) { - _mag_sc.z_offset = v; - PX4_DEBUG("mpu9x50_parameters_update mag_z_offset %f", v); - } - - if (param_get(_params_handles.mag_z_scale, &v) == 0) { - _mag_sc.z_scale = v; - PX4_DEBUG("mpu9x50_parameters_update mag_z_scale %f", v); - } - - // LPF params - if (param_get(_params_handles.gyro_lpf_enum, &v_int) == 0) { - if (v_int >= NUM_MPU9X50_GYRO_LPF) { - PX4_WARN("invalid gyro_lpf_enum %d use default %d", v_int, _gyro_lpf); - - } else { - _gyro_lpf = (enum gyro_lpf_e)v_int; - PX4_DEBUG("mpu9x50_parameters_update gyro_lpf_enum %d", _gyro_lpf); - } - } - - if (param_get(_params_handles.accel_lpf_enum, &v_int) == 0) { - if (v_int >= NUM_MPU9X50_ACC_LPF) { - PX4_WARN("invalid accel_lpf_enum %d use default %d", v_int, _accel_lpf); - - } else { - _accel_lpf = (enum acc_lpf_e)v_int; - PX4_DEBUG("mpu9x50_parameters_update accel_lpf_enum %d", _accel_lpf); - } - } - - if (param_get(_params_handles.imu_sample_rate_enum, &v_int) == 0) { - if (v_int >= NUM_MPU9X50_SAMPLE_RATE) { - PX4_WARN("invalid imu_sample_rate %d use default %d", v_int, _imu_sample_rate); - - } else { - _imu_sample_rate = (enum gyro_sample_rate_e)v_int; - PX4_DEBUG("mpu9x50_parameters_update imu_sample_rate %d", _imu_sample_rate); - } - } -} - -void parameters_init() -{ - _params_handles.accel_x_offset = param_find("CAL_ACC0_XOFF"); - _params_handles.accel_x_scale = param_find("CAL_ACC0_XSCALE"); - _params_handles.accel_y_offset = param_find("CAL_ACC0_YOFF"); - _params_handles.accel_y_scale = param_find("CAL_ACC0_YSCALE"); - _params_handles.accel_z_offset = param_find("CAL_ACC0_ZOFF"); - _params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE"); - - _params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF"); - _params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF"); - _params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF"); - - _params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF"); - _params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE"); - _params_handles.mag_y_offset = param_find("CAL_MAG0_YOFF"); - _params_handles.mag_y_scale = param_find("CAL_MAG0_YSCALE"); - _params_handles.mag_z_offset = param_find("CAL_MAG0_ZOFF"); - _params_handles.mag_z_scale = param_find("CAL_MAG0_ZSCALE"); - - _params_handles.gyro_lpf_enum = param_find("MPU_GYRO_LPF_ENM"); - _params_handles.accel_lpf_enum = param_find("MPU_ACC_LPF_ENM"); - - _params_handles.imu_sample_rate_enum = param_find("MPU_SAMPLE_R_ENM"); - - parameters_update(); -} - -bool create_pubs() -{ - // initialize the reports - memset(&_gyro, 0, sizeof(sensor_gyro_s)); - memset(&_accel, 0, sizeof(sensor_accel_s)); - memset(&_mag, 0, sizeof(sensor_mag_s)); - - _gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro, - &_gyro_orb_class_instance, ORB_PRIO_HIGH - 1); - - if (_gyro_pub == nullptr) { - PX4_ERR("sensor_gyro advert fail"); - return false; - } - - _accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel, - &_accel_orb_class_instance, ORB_PRIO_HIGH - 1); - - if (_accel_pub == nullptr) { - PX4_ERR("sensor_accel advert fail"); - return false; - } - - _mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag, - &_mag_orb_class_instance, ORB_PRIO_HIGH - 1); - - if (_mag_pub == nullptr) { - PX4_ERR("sensor_mag advert fail"); - return false; - } - - return true; -} - -void update_reports() -{ - _gyro.timestamp = _data.timestamp; - _gyro.x = (_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset; - _gyro.y = (_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset; - _gyro.z = (_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset; - _gyro.temperature = _data.temperature; - _gyro.scaling = _data.gyro_scaling; - _gyro.x_raw = _data.gyro_raw[0]; - _gyro.y_raw = _data.gyro_raw[1]; - _gyro.z_raw = _data.gyro_raw[2]; - - _accel.timestamp = _data.timestamp; - _accel.x = ((_data.accel_raw[0] * _data.accel_scaling) - _accel_sc.x_offset) * _accel_sc.x_scale; - _accel.y = ((_data.accel_raw[1] * _data.accel_scaling) - _accel_sc.y_offset) * _accel_sc.y_scale; - _accel.z = ((_data.accel_raw[2] * _data.accel_scaling) - _accel_sc.z_offset) * _accel_sc.z_scale; - _accel.temperature = _data.temperature; - _accel.scaling = _data.accel_scaling; - _accel.x_raw = _data.accel_raw[0]; - _accel.y_raw = _data.accel_raw[1]; - _accel.z_raw = _data.accel_raw[2]; - - if (_data.mag_data_ready) { - _mag.timestamp = _data.timestamp; - _mag.x = ((_data.mag_raw[0] * _data.mag_scaling) - _mag_sc.x_offset) * _mag_sc.x_scale; - _mag.y = ((_data.mag_raw[1] * _data.mag_scaling) - _mag_sc.y_offset) * _mag_sc.y_scale; - _mag.z = ((_data.mag_raw[2] * _data.mag_scaling) - _mag_sc.z_offset) * _mag_sc.z_scale; - _mag.scaling = _data.mag_scaling; - _mag.temperature = _data.temperature; - _mag.x_raw = _data.mag_raw[0]; - _mag.y_raw = _data.mag_raw[1]; - _mag.z_raw = _data.mag_raw[2]; - } -} - -void publish_reports() -{ - if (orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &_gyro) != OK) { - PX4_WARN("failed to publish gyro report"); - - } else { - //PX4_DEBUG("MPU_GYRO_RAW: %d %d %d", _gyro.x_raw, _gyro.y_raw, _gyro.z_raw) - //PX4_DEBUG("MPU_GYRO: %f %f %f", _gyro.x, _gyro.y, _gyro.z) - } - - if (orb_publish(ORB_ID(sensor_accel), _accel_pub, &_accel) != OK) { - PX4_WARN("failed to publish accel report"); - - } else { - //PX4_DEBUG("MPU_ACCEL_RAW: %d %d %d", _accel.x_raw, _accel.y_raw, _accel.z_raw) - //PX4_DEBUG("MPU_ACCEL: %f %f %f", _accel.x, _accel.y, _accel.z) - } - - if (_data.mag_data_ready) { - if (orb_publish(ORB_ID(sensor_mag), _mag_pub, &_mag) != OK) { - PX4_WARN("failed to publish mag report"); - - } else { - //PX4_DEBUG("MPU_MAG_RAW: %d %d %d", _mag.x_raw, _mag.y_raw, _mag.z_raw) - //PX4_DEBUG("MPU_MAG: %f %f %f", _mag.x, _mag.y, _mag.z) - } - } -} - -void task_main(int argc, char *argv[]) -{ - PX4_WARN("enter mpu9x50 task_main"); - - sigset_t set; - int sig = 0; - int rv; - exit_mreasurement = false; - - parameters_init(); - - // create the mpu9x50 default configuration structure - struct mpu9x50_config config = { - .gyro_lpf = _gyro_lpf, - .acc_lpf = _accel_lpf, - .gyro_fsr = MPU9X50_GYRO_FSR_500DPS, - .acc_fsr = MPU9X50_ACC_FSR_4G, - .gyro_sample_rate = _imu_sample_rate, - .compass_enabled = true, - .compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ, - .spi_dev_path = _device, - }; - - // TODO-JYW: - // manually register with the DriverFramework to allow this driver to - // be found by other modules -// DriverFramework::StubSensor stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel"); - - if (mpu9x50_initialize(&config) != 0) { - PX4_WARN("error initializing mpu9x50. Quit!"); - goto exit; - } - - if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL) - //if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL) - - != 0) { - PX4_WARN("error registering data ready interrupt. Quit!"); - goto exit; - } - - // create all uorb publications - if (!create_pubs()) { - goto exit; - } - - // subscribe to parameter_update topic - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - - // initialize signal - sigemptyset(&set); - sigaddset(&set, SIGRTMIN); - - while (!_task_should_exit) { - // wait on signal - rv = sigwait(&set, &sig); - - // check if we are waken up by the proper signal - if (rv != 0 || sig != SIGRTMIN) { - PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig); - continue; - } - - // read new IMU data and store the results in data - if (mpu9x50_get_data(&_data) != 0) { - PX4_WARN("mpu9x50_get_data() failed"); - continue; - } - - // since the context switch takes long time, override the timestamp provided by mpu9x50_get_data() - // with the ts of isr. - // Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue - // as the data is not consistent. - _data.timestamp = _isr_data_ready_timestamp; - - // poll parameter update - parameter_update_poll(); - - // data is ready - update_reports(); - - // publish all sensor reports - publish_reports(); - - } - - exit_mreasurement = true; - -exit: - PX4_WARN("closing mpu9x50"); - mpu9x50_close(); -} - -/** mpu9x50 main entrance */ -void task_main_trampoline(int argc, char *argv[]) -{ - PX4_WARN("task_main_trampoline"); - task_main(argc, argv); -} - -void start() -{ - ASSERT(_task_handle == -1); - - /* start the task */ - _task_handle = px4_task_spawn_cmd("mpu9x50_main", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 1500, - (px4_main_t)&task_main_trampoline, - nullptr); - - if (_task_handle < 0) { - warn("mpu9x50_main task start failed"); - return; - } - - _is_running = true; -} - -void stop() -{ - // TODO - set thread exit signal to terminate the task main thread - - _is_running = false; - _task_handle = -1; -} - -void usage() -{ - PX4_WARN("missing command: try 'start', 'stop', 'status'"); - PX4_WARN("options:"); - PX4_WARN(" -D device"); -} - -}; // namespace mpu9x50 - - -int mpu9x50_main(int argc, char *argv[]) -{ - const char *device = NULL; - int ch; - int myoptind = 1; - const char *myoptarg = NULL; - - while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'D': - device = myoptarg; - break; - - default: - mpu9x50::usage(); - return 1; - } - } - - // Check on required arguments - if (device == NULL || strlen(device) == 0) { - mpu9x50::usage(); - return 1; - } - - memset(mpu9x50::_device, 0, MAX_LEN_DEV_PATH); - strncpy(mpu9x50::_device, device, strlen(device)); - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - if (mpu9x50::_is_running) { - PX4_WARN("mpu9x50 already running"); - return 1; - } - - mpu9x50::start(); - - } else if (!strcmp(verb, "stop")) { - if (mpu9x50::_is_running) { - PX4_WARN("mpu9x50 is not running"); - return 1; - } - - mpu9x50::stop(); - - } else if (!strcmp(verb, "status")) { - PX4_WARN("mpu9x50 is %s", mpu9x50::_is_running ? "running" : "stopped"); - return 0; - - } else { - mpu9x50::usage(); - return 1; - } - - return 0; -} diff --git a/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c b/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c deleted file mode 100644 index 91f81cdc56..0000000000 --- a/src/drivers/qurt/fc_addon/mpu_spi/mpu9x50_params.c +++ /dev/null @@ -1,86 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file mpu9x50_params.c - * - * Parameters defined by the mpu9x50 driver - */ - -#include -#include - -/** - * Low pass filter frequency for Gyro - * - * @value 0 MPU9X50_GYRO_LPF_250HZ - * @value 1 MPU9X50_GYRO_LPF_184HZ - * @value 2 MPU9X50_GYRO_LPF_92HZ - * @value 3 MPU9X50_GYRO_LPF_41HZ - * @value 4 MPU9X50_GYRO_LPF_20HZ - * @value 5 MPU9X50_GYRO_LPF_10HZ - * @value 6 MPU9X50_GYRO_LPF_5HZ - * @value 7 MPU9X50_GYRO_LPF_3600HZ_NOLPF - * - * @group MPU9x50 Configuration - */ -PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENM, 4); - -/** - * Low pass filter frequency for Accelerometer - * - * - * @value 0 MPU9X50_ACC_LPF_460HZ - * @value 1 MPU9X50_ACC_LPF_184HZ - * @value 2 MPU9X50_ACC_LPF_92HZ - * @value 3 MPU9X50_ACC_LPF_41HZ - * @value 4 MPU9X50_ACC_LPF_20HZ - * @value 5 MPU9X50_ACC_LPF_10HZ - * @value 6 MPU9X50_ACC_LPF_5HZ - * @value 7 MPU9X50_ACC_LPF_460HZ_NOLPF - * - * @group MPU9x50 Configuration - */ -PARAM_DEFINE_INT32(MPU_ACC_LPF_ENM, 4); - -/** - * Sample rate in Hz - * - * @value 0 MPU9x50_SAMPLE_RATE_100HZ - * @value 1 MPU9x50_SAMPLE_RATE_200HZ - * @value 2 MPU9x50_SAMPLE_RATE_500HZ - * @value 3 MPU9x50_SAMPLE_RATE_1000HZ - * - * @group MPU9x50 Configuration - */ -PARAM_DEFINE_INT32(MPU_SAMPLE_R_ENM, 2); diff --git a/src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt b/src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt deleted file mode 100644 index d222a5be3a..0000000000 --- a/src/drivers/qurt/fc_addon/rc_receiver/CMakeLists.txt +++ /dev/null @@ -1,55 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 Mark Charlebois. 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 AtlFlight 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. -# -############################################################################ - -set(TOOLS_ERROR_MSG - "The FC_ADDON must be installed and the environment variable FC_ADDON must be set" - "(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)") - -if ("$ENV{FC_ADDON}" STREQUAL "") - message(FATAL_ERROR ${TOOLS_ERROR_MSG}) -else() - set(FC_ADDON $ENV{FC_ADDON}) -endif() - -add_library(rc_receiver SHARED IMPORTED GLOBAL) -set_target_properties(rc_receiver PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/librc_receiver.so) - -px4_add_module( - MODULE platforms__qurt__fc_addon__rc_receiver - MAIN rc_receiver - INCLUDES - ${FC_ADDON}/flight_controller/hexagon/inc - SRCS - rc_receiver_main.cpp - ) -target_link_libraries(platforms__qurt__fc_addon__rc_receiver PRIVATE rc_receiver) diff --git a/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp b/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp deleted file mode 100644 index a17b3e473f..0000000000 --- a/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp +++ /dev/null @@ -1,347 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif -#include -#include "rc_receiver_api.h" -#ifdef __cplusplus -} -#endif - -/** driver 'main' command */ -extern "C" { __EXPORT int rc_receiver_main(int argc, char *argv[]); } - -#define MAX_LEN_DEV_PATH 32 - -namespace rc_receiver -{ - -/** Threshold value to detect rc receiver signal lost in millisecond */ -#define SIGNAL_LOST_THRESHOLD_MS 500 - -/** serial device path that rc receiver is connected to */ -static char _device[MAX_LEN_DEV_PATH]; - -/** flag indicating if rc_receiver module is running */ -static bool _is_running = false; - -/** flag indicating if task thread should exit */ -static bool _task_should_exit = false; - -/** handle to the task main thread */ -static px4_task_t _task_handle = -1; - -/** RC receiver type */ -static enum RC_RECEIVER_TYPES _rc_receiver_type; - -/** RC input channel value array accomondating up to RC_INPUT_MAX_CHANNELS */ -static uint16_t rc_inputs[input_rc_s::RC_INPUT_MAX_CHANNELS]; - -/** rc_input uorb topic publication handle */ -static orb_advert_t _input_rc_pub = nullptr; - -/** rc_input uorb topic data */ -static struct input_rc_s _rc_in; - -/**< parameter update subscription */ -static int _params_sub; - -struct { - param_t rc_receiver_type; -} _params_handles; /**< parameter handles */ - -/** Print out the usage information */ -static void usage(); - -/** mpu9x50 start measurement */ -static void start(); - -/** mpu9x50 stop measurement */ -static void stop(); - -/** task main trampoline function */ -static void task_main_trampoline(int argc, char *argv[]); - -/** mpu9x50 measurement thread primary entry point */ -static void task_main(int argc, char *argv[]); - -/** update all parameters */ -static void parameters_update(); - -/** poll parameter update */ -static void parameter_update_poll(); - -void parameters_update() -{ - PX4_DEBUG("rc_receiver_parameters_update"); - float v; - - // accel params - if (param_get(_params_handles.rc_receiver_type, &v) == 0) { - _rc_receiver_type = (enum RC_RECEIVER_TYPES)v; - PX4_DEBUG("rc_receiver_parameters_update rc_receiver_type %f", v); - } -} - -void parameters_init() -{ - _params_handles.rc_receiver_type = param_find("RC_RECEIVER_TYPE"); - - parameters_update(); -} - -void parameter_update_poll() -{ - bool updated; - - /* Check if parameters have changed */ - orb_check(_params_sub, &updated); - - if (updated) { - struct parameter_update_s param_update; - orb_copy(ORB_ID(parameter_update), _params_sub, ¶m_update); - parameters_update(); - } -} - -void start() -{ - ASSERT(_task_handle == -1); - - /* start the task */ - _task_handle = px4_task_spawn_cmd("rc_receiver_main", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 1500, - (px4_main_t)&task_main_trampoline, - nullptr); - - if (_task_handle < 0) { - warn("task start failed"); - return; - } - - _is_running = true; -} - -void stop() -{ - // TODO - set thread exit signal to terminate the task main thread - - _is_running = false; - _task_handle = -1; -} - -void task_main_trampoline(int argc, char *argv[]) -{ - PX4_WARN("task_main_trampoline"); - task_main(argc, argv); -} - -void task_main(int argc, char *argv[]) -{ - PX4_WARN("enter rc_receiver task_main"); - uint32_t fd; - - // clear the rc_input report for topic advertise - memset(&_rc_in, 0, sizeof(struct input_rc_s)); - - _input_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc_in); - - if (_input_rc_pub == nullptr) { - PX4_WARN("failed to advertise input_rc uorb topic. Quit!"); - return ; - } - - // subscribe to parameter_update topic - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - - // Open the RC receiver device on the specified serial port - fd = rc_receiver_open(_rc_receiver_type, _device); - - if (fd <= 0) { - PX4_WARN("failed to open rc receiver type %d dev %s. Quit!", - _rc_receiver_type, _device); - return ; - } - - // Continuously receive RC packet from serial device, until task is signaled - // to exit - uint32_t num_channels; - uint64_t ts = hrt_absolute_time(); - int ret; - int counter = 0; - - _rc_in.timestamp_last_signal = ts; - - while (!_task_should_exit) { - // poll parameter update - parameter_update_poll(); - - // read RC packet from serial device in blocking mode. - num_channels = input_rc_s::RC_INPUT_MAX_CHANNELS; - - ret = rc_receiver_get_packet(fd, rc_inputs, &num_channels); - ts = hrt_absolute_time(); - - _rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_QURT; - - if (ret < 0) { - // enum RC_RECEIVER_ERRORS error_code = rc_receiver_get_last_error(fd); - // PX4_WARN("RC packet receiving timed out. error code %d", error_code); - - uint64_t time_diff_us = ts - _rc_in.timestamp_last_signal; - - if (time_diff_us > SIGNAL_LOST_THRESHOLD_MS * 1000) { - _rc_in.rc_lost = true; - - if (++counter == 500) { - PX4_WARN("RC signal lost for %u ms", time_diff_us / 1000); - counter = 0; - } - - } else { - continue; - } - } - - // populate the input_rc_s structure - if (ret == 0) { - _rc_in.timestamp = ts; - _rc_in.timestamp_last_signal = _rc_in.timestamp; - _rc_in.channel_count = num_channels; - - // TODO - need to add support for RSSI, failsafe mode - _rc_in.rssi = RC_INPUT_RSSI_MAX; - _rc_in.rc_failsafe = false; - _rc_in.rc_lost = false; - _rc_in.rc_lost_frame_count = 0; - _rc_in.rc_total_frame_count = 1; - } - - for (uint32_t i = 0; i < num_channels; i++) { - // Scale the Spektrum DSM value to ppm encoding. This is for the - // consistency with PX4 which internally translates DSM to PPM. - // See modules/px4iofirmware/dsm.c for details - // NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM - // format, so we need to double the channel value before the scaling - _rc_in.values[i] = ((((int)(rc_inputs[i] * 2) - 1024) * 1000) / 1700) + 1500; - } - - orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in); - } - - rc_receiver_close(fd); -} - -void usage() -{ - PX4_WARN("missing command: try 'start', 'stop', 'status'"); - PX4_WARN("options:"); - PX4_WARN(" -D device"); -} - -} // namespace rc_receiver - -int rc_receiver_main(int argc, char *argv[]) -{ - const char *device = NULL; - int ch; - int myoptind = 1; - const char *myoptarg = NULL; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'D': - device = myoptarg; - break; - - default: - rc_receiver::usage(); - return 1; - } - } - - // Check on required arguments - if (device == NULL || strlen(device) == 0) { - rc_receiver::usage(); - return 1; - } - - memset(rc_receiver::_device, 0, MAX_LEN_DEV_PATH); - strncpy(rc_receiver::_device, device, strlen(device)); - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - if (rc_receiver::_is_running) { - PX4_WARN("rc_receiver already running"); - return 1; - } - - rc_receiver::start(); - - } else if (!strcmp(verb, "stop")) { - if (rc_receiver::_is_running) { - PX4_WARN("rc_receiver is not running"); - return 1; - } - - rc_receiver::stop(); - - } else if (!strcmp(verb, "status")) { - PX4_WARN("rc_receiver is %s", rc_receiver::_is_running ? "running" : "stopped"); - return 0; - - } else { - rc_receiver::usage(); - return 1; - } - - return 0; -} diff --git a/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c b/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c deleted file mode 100644 index 485c3c089b..0000000000 --- a/src/drivers/qurt/fc_addon/rc_receiver/rc_receiver_params.c +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file rc_receiver_params.c - * - * Parameters defined by the rc_receiver driver - */ - -#include -#include - -/** - * RC receiver type - * - * Acceptable values: - * - * - RC_RECEIVER_SPEKTRUM = 1, - * - RC_RECEIVER_LEMONRX = 2, - * - * @group RC Receiver Configuration - */ -PARAM_DEFINE_INT32(RC_RECEIVER_TYPE, 1); diff --git a/src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt b/src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt deleted file mode 100644 index 5ccfb73af9..0000000000 --- a/src/drivers/qurt/fc_addon/uart_esc/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -############################################################################ -# -# Copyright (C) 2015 Mark Charlebois. 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. -# -############################################################################ - -set(TOOLS_ERROR_MSG - "The FC_ADDON must be installed and the environment variable FC_ADDON must be set" - "(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)") - -if ("$ENV{FC_ADDON}" STREQUAL "") - message(FATAL_ERROR ${TOOLS_ERROR_MSG}) -else() - set(FC_ADDON $ENV{FC_ADDON}) -endif() - -add_library(uart_esc SHARED IMPORTED GLOBAL) -set_target_properties(uart_esc PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libuart_esc.so) - -px4_add_module( - MODULE platforms__qurt__fc_addon__uart_esc - MAIN uart_esc - INCLUDES - ${FC_ADDON}/flight_controller/hexagon/inc - SRCS - uart_esc_main.cpp - DEPENDS - ) -target_link_libraries(platforms__qurt__fc_addon__uart_esc PRIVATE uart_esc) diff --git a/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp b/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp deleted file mode 100644 index 52a67e7768..0000000000 --- a/src/drivers/qurt/fc_addon/uart_esc/uart_esc_main.cpp +++ /dev/null @@ -1,521 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 Mark Charlebois. All rights reserved. -* -* Redistribution and use in source and binary forms, with or without -* modification, are permitted provided that the following conditions -* are met: -* -* 1. Redistributions of source code must retain the above copyright -* notice, this list of conditions and the following disclaimer. -* 2. Redistributions in binary form must reproduce the above copyright -* notice, this list of conditions and the following disclaimer in -* the documentation and/or other materials provided with the -* distribution. -* 3. Neither the name PX4 nor the names of its contributors may be -* used to endorse or promote products derived from this software -* without specific prior written permission. -* -* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -* POSSIBILITY OF SUCH DAMAGE. -* -****************************************************************************/ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#ifdef __cplusplus -extern "C" { -#endif -#include -#ifdef __cplusplus -} -#endif - -/** driver 'main' command */ -extern "C" { __EXPORT int uart_esc_main(int argc, char *argv[]); } - -#define MAX_LEN_DEV_PATH 32 - -namespace uart_esc -{ -#define UART_ESC_MAX_MOTORS 4 - -volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit -static char _device[MAX_LEN_DEV_PATH]; -static bool _is_running = false; // flag indicating if uart_esc app is running -static px4_task_t _task_handle = -1; // handle to the task main thread -UartEsc *esc; // esc instance -void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors); // motor re-mapping - -// subscriptions -int _controls_sub; -int _armed_sub; -int _param_sub; - -// filenames -// /dev/fs/ is mapped to /usr/share/data/adsp/ -static const char *MIXER_FILENAME = "/dev/fs/mixer_config.mix"; - - -// publications -orb_advert_t _outputs_pub; - -// topic structures -actuator_controls_s _controls; -actuator_armed_s _armed; -parameter_update_s _param_update; -actuator_outputs_s _outputs; - -/** Print out the usage information */ -static void usage(); - -/** uart_esc start */ -static void start(const char *device) __attribute__((unused)); - -/** uart_esc stop */ -static void stop(); - -/** task main trampoline function */ -static void task_main_trampoline(int argc, char *argv[]); - -/** uart_esc thread primary entry point */ -static void task_main(int argc, char *argv[]); - -/** mixer initialization */ -static MultirotorMixer *mixer; -static int initialize_mixer(const char *mixer_filename); -static int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - -static void parameters_init(); -static void parameters_update(); - -struct { - int model; - int baudrate; - int px4_motor_mapping[UART_ESC_MAX_MOTORS]; -} _parameters; - -struct { - param_t model; - param_t baudrate; - param_t px4_motor_mapping[UART_ESC_MAX_MOTORS]; -} _parameter_handles; - -void parameters_init() -{ - _parameter_handles.model = param_find("UART_ESC_MODEL"); - _parameter_handles.baudrate = param_find("UART_ESC_BAUD"); - - /* PX4 motor mapping parameters */ - for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) { - char nbuf[20]; - - /* min values */ - sprintf(nbuf, "UART_ESC_MOTOR%d", i + 1); - _parameter_handles.px4_motor_mapping[i] = param_find(nbuf); - } - - parameters_update(); -} - -void parameters_update() -{ - PX4_WARN("uart_esc_main parameters_update"); - int32_t v_int; - - if (param_get(_parameter_handles.model, &v_int) == 0) { - _parameters.model = v_int; - PX4_WARN("UART_ESC_MODEL %d", _parameters.model); - } - - if (param_get(_parameter_handles.baudrate, &v_int) == 0) { - _parameters.baudrate = v_int; - PX4_WARN("UART_ESC_BAUD %d", _parameters.baudrate); - } - - for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) { - if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) { - _parameters.px4_motor_mapping[i] = v_int; - PX4_WARN("UART_ESC_MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]); - } - } -} - -int mixer_control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) -{ - const actuator_controls_s *controls = (actuator_controls_s *)handle; - - input = controls[control_group].control[control_index]; - - /* motor spinup phase - lock throttle to zero * - if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) { - if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE && - control_index == actuator_controls_s::INDEX_THROTTLE) { - * limit the throttle output to zero during motor spinup, - * as the motors cannot follow any demand yet - * - input = 0.0f; - } - } - */ - return 0; -} - - -int initialize_mixer(const char *mixer_filename) -{ - mixer = nullptr; - - int mixer_initialized = -1; - - char buf[2048]; - unsigned int buflen = sizeof(buf); - - PX4_INFO("Initializing mixer from config file in %s", mixer_filename); - - int fd_load = open(mixer_filename, O_RDONLY); - - if (fd_load != -1) { - int nRead = read(fd_load, buf, buflen); - close(fd_load); - - if (nRead > 0) { - mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen); - - if (mixer != nullptr) { - PX4_INFO("Successfully initialized mixer from config file"); - mixer_initialized = 0; - - } else { - PX4_WARN("Unable to parse from mixer config file"); - } - - } else { - PX4_WARN("Unable to read from mixer config file"); - } - - } else { - PX4_WARN("Unable to open mixer config file"); - } - - // mixer file loading failed, fall back to default mixer configuration for - // QUAD_X airframe - if (mixer_initialized < 0) { - float roll_scale = 1; - float pitch_scale = 1; - float yaw_scale = 1; - float deadband = 0; - - mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls, - MultirotorGeometry::QUAD_X, - roll_scale, pitch_scale, yaw_scale, deadband); - - if (mixer == nullptr) { - PX4_ERR("mixer initialization failed"); - mixer_initialized = -1; - return mixer_initialized; - } - - PX4_WARN("mixer config file not found, successfully initialized default quad x mixer"); - mixer_initialized = 0; - } - - return mixer_initialized; -} - -/** -* Rotate the motor rpm values based on the motor mappings configuration stored -* in motor_mapping -*/ -void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors) -{ - ASSERT(num_rotors <= UART_ESC_MAX_MOTORS); - int i; - int16_t motor_rpm_copy[UART_ESC_MAX_MOTORS]; - - for (i = 0; i < num_rotors; i++) { - motor_rpm_copy[i] = motor_rpm[i]; - } - - for (i = 0; i < num_rotors; i++) { - motor_rpm[_parameters.px4_motor_mapping[i] - 1] = motor_rpm_copy[i]; - } -} - -void task_main(int argc, char *argv[]) -{ - PX4_INFO("enter uart_esc task_main"); - - _outputs_pub = nullptr; - - parameters_init(); - - esc = UartEsc::get_instance(); - - if (esc == NULL) { - PX4_ERR("failed to new UartEsc instance"); - - } else if (esc->initialize((enum esc_model_t)_parameters.model, - _device, _parameters.baudrate) < 0) { - PX4_ERR("failed to initialize UartEsc"); - - } else { - // Subscribe for orb topics - _controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now - _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - _param_sub = orb_subscribe(ORB_ID(parameter_update)); - - // initialize publication structures - memset(&_outputs, 0, sizeof(_outputs)); - - // set up poll topic and limit poll interval - px4_pollfd_struct_t fds[1]; - fds[0].fd = _controls_sub; - fds[0].events = POLLIN; - //orb_set_interval(_controls_sub, 10); // max actuator update period, ms - - // set up mixer - if (initialize_mixer(MIXER_FILENAME) < 0) { - PX4_ERR("Mixer initialization failed."); - _task_should_exit = true; - } - - // Main loop - while (!_task_should_exit) { - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } - - // Handle new actuator controls data - if (fds[0].revents & POLLIN) { - - // Grab new controls data - orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls); - // Mix to the outputs - _outputs.timestamp = hrt_absolute_time(); - int16_t motor_rpms[UART_ESC_MAX_MOTORS]; - - if (_armed.armed && !_armed.lockdown && !_armed.manual_lockdown) { - _outputs.noutputs = mixer->mix(&_outputs.output[0], actuator_controls_s::NUM_ACTUATOR_CONTROLS); - - // Make sure we support only up to UART_ESC_MAX_MOTORS motors - if (_outputs.noutputs > UART_ESC_MAX_MOTORS) { - PX4_ERR("Unsupported motors %d, up to %d motors supported", - _outputs.noutputs, UART_ESC_MAX_MOTORS); - continue; - } - - // Send outputs to the ESCs - for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { - // map -1.0 - 1.0 outputs to RPMs - motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) * - (esc->max_rpm() - esc->min_rpm()) + esc->min_rpm()); - } - - uart_esc_rotate_motors(motor_rpms, _outputs.noutputs); - - } else { - _outputs.noutputs = UART_ESC_MAX_MOTORS; - - for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) { - motor_rpms[outIdx] = 0; - _outputs.output[outIdx] = -1.0; - } - } - - esc->send_rpms(&motor_rpms[0], _outputs.noutputs); - - // TODO-JYW: TESTING-TESTING - // MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS -// static int count=0; -// count++; -// if (!(count % 100)) { -// PX4_DEBUG(" "); -// PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed); -// PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]); -// PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]); -// } - // TODO-JYW: TESTING-TESTING - - - /* Publish mixed control outputs */ - if (_outputs_pub != nullptr) { - orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs); - - } else { - _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs); - } - } - - // Check for updates in other subscripions - bool updated = false; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed); - } - - orb_check(_param_sub, &updated); - - if (updated) { - // Even though we are only interested in the update status of the parameters, copy - // the subscription to clear the update status. - orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update); - parameters_update(); - } - } - } - - PX4_WARN("closing uart_esc"); - - delete esc; -} - -/** uart_esc main entrance */ -void task_main_trampoline(int argc, char *argv[]) -{ - PX4_WARN("task_main_trampoline"); - task_main(argc, argv); -} - -void start() -{ - ASSERT(_task_handle == -1); - - /* start the task */ - _task_handle = px4_task_spawn_cmd("uart_esc_main", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX, - 1500, - (px4_main_t)&task_main_trampoline, - nullptr); - - if (_task_handle < 0) { - warn("task start failed"); - return; - } - - _is_running = true; -} - -void stop() -{ - // TODO - set thread exit signal to terminate the task main thread - - _is_running = false; - _task_handle = -1; -} - -void usage() -{ - PX4_WARN("missing command: try 'start', 'stop', 'status'"); - PX4_WARN("options:"); - PX4_WARN(" -D device"); -} - -}; // namespace uart_esc - -int uart_esc_main(int argc, char *argv[]) -{ - const char *device = NULL; - int ch; - int myoptind = 1; - const char *myoptarg = NULL; - - while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'D': - device = myoptarg; - break; - - default: - uart_esc::usage(); - return 1; - } - } - - // Check on required arguments - if (device == NULL || strlen(device) == 0) { - uart_esc::usage(); - return 1; - } - - memset(uart_esc::_device, 0, MAX_LEN_DEV_PATH); - strncpy(uart_esc::_device, device, strlen(device)); - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - if (uart_esc::_is_running) { - PX4_WARN("uart_esc already running"); - return 1; - } - - uart_esc::start(); - } - - else if (!strcmp(verb, "stop")) { - if (uart_esc::_is_running) { - PX4_WARN("uart_esc is not running"); - return 1; - } - - uart_esc::stop(); - } - - else if (!strcmp(verb, "status")) { - PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "stopped"); - return 0; - - } else { - uart_esc::usage(); - return 1; - } - - return 0; -} diff --git a/src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c b/src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c deleted file mode 100644 index 657179031a..0000000000 --- a/src/drivers/qurt/fc_addon/uart_esc/uart_esc_params.c +++ /dev/null @@ -1,139 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Ramakrishna Kintada. 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. - * - ****************************************************************************/ - -/** - * @file uart_esc_params.c - * - * Parameters defined for the uart esc driver - */ -#include -#include - -/** - * ESC model - * - * See esc_model_t enum definition in uart_esc_dev.h for all supported ESC - * model enum values. - * - * @value 0 ESC_200QX - * @value 1 ESC_350QX - * @value 2 ESC_210QC - * - * @group Snapdragon UART ESC - */ -PARAM_DEFINE_INT32(UART_ESC_MODEL, 2); - -/** - * ESC UART baud rate - * - * Default rate is 250Kbps, whic is used in off-the-shelf QRP ESC products. - * @group Snapdragon UART ESC - */ -PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000); - -/** - * The PX4 default motor mappings are - * 1 4 - * [front] - * 3 2 - * - * The following paramters define the motor mappings in reference to the - * PX4 motor mapping convention. - */ -/** - * Default PX4 motor mappings - * 1 4 - * [front] - * 3 2 - */ -// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 1); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 3); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 4); - -/** - * Motor mappings for 350QX - * 4 3 - * [front] - * 1 2 - */ -// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3); - -/** - * Motor mappings for 200QX - * 2 3 - * [front] - * 1 4 - */ -// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 2); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 4); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1); -// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3); - -/** - * Motor mappings for 210QC - * 4 3 - * [front] - * 1 2 - */ - -/** - * Motor 1 Mapping - * - * @group Snapdragon UART ESC - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4); - -/** - * Motor 2 Mapping - * - * @group Snapdragon UART ESC - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2); - -/** - * Motor 3 Mapping - * - * @group Snapdragon UART ESC - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1); - -/** - * Motor 4 Mapping - * - * @group Snapdragon UART ESC - */ -PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3); diff --git a/src/drivers/qurt/tests/hello/CMakeLists.txt b/src/drivers/qurt/tests/hello/CMakeLists.txt deleted file mode 100644 index d0e41237b5..0000000000 --- a/src/drivers/qurt/tests/hello/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE platforms__qurt__tests__hello - MAIN hello - SRCS - hello_main.cpp - hello_start_qurt.cpp - hello_example.cpp - DEPENDS - ) diff --git a/src/drivers/qurt/tests/hello/hello_example.cpp b/src/drivers/qurt/tests/hello/hello_example.cpp deleted file mode 100644 index ef2a2d5e6b..0000000000 --- a/src/drivers/qurt/tests/hello/hello_example.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file hello_example.cpp - * Example for Linux - * - * @author Mark Charlebois - */ - -#include "hello_example.h" -#include -#include -#include - -px4::AppState HelloExample::appState; - -int HelloExample::main() -{ - appState.setRunning(true); - - int i = 0; - - while (!appState.exitRequested() && i < 5) { - - PX4_DEBUG(" Doing work..."); - ++i; - } - - return 0; -} diff --git a/src/drivers/qurt/tests/hello/hello_example.h b/src/drivers/qurt/tests/hello/hello_example.h deleted file mode 100644 index 48eb882abe..0000000000 --- a/src/drivers/qurt/tests/hello/hello_example.h +++ /dev/null @@ -1,54 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file hello_example.h - * Example app for Linux - * - * @author Mark Charlebois - */ -#pragma once - -#include - -class HelloExample -{ -public: - HelloExample() {} - - ~HelloExample() {} - - int main(); - - static px4::AppState appState; /* track requests to terminate app */ -}; diff --git a/src/drivers/qurt/tests/hello/hello_main.cpp b/src/drivers/qurt/tests/hello/hello_main.cpp deleted file mode 100644 index cee761b1a4..0000000000 --- a/src/drivers/qurt/tests/hello/hello_main.cpp +++ /dev/null @@ -1,56 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file hello_main.cpp - * Example for Linux - * - * @author Mark Charlebois - */ -#include -#include -#include -#include "hello_example.h" -#include - -int PX4_MAIN(int argc, char **argv) -{ - px4::init(argc, argv, "hello"); - - PX4_DEBUG("hello"); - HelloExample hello; - hello.main(); - - PX4_DEBUG("goodbye"); - return 0; -} diff --git a/src/drivers/qurt/tests/hello/hello_start_qurt.cpp b/src/drivers/qurt/tests/hello/hello_start_qurt.cpp deleted file mode 100644 index cb933665f3..0000000000 --- a/src/drivers/qurt/tests/hello/hello_start_qurt.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file hello_start_linux.cpp - * - * @author Thomas Gubler - * @author Mark Charlebois - */ -#include "hello_example.h" -#include -#include -#include -#include -#include -#include - -static int daemon_task; /* Handle of deamon task / thread */ - -//using namespace px4; - -extern "C" __EXPORT int hello_main(int argc, char *argv[]); - -static void usage() -{ - PX4_DEBUG("usage: hello {start|stop|status}"); -} -int hello_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (HelloExample::appState.isRunning()) { - PX4_DEBUG("already running"); - /* this is not an error */ - return 0; - } - - daemon_task = px4_task_spawn_cmd("hello", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, - (char *const *)argv); - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - HelloExample::appState.requestExit(); - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (HelloExample::appState.isRunning()) { - PX4_DEBUG("is running"); - - } else { - PX4_DEBUG("not started"); - } - - return 0; - } - - usage(); - return 1; -} diff --git a/src/drivers/qurt/tests/muorb/CMakeLists.txt b/src/drivers/qurt/tests/muorb/CMakeLists.txt deleted file mode 100644 index c894349f07..0000000000 --- a/src/drivers/qurt/tests/muorb/CMakeLists.txt +++ /dev/null @@ -1,40 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 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. -# -############################################################################ -px4_add_module( - MODULE platforms__qurt__tests__muorb - MAIN muorb_test - SRCS - muorb_test_start_qurt.cpp - muorb_test_example.cpp - DEPENDS - ) diff --git a/src/drivers/qurt/tests/muorb/muorb_test_example.cpp b/src/drivers/qurt/tests/muorb/muorb_test_example.cpp deleted file mode 100644 index bbe8f501b0..0000000000 --- a/src/drivers/qurt/tests/muorb/muorb_test_example.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file muorb_test_example.cpp - * Example for muorb - * - * @author Mark Charlebois - */ - -#include "muorb_test_example.h" -#include -#include -#include -#include -#include "uORB/topics/sensor_combined.h" -#include "uORB/topics/pwm_input.h" -#include "modules/uORB/uORB.h" -#include "px4_middleware.h" -#include "px4_defines.h" -#include -#include - -px4::AppState MuorbTestExample::appState; - -int MuorbTestExample::main() -{ - int rc; - appState.setRunning(true); - rc = PingPongTest(); - rc = FileReadTest(); - rc = uSleepTest(); - appState.setRunning(false); - return rc; -} - -int MuorbTestExample::DefaultTest() -{ - struct pwm_input_s pwm; - struct sensor_combined_s sc; - - memset(&pwm, 0, sizeof(pwm_input_s)); - memset(&sc, 0, sizeof(sensor_combined_s)); - PX4_WARN("Successful after memset... "); - orb_advert_t pub_fd = orb_advertise(ORB_ID(pwm_input), &pwm); - - if (pub_fd == nullptr) { - PX4_WARN("Error: advertizing pwm_input topic"); - return -1; - } - - orb_advert_t pub_sc = orb_advertise(ORB_ID(sensor_combined), &sc); - - if (pub_sc == nullptr) { - PX4_WARN("Error: advertizing sensor_combined topic"); - return -1; - } - - int i = 0; - pwm.error_count++; - /*sc.gyro_errcount[i]++;*/ // no member named 'gyro_errcount' in 'sensor_combined_s' - - while (!appState.exitRequested() && i < 10) { - - PX4_INFO(" Doing work..."); - orb_publish(ORB_ID(pwm_input), pub_fd, &pwm); - orb_publish(ORB_ID(sensor_combined), pub_sc, &sc); - - usleep(1000000); - ++i; - } - - return 0; -} - -int MuorbTestExample::PingPongTest() -{ - int i = 0; - orb_advert_t pub_id_esc_status = orb_advertise(ORB_ID(esc_status), & m_esc_status); - - if (pub_id_esc_status == 0) { - PX4_ERR("error publishing esc_status"); - return -1; - } - - if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the esc_status message", i); - return -1; - } - - int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); - - if (sub_vc == PX4_ERROR) { - PX4_ERR("Error subscribing to vehicle_command topic"); - return -1; - } - - while (!appState.exitRequested()) { - - PX4_DEBUG("[%d] Doing work...", i); - bool updated = false; - - if (orb_check(sub_vc, &updated) == 0) { - if (updated) { - PX4_WARN("[%d]vechile command status is updated... reading new value", i); - - if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) { - PX4_ERR("[%d]Error calling orb copy for vechicle... ", i); - break; - } - - if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) { - PX4_ERR("[%d]Error publishing the esc_status message", i); - break; - } - - } else { - PX4_WARN("[%d] vechicle command topic is not updated", i); - } - - } else { - PX4_ERR("[%d]Error checking the updated status for vehicle command ", i); - break; - } - - // sleep for 1 sec. - usleep(1000000); - - ++i; - } - - return 0; -} - -int MuorbTestExample::uSleepTest() -{ - PX4_WARN("before usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time()); - usleep(1000000); - PX4_INFO("After usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time()); - - for (int i = 0; i < 10; ++i) { - PX4_INFO("In While Loop: B4 Sleep for[%d] seconds [%" PRIu64 "]", i + 1, hrt_absolute_time()); - usleep((i + 1) * 1000000); - PX4_INFO("In While Loop: After Sleep for[%d] seconds [%" PRIu64 "]", i + 1, hrt_absolute_time()); - } - - PX4_INFO("exiting sleep test..."); - return 0; -} - -int MuorbTestExample::FileReadTest() -{ - int rc = OK; - static const char TEST_FILE_PATH[] = "./test.txt"; - FILE *fp; - char *line = NULL; - /*size_t len = 0; - ssize_t read;*/ - - fp = fopen(TEST_FILE_PATH, "r"); - - if (fp == NULL) { - PX4_WARN("unable to open file[%s] for reading", TEST_FILE_PATH); - rc = PX4_ERROR; - - } else { - /* - int i = 0; - while( ( read = getline( &line, &len, fp ) ) != -1 ) - { - ++i; - PX4_INFO( "LineNum[%d] LineLength[%d]", i, len ); - PX4_INFO( "LineNum[%d] Line[%s]", i, line ); - } - */ - PX4_INFO("Successfully opened file [%s]", TEST_FILE_PATH); - fclose(fp); - - if (line != NULL) { - free(line); - } - } - - return rc; -} diff --git a/src/drivers/qurt/tests/muorb/muorb_test_example.h b/src/drivers/qurt/tests/muorb/muorb_test_example.h deleted file mode 100644 index ccd6a3595e..0000000000 --- a/src/drivers/qurt/tests/muorb/muorb_test_example.h +++ /dev/null @@ -1,59 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include "uORB/topics/esc_status.h" -#include "uORB/topics/vehicle_command.h" - -class MuorbTestExample -{ -public: - MuorbTestExample() {} - ~MuorbTestExample() {} - - int main(); - - static px4::AppState appState; /* track requests to terminate app */ - -private: - int DefaultTest(); - int PingPongTest(); - int FileReadTest(); - int uSleepTest(); - - struct esc_status_s m_esc_status; - struct vehicle_command_s m_vc = {}; - -}; diff --git a/src/drivers/qurt/tests/muorb/muorb_test_main.cpp b/src/drivers/qurt/tests/muorb/muorb_test_main.cpp deleted file mode 100644 index b818f5e79c..0000000000 --- a/src/drivers/qurt/tests/muorb/muorb_test_main.cpp +++ /dev/null @@ -1,57 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file muorb_test_main.cpp - * Test of Multi-uORB supoprt - * - * @author Mark Charlebois - */ -#include -#include -#include -#include "muorb_test_example.h" - -extern "C" __EXPORT int muorb_test_entry(int argc, char **argv); - -int muorb_test_entry(int argc, char **argv) -{ - px4::init(argc, argv, "muorb_test"); - - PX4_DEBUG("muorb_test"); - MuorbTestExample hello; - hello.main(); - - PX4_DEBUG("goodbye"); - return 0; -} diff --git a/src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp deleted file mode 100644 index cd4e25fa12..0000000000 --- a/src/drivers/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2015 Mark Charlebois. 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. - * - ****************************************************************************/ - -/** - * @file muorb_test_start_qurt.cpp - * - * @author Mark Charlebois - */ -#include "muorb_test_example.h" -#include -#include -#include -#include -#include -#include -#include - -static int daemon_task; /* Handle of deamon task / thread */ - -//using namespace px4; - -extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); - -int muorb_test_entry(int argc, char **argv) -{ - //px4::init(argc, argv, "muorb_test"); - - PX4_INFO("muorb_test entry....."); - MuorbTestExample hello; - hello.main(); - - PX4_INFO("goodbye"); - return 0; -} - -static void usage() -{ - PX4_DEBUG("usage: muorb_test {start|stop|status}"); -} -int muorb_test_main(int argc, char *argv[]) -{ - if (argc < 2) { - usage(); - return 1; - } - - if (!strcmp(argv[1], "start")) { - - if (MuorbTestExample::appState.isRunning()) { - PX4_DEBUG("already running"); - /* this is not an error */ - return 0; - } - - PX4_INFO("before starting the muorb_test_entry task"); - - daemon_task = px4_task_spawn_cmd("muorb_test", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 5, - 8192, - muorb_test_entry, - (char *const *)argv); - - return 0; - } - - if (!strcmp(argv[1], "stop")) { - MuorbTestExample::appState.requestExit(); - return 0; - } - - if (!strcmp(argv[1], "status")) { - if (MuorbTestExample::appState.isRunning()) { - PX4_DEBUG("is running"); - - } else { - PX4_DEBUG("not started"); - } - - return 0; - } - - usage(); - return 1; -} diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index b2bcd1906b..eae1504e03 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -51,7 +51,7 @@ #include #include -#include + #include #include diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index cc8dabc617..70ede7688f 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -54,7 +54,7 @@ #include -#include + #include "RoboClaw.hpp" static bool thread_running = false; /**< Deamon status flag */ diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp index e9ad5eeaef..73fe29e73a 100644 --- a/src/drivers/uavcan/uavcan_main.cpp +++ b/src/drivers/uavcan/uavcan_main.cpp @@ -51,7 +51,7 @@ #include #include #include -#include + #include #include diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp index 63c442f453..a11590b838 100644 --- a/src/drivers/uavcan/uavcan_servers.cpp +++ b/src/drivers/uavcan/uavcan_servers.cpp @@ -45,7 +45,7 @@ #include #include #include -#include + #include #include "uavcan_main.hpp" diff --git a/src/drivers/uavcannode/led.cpp b/src/drivers/uavcannode/led.cpp index fcadc5c4ee..7583c7812d 100644 --- a/src/drivers/uavcannode/led.cpp +++ b/src/drivers/uavcannode/led.cpp @@ -35,7 +35,7 @@ #include -#include + #include "hardware/stm32_tim.h" diff --git a/src/examples/bottle_drop/bottle_drop.cpp b/src/examples/bottle_drop/bottle_drop.cpp index c11d64fb56..49393c49bc 100644 --- a/src/examples/bottle_drop/bottle_drop.cpp +++ b/src/examples/bottle_drop/bottle_drop.cpp @@ -54,7 +54,7 @@ #include #include #include -#include + #include #include #include diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework deleted file mode 160000 index c7d2fe2d4f..0000000000 --- a/src/lib/DriverFramework +++ /dev/null @@ -1 +0,0 @@ -Subproject commit c7d2fe2d4f5954b9042af4edf7e5aeb21dbdd129 diff --git a/src/lib/cdev/CDev.cpp b/src/lib/cdev/CDev.cpp index 336dd6fa34..40d9406997 100644 --- a/src/lib/cdev/CDev.cpp +++ b/src/lib/cdev/CDev.cpp @@ -188,27 +188,6 @@ CDev::close(file_t *filep) return ret; } -int -CDev::ioctl(file_t *filep, int cmd, unsigned long arg) -{ - PX4_DEBUG("CDev::ioctl"); - int ret = -ENOTTY; - - switch (cmd) { - - /* fetch a pointer to the driver's private data */ - case DIOC_GETPRIV: - *(void **)(uintptr_t)arg = (void *)this; - ret = PX4_OK; - break; - - default: - break; - } - - return ret; -} - int CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) { @@ -327,7 +306,7 @@ CDev::poll(file_t *filep, px4_pollfd_struct_t *fds, bool setup) } void -CDev::poll_notify(pollevent_t events) +CDev::poll_notify(px4_pollevent_t events) { PX4_DEBUG("CDev::poll_notify events = %0x", events); @@ -344,7 +323,7 @@ CDev::poll_notify(pollevent_t events) } void -CDev::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +CDev::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) { PX4_DEBUG("CDev::poll_notify_one"); diff --git a/src/lib/cdev/CDev.hpp b/src/lib/cdev/CDev.hpp index 3e62807b15..a1ee318a48 100644 --- a/src/lib/cdev/CDev.hpp +++ b/src/lib/cdev/CDev.hpp @@ -137,8 +137,8 @@ public: /** * Perform an ioctl operation on the device. * - * The default implementation handles DIOC_GETPRIV, and otherwise - * returns -ENOTTY. Subclasses should call the default implementation + * The default implementation returns -ENOTTY. + * Subclasses should call the default implementation * for any command they do not handle themselves. * * @param filep Pointer to the NuttX file structure. @@ -146,7 +146,7 @@ public: * @param arg The ioctl argument value. * @return OK on success, or -errno otherwise. */ - virtual int ioctl(file_t *filep, int cmd, unsigned long arg); + virtual int ioctl(file_t *filep, int cmd, unsigned long arg) { return -ENOTTY; }; /** * Perform a poll setup/teardown operation. @@ -187,7 +187,7 @@ protected: * @param filep The file that's interested. * @return The current set of poll events. */ - virtual pollevent_t poll_state(file_t *filep) { return 0; } + virtual px4_pollevent_t poll_state(file_t *filep) { return 0; } /** * Report new poll events. @@ -197,7 +197,7 @@ protected: * * @param events The new event(s) being announced. */ - virtual void poll_notify(pollevent_t events); + virtual void poll_notify(px4_pollevent_t events); /** * Internal implementation of poll_notify. @@ -205,7 +205,7 @@ protected: * @param fds A poll waiter to notify. * @param events The event(s) to send to the waiter. */ - virtual void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events); + virtual void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events); /** * Notification of the first open. diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp index 0491b68ab4..c643792f0a 100644 --- a/src/lib/cdev/posix/cdev_platform.cpp +++ b/src/lib/cdev/posix/cdev_platform.cpp @@ -44,10 +44,7 @@ #include #include -#include "DevMgr.hpp" - using namespace std; -using namespace DriverFramework; const cdev::px4_file_operations_t cdev::CDev::fops = {}; @@ -60,8 +57,6 @@ static cdev::file_t filemap[PX4_MAX_FD] = {}; extern "C" { - int px4_errno; - static cdev::CDev *getDev(const char *path) { PX4_DEBUG("CDev::getDev"); @@ -233,7 +228,6 @@ extern "C" { } if (ret < 0) { - px4_errno = -ret; ret = PX4_ERROR; } @@ -255,7 +249,6 @@ extern "C" { } if (ret < 0) { - px4_errno = -ret; ret = PX4_ERROR; } @@ -277,7 +270,6 @@ extern "C" { } if (ret < 0) { - px4_errno = -ret; ret = PX4_ERROR; } @@ -298,14 +290,10 @@ extern "C" { ret = -EINVAL; } - if (ret < 0) { - px4_errno = -ret; - } - return ret; } - int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout) + int px4_poll(px4_pollfd_struct_t *fds, unsigned int nfds, int timeout) { if (nfds == 0) { PX4_WARN("px4_poll with no fds"); @@ -315,7 +303,6 @@ extern "C" { px4_sem_t sem; int count = 0; int ret = -1; - unsigned int i; const unsigned NAMELEN = 32; char thread_name[NAMELEN] = {}; @@ -339,7 +326,7 @@ extern "C" { // Go through all fds and check them for a pollable state bool fd_pollable = false; - for (i = 0; i < nfds; ++i) { + for (unsigned int i = 0; i < nfds; ++i) { fds[i].sem = &sem; fds[i].revents = 0; fds[i].priv = nullptr; @@ -393,7 +380,7 @@ extern "C" { // We have waited now (or not, depending on timeout), // go through all fds and count how many have data - for (i = 0; i < nfds; ++i) { + for (unsigned int i = 0; i < nfds; ++i) { cdev::CDev *dev = get_vdev(fds[i].fd); @@ -421,11 +408,6 @@ extern "C" { return (count) ? count : ret; } - int px4_fsync(int fd) - { - return 0; - } - int px4_access(const char *pathname, int mode) { if (mode != F_OK) { @@ -437,51 +419,6 @@ extern "C" { return (dev != nullptr) ? 0 : -1; } - void px4_show_devices() - { - int i = 0; - PX4_INFO("PX4 Devices:"); - - pthread_mutex_lock(&devmutex); - - for (const auto &dev : devmap) { - if (strncmp(dev.first.c_str(), "/dev/", 5) == 0) { - PX4_INFO(" %s", dev.first.c_str()); - } - } - - pthread_mutex_unlock(&devmutex); - - PX4_INFO("DF Devices:"); - const char *dev_path; - unsigned int index = 0; - i = 0; - - do { - // Each look increments index and returns -1 if end reached - i = DevMgr::getNextDeviceName(index, &dev_path); - - if (i == 0) { - PX4_INFO(" %s", dev_path); - } - } while (i == 0); - } - - void px4_show_topics() - { - PX4_INFO("Devices:"); - - pthread_mutex_lock(&devmutex); - - for (const auto &dev : devmap) { - if (strncmp(dev.first.c_str(), "/obj/", 5) == 0) { - PX4_INFO(" %s", dev.first.c_str()); - } - } - - pthread_mutex_unlock(&devmutex); - } - void px4_show_files() { PX4_INFO("Files:"); @@ -489,10 +426,7 @@ extern "C" { pthread_mutex_lock(&devmutex); for (const auto &dev : devmap) { - if (strncmp(dev.first.c_str(), "/obj/", 5) != 0 && - strncmp(dev.first.c_str(), "/dev/", 5) != 0) { - PX4_INFO(" %s", dev.first.c_str()); - } + PX4_INFO_RAW(" %s\n", dev.first.c_str()); } pthread_mutex_unlock(&devmutex); diff --git a/src/lib/drivers/barometer/PX4Barometer.cpp b/src/lib/drivers/barometer/PX4Barometer.cpp index 7307ce3448..ac37f08602 100644 --- a/src/lib/drivers/barometer/PX4Barometer.cpp +++ b/src/lib/drivers/barometer/PX4Barometer.cpp @@ -74,8 +74,6 @@ PX4Barometer::update(hrt_abstime timestamp, float pressure) report.timestamp = timestamp; report.pressure = pressure; - poll_notify(POLLIN); - _sensor_baro_pub.update(); } diff --git a/src/lib/drivers/device/CDev.cpp b/src/lib/drivers/device/CDev.cpp index b97469ba73..7da14b7c70 100644 --- a/src/lib/drivers/device/CDev.cpp +++ b/src/lib/drivers/device/CDev.cpp @@ -53,10 +53,9 @@ CDev::CDev(const char *name, const char *devname) : { } -int -CDev::init() +int CDev::init() { - DEVICE_DEBUG("CDev::init"); + PX4_DEBUG("CDev::init"); // base class init first int ret = Device::init(); @@ -78,29 +77,12 @@ out: return ret; } -int -CDev::ioctl(file_t *filep, int cmd, unsigned long arg) +int CDev::ioctl(file_t *filep, int cmd, unsigned long arg) { - DEVICE_DEBUG("CDev::ioctl"); + PX4_DEBUG("CDev::ioctl"); int ret = -ENOTTY; switch (cmd) { - - /* fetch a pointer to the driver's private data */ - case DIOC_GETPRIV: - *(void **)(uintptr_t)arg = (void *)this; - ret = PX4_OK; - break; - - case DEVIOCSPUBBLOCK: - _pub_blocked = (arg != 0); - ret = PX4_OK; - break; - - case DEVIOCGPUBBLOCK: - ret = _pub_blocked; - break; - case DEVIOCGDEVICEID: ret = (int)_device_id.devid; break; diff --git a/src/lib/drivers/device/CDev.hpp b/src/lib/drivers/device/CDev.hpp index 6747054689..415afbd1e8 100644 --- a/src/lib/drivers/device/CDev.hpp +++ b/src/lib/drivers/device/CDev.hpp @@ -75,7 +75,7 @@ public: /** * Perform an ioctl operation on the device. * - * The default implementation handles DIOC_GETPRIV, and otherwise + * The default implementation handles DEVIOCGDEVICEID, and otherwise * returns -ENOTTY. Subclasses should call the default implementation * for any command they do not handle themselves. * diff --git a/src/lib/drivers/device/integrator.cpp b/src/lib/drivers/device/integrator.cpp index 4810d60938..8cd298d21b 100644 --- a/src/lib/drivers/device/integrator.cpp +++ b/src/lib/drivers/device/integrator.cpp @@ -114,50 +114,6 @@ Integrator::put(const hrt_abstime ×tamp, const matrix::Vector3f &val, matri } } -bool -Integrator::put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral, - uint32_t &integral_dt) -{ - if (_last_integration_time == 0) { - /* this is the first item in the integrator */ - hrt_abstime now = hrt_absolute_time(); - _last_integration_time = now; - _last_reset_time = now; - _last_val = val; - - return false; - } - - // Create the timestamp artifically. - const hrt_abstime timestamp = _last_integration_time + interval_us; - - return put(timestamp, val, integral, integral_dt); -} - -matrix::Vector3f -Integrator::get(bool reset, uint32_t &integral_dt) -{ - matrix::Vector3f val = _alpha; - - if (reset) { - _reset(integral_dt); - } - - return val; -} - -matrix::Vector3f -Integrator::get_and_filtered(bool reset, uint32_t &integral_dt, matrix::Vector3f &filtered_val) -{ - // Do the usual get with reset first but don't return yet. - const matrix::Vector3f ret_integral = get(reset, integral_dt); - - // Because we need both the integral and the integral_dt. - filtered_val = ret_integral * 1000000 / integral_dt; - - return ret_integral; -} - void Integrator::_reset(uint32_t &integral_dt) { diff --git a/src/lib/drivers/device/integrator.h b/src/lib/drivers/device/integrator.h index 517a74bad0..02586aa03d 100644 --- a/src/lib/drivers/device/integrator.h +++ b/src/lib/drivers/device/integrator.h @@ -51,12 +51,6 @@ public: Integrator(uint32_t auto_reset_interval = 4000 /* 250 Hz */, bool coning_compensation = false); ~Integrator() = default; - // no copy, assignment, move, move assignment - Integrator(const Integrator &) = delete; - Integrator &operator=(const Integrator &) = delete; - Integrator(Integrator &&) = delete; - Integrator &operator=(Integrator &&) = delete; - /** * Put an item into the integral. * @@ -69,42 +63,6 @@ public: */ bool put(const uint64_t ×tamp, const matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt); - /** - * Put an item into the integral but provide an interval instead of a timestamp. - * - * @param interval_us Interval in us since last integration. - * @param val Item to put. - * @param integral Current integral in case the integrator did reset, else the value will not be modified - * @param integral_dt Get the dt in us of the current integration (only if reset). Note that this - * values might not be accurate vs. hrt_absolute_time because it is just the sum of the - * supplied intervals. - * @return true if putting the item triggered an integral reset and the integral should be - * published. - */ - bool put_with_interval(unsigned interval_us, matrix::Vector3f &val, matrix::Vector3f &integral, uint32_t &integral_dt); - - /** - * Get the current integral and reset the integrator if needed. - * - * @param reset Reset the integral to zero. - * @param integral_dt Get the dt in us of the current integration (only if reset). - * @return the integral since the last read-reset - */ - matrix::Vector3f get(bool reset, uint32_t &integral_dt); - - - /** - * Get the current integral and reset the integrator if needed. Additionally give the - * integral over the samples differentiated by the integration time (mean filtered values). - * - * @param reset Reset the integral to zero. - * @param integral_dt Get the dt in us of the current integration (only if reset). - * @param filtered_val The integral differentiated by the integration time. - * @return the integral since the last read-reset - */ - matrix::Vector3f get_and_filtered(bool reset, uint32_t &integral_dt, matrix::Vector3f &filtered_val); - - /** * Set auto reset interval during runtime. This won't reset the integrator. * diff --git a/src/lib/drivers/device/nuttx/SPI.hpp b/src/lib/drivers/device/nuttx/SPI.hpp index 0bab23b312..b2f94faada 100644 --- a/src/lib/drivers/device/nuttx/SPI.hpp +++ b/src/lib/drivers/device/nuttx/SPI.hpp @@ -42,6 +42,8 @@ #include "../CDev.hpp" +#include + namespace device __EXPORT { diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index 8c697c4f44..2c8976ec63 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp @@ -131,7 +131,6 @@ PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z) report.y = val_calibrated(1); report.z = val_calibrated(2); - poll_notify(POLLIN); _sensor_mag_pub.update(); } diff --git a/src/lib/systemlib/CMakeLists.txt b/src/lib/systemlib/CMakeLists.txt index 4975dfcb12..43216fe499 100644 --- a/src/lib/systemlib/CMakeLists.txt +++ b/src/lib/systemlib/CMakeLists.txt @@ -33,20 +33,20 @@ set(SRCS conversions.c - cpuload.c crc.c mavlink_log.cpp - otp.c - ) +) if(${PX4_PLATFORM} STREQUAL "nuttx") list(APPEND SRCS + cpuload.c print_load_nuttx.c - ) + otp.c + ) else() list(APPEND SRCS print_load_posix.c - ) + ) endif() px4_add_library(systemlib ${SRCS}) diff --git a/src/lib/systemlib/cpuload.c b/src/lib/systemlib/cpuload.c index 24ad7f07f0..5247ae2978 100644 --- a/src/lib/systemlib/cpuload.c +++ b/src/lib/systemlib/cpuload.c @@ -51,7 +51,7 @@ #include -#include + #include #include "cpuload.h" diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 2dbbbf5043..d0717378b7 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -52,7 +52,6 @@ px4_add_module( state_machine_helper.cpp DEPENDS circuit_breaker - df_driver_framework failure_detector git_ecl ecl_geo diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 11260ae68c..579f458419 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -132,10 +132,8 @@ #include #include #include -#include #include #include -#include #include #include #include @@ -180,7 +178,7 @@ typedef struct { int do_accel_calibration(orb_advert_t *mavlink_log_pub) { -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage int fd; #endif @@ -200,7 +198,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); @@ -422,7 +420,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index); fd = px4_open(str, 0); @@ -525,7 +523,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub sensor_accel_s report = {}; orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report); -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL // and match it up with the one from the uORB subscription, because the diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 043e91cef5..8ecd118d6d 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 7c9824d87a..e86f9abd29 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -38,7 +38,6 @@ #include "baro_calibration.h" -#include #include #include #include diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index cafe5e84e8..429bdce152 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index bc1a6a91ce..2cf24cd155 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -57,35 +57,8 @@ using namespace time_literals; int do_esc_calibration(orb_advert_t *mavlink_log_pub, struct actuator_armed_s *armed) { int return_code = PX4_OK; - -#if defined(__PX4_POSIX_OCPOC) || defined(__PX4_POSIX_BBBLUE) - hrt_abstime timeout_start = 0; - hrt_abstime timeout_wait = 60_s; - armed->in_esc_calibration_mode = true; - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "begin esc"); - timeout_start = hrt_absolute_time(); - - while (true) { - if (hrt_absolute_time() - timeout_start > timeout_wait) { - break; - - } else { - px4_usleep(50000); - } - } - - armed->in_esc_calibration_mode = false; - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "end esc"); - - if (return_code == OK) { - calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc"); - } - - return return_code; - -#else int fd = -1; - bool batt_connected = true; // for safety resons assume battery is connected, will be cleared below if not the case + bool batt_connected = true; // for safety reasons assume battery is connected, will be cleared below if not the case hrt_abstime timeout_start = 0; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "esc"); @@ -207,5 +180,4 @@ Out: } return return_code; -#endif } diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 36c5e4b1c4..044ba15211 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -240,7 +239,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) // Reset all offsets to 0 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero)); -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); @@ -301,7 +300,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) sensor_gyro_s report{}; orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report); -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL // and match it up with the one from the uORB subscription, because the @@ -473,7 +472,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_GYRO%u_ID", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index]))); -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index); int fd = px4_open(str, 0); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9a91593939..1a45c10e19 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -153,7 +152,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) (void)append_to_existing_calibration; for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); @@ -210,7 +209,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) param_notify_changes(); -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // Attempt to open mag (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); int fd = px4_open(str, O_RDONLY); @@ -609,7 +608,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma sensor_mag_s report{}; orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL // and match it up with the one from the uORB subscription, because the @@ -804,7 +803,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma if (device_ids[cur_mag] != 0) { mag_calibration_s mscale; -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage int fd_mag = -1; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); @@ -819,7 +818,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma if (result == calibrate_return_ok) { -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // Read existing calibration if (px4_ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != PX4_OK) { @@ -852,7 +851,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma #endif } -#ifdef __PX4_NUTTX +#if 1 // TODO: replace all IOCTL usage // Mag device no longer needed if (fd_mag >= 0) { @@ -875,15 +874,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); - // FIXME: scaling is not used right now on QURT -#ifndef __PX4_QURT (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); -#endif if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); @@ -892,10 +888,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } else { calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); -#ifndef __PX4_QURT + calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); -#endif + px4_usleep(200000); } } diff --git a/src/modules/events/temperature_calibration/polyfit.hpp b/src/modules/events/temperature_calibration/polyfit.hpp index 2758556c54..44fbb7d931 100644 --- a/src/modules/events/temperature_calibration/polyfit.hpp +++ b/src/modules/events/temperature_calibration/polyfit.hpp @@ -100,7 +100,6 @@ Author: Siddharth Bharat Purohit #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 68d6ebae20..ec3ddf3a31 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -47,7 +47,9 @@ #include #include #include + #include +#include #ifdef CONFIG_NET #include diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5ba48f1cda..8d87810906 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -68,14 +68,11 @@ #include #include -#include - #include "parameters.h" #include "voted_sensors_update.h" #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" -using namespace DriverFramework; using namespace sensors; using namespace time_literals; @@ -128,7 +125,7 @@ private: DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - DevHandle _h_adc; /**< ADC driver handle */ + int _adc_fd {-1}; /**< ADC driver handle */ hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */ @@ -219,17 +216,13 @@ Sensors::adc_init() { if (!_hil_enabled) { #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + _adc_fd = px4_open(ADC0_DEVICE_PATH, O_RDONLY); - - - DevMgr::getHandle(ADC0_DEVICE_PATH, _h_adc); - - if (!_h_adc.isValid()) { - PX4_ERR("no ADC found: %s (%d)", ADC0_DEVICE_PATH, _h_adc.getError()); + if (_adc_fd == -1) { + PX4_ERR("no ADC found: %s", ADC0_DEVICE_PATH); return PX4_ERROR; } - #endif // ADC_AIRSPEED_VOLTAGE_CHANNEL } @@ -345,7 +338,7 @@ Sensors::adc_poll() /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ px4_adc_msg_t buf_adc[PX4_MAX_ADC_CHANNELS]; /* read all channels available */ - int ret = _h_adc.read(&buf_adc, sizeof(buf_adc)); + int ret = px4_read(_adc_fd, &buf_adc, sizeof(buf_adc)); if (ret >= (int)sizeof(buf_adc[0])) { diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 2c264bcc57..c3e98ba332 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -49,7 +49,6 @@ #define CAL_ERROR_APPLY_CAL_MSG "FAILED APPLYING %s CAL #%u" using namespace sensors; -using namespace DriverFramework; using namespace matrix; VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) @@ -202,26 +201,24 @@ void VotedSensorsUpdate::parametersUpdate() /* set offset parameters to new values */ bool failed = false; - char str[30] {}; - unsigned gyro_count = 0; - unsigned accel_count = 0; - unsigned gyro_cal_found_count = 0; - unsigned accel_cal_found_count = 0; /* run through all gyro sensors */ + unsigned gyro_count = 0; + unsigned gyro_cal_found_count = 0; + for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) { _gyro.enabled[driver_index] = true; + char str[30] {}; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index); - DevHandle h; - DevMgr::getHandle(str, h); + int fd = px4_open(str, O_RDWR); - if (!h.isValid()) { + if (fd < 0) { continue; } - uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); + uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0); bool config_ok = false; /* run through all stored calibrations that are applied at the driver level*/ @@ -231,11 +228,11 @@ void VotedSensorsUpdate::parametersUpdate() (void)sprintf(str, "CAL_GYRO%u_ID", i); int32_t device_id = 0; - failed = failed || (OK != param_get(param_find(str), &device_id)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_GYRO%u_EN", i); int32_t device_enabled = 1; - failed = failed || (OK != param_get(param_find(str), &device_enabled)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled)); if (failed) { continue; @@ -249,28 +246,27 @@ void VotedSensorsUpdate::parametersUpdate() if ((uint32_t)device_id == driver_device_id) { _gyro.enabled[driver_index] = (device_enabled == 1); - if (!_gyro.enabled[driver_index]) { _gyro.priority[driver_index] = 0; } + if (!_gyro.enabled[driver_index]) { + _gyro.priority[driver_index] = 0; + } - struct gyro_calibration_s gscale = {}; + gyro_calibration_s gscale{}; (void)sprintf(str, "CAL_GYRO%u_XOFF", i); - - failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &gscale.x_offset)); (void)sprintf(str, "CAL_GYRO%u_YOFF", i); - - failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &gscale.y_offset)); (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); - - failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &gscale.z_offset)); if (failed) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); } else { /* apply new scaling and offsets */ - config_ok = applyGyroCalibration(h, &gscale, device_id); + config_ok = (px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale) == 0); if (!config_ok) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i); @@ -284,35 +280,41 @@ void VotedSensorsUpdate::parametersUpdate() if (config_ok) { gyro_count++; } + + px4_close(fd); } - // There are less gyros than calibrations + // There are fewer gyros than calibrations // reset the board calibration and fail the initial load if (gyro_count < gyro_cal_found_count) { + PX4_ERR("fewer accels than calibrations, resetting all CAL_GYROx_ID"); // run through all stored calibrations and reset them for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) { - - int32_t device_id = 0; + char str[30] {}; (void)sprintf(str, "CAL_GYRO%u_ID", i); + int32_t device_id = 0; (void)param_set(param_find(str), &device_id); } } /* run through all accel sensors */ + unsigned accel_count = 0; + unsigned accel_cal_found_count = 0; + for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) { _accel.enabled[driver_index] = true; + char str[30] {}; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index); - DevHandle h; - DevMgr::getHandle(str, h); + int fd = px4_open(str, O_RDWR); - if (!h.isValid()) { + if (fd < 0) { continue; } - uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); + uint32_t driver_device_id = px4_ioctl(fd, DEVIOCGDEVICEID, 0); bool config_ok = false; /* run through all stored calibrations */ @@ -322,11 +324,11 @@ void VotedSensorsUpdate::parametersUpdate() (void)sprintf(str, "CAL_ACC%u_ID", i); int32_t device_id = 0; - failed = failed || (OK != param_get(param_find(str), &device_id)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_ACC%u_EN", i); int32_t device_enabled = 1; - failed = failed || (OK != param_get(param_find(str), &device_enabled)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled)); if (failed) { continue; @@ -340,40 +342,36 @@ void VotedSensorsUpdate::parametersUpdate() if ((uint32_t)device_id == driver_device_id) { _accel.enabled[driver_index] = (device_enabled == 1); - if (!_accel.enabled[driver_index]) { _accel.priority[driver_index] = 0; } + if (!_accel.enabled[driver_index]) { + _accel.priority[driver_index] = 0; + } - struct accel_calibration_s ascale = {}; + accel_calibration_s ascale{}; (void)sprintf(str, "CAL_ACC%u_XOFF", i); - - failed = failed || (OK != param_get(param_find(str), &ascale.x_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_offset)); (void)sprintf(str, "CAL_ACC%u_YOFF", i); - - failed = failed || (OK != param_get(param_find(str), &ascale.y_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_offset)); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); - - failed = failed || (OK != param_get(param_find(str), &ascale.z_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_offset)); (void)sprintf(str, "CAL_ACC%u_XSCALE", i); - - failed = failed || (OK != param_get(param_find(str), &ascale.x_scale)); + failed = failed || (PX4_OK != param_get(param_find(str), &ascale.x_scale)); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - - failed = failed || (OK != param_get(param_find(str), &ascale.y_scale)); + failed = failed || (PX4_OK != param_get(param_find(str), &ascale.y_scale)); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - - failed = failed || (OK != param_get(param_find(str), &ascale.z_scale)); + failed = failed || (PX4_OK != param_get(param_find(str), &ascale.z_scale)); if (failed) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ - config_ok = applyAccelCalibration(h, &ascale, device_id); + config_ok = (px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale) == 0); if (!config_ok) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "accel ", i); @@ -387,17 +385,20 @@ void VotedSensorsUpdate::parametersUpdate() if (config_ok) { accel_count++; } + + px4_close(fd); } - // There are less accels than calibrations + // There are fewer accels than calibrations // reset the board calibration and fail the initial load if (accel_count < accel_cal_found_count) { + PX4_ERR("fewer accels than calibrations, resetting all CAL_ACCx_ID"); // run through all stored calibrations and reset them for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) { - - int32_t device_id = 0; + char str[30] {}; (void)sprintf(str, "CAL_ACC%u_ID", i); + int32_t device_id = 0; (void)param_set(param_find(str), &device_id); } } @@ -417,37 +418,38 @@ void VotedSensorsUpdate::parametersUpdate() for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX && topic_instance < _mag.subscription_count; ++topic_instance) { - sensor_mag_s report; + sensor_mag_s report{}; if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != 0) { continue; } - int topic_device_id = report.device_id; + uint32_t topic_device_id = report.device_id; bool is_external = report.is_external; _mag_device_id[topic_instance] = topic_device_id; // find the driver handle that matches the topic_device_id - DevHandle h; + int fd = -1; + char str[30] {}; for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; ++driver_index) { (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index); - DevMgr::getHandle(str, h); + fd = px4_open(str, O_RDWR); - if (!h.isValid()) { + if (fd < 0) { /* the driver is not running, continue with the next */ continue; } - int driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); + uint32_t driver_device_id = (uint32_t)px4_ioctl(fd, DEVIOCGDEVICEID, 0); if (driver_device_id == topic_device_id) { break; // we found the matching driver } else { - DevMgr::releaseHandle(h); + px4_close(fd); } } @@ -460,11 +462,11 @@ void VotedSensorsUpdate::parametersUpdate() (void)sprintf(str, "CAL_MAG%u_ID", i); int32_t device_id = 0; - failed = failed || (OK != param_get(param_find(str), &device_id)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_MAG%u_EN", i); int32_t device_enabled = 1; - failed = failed || (OK != param_get(param_find(str), &device_enabled)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled)); if (failed) { continue; @@ -476,38 +478,32 @@ void VotedSensorsUpdate::parametersUpdate() // the mags that were published after the initial parameterUpdate // would be given the priority even if disabled. Reset it to 0 in this case - if (!_mag.enabled[topic_instance]) { _mag.priority[topic_instance] = 0; } + if (!_mag.enabled[topic_instance]) { + _mag.priority[topic_instance] = 0; + } - struct mag_calibration_s mscale = {}; + mag_calibration_s mscale{}; (void)sprintf(str, "CAL_MAG%u_XOFF", i); - - failed = failed || (OK != param_get(param_find(str), &mscale.x_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_offset)); (void)sprintf(str, "CAL_MAG%u_YOFF", i); - - failed = failed || (OK != param_get(param_find(str), &mscale.y_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_offset)); (void)sprintf(str, "CAL_MAG%u_ZOFF", i); - - failed = failed || (OK != param_get(param_find(str), &mscale.z_offset)); + failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_offset)); (void)sprintf(str, "CAL_MAG%u_XSCALE", i); - - failed = failed || (OK != param_get(param_find(str), &mscale.x_scale)); + failed = failed || (PX4_OK != param_get(param_find(str), &mscale.x_scale)); (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - - failed = failed || (OK != param_get(param_find(str), &mscale.y_scale)); + failed = failed || (PX4_OK != param_get(param_find(str), &mscale.y_scale)); (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - - failed = failed || (OK != param_get(param_find(str), &mscale.z_scale)); + failed = failed || (PX4_OK != param_get(param_find(str), &mscale.z_scale)); (void)sprintf(str, "CAL_MAG%u_ROT", i); - - int32_t mag_rot; - + int32_t mag_rot = 0; param_get(param_find(str), &mag_rot); if (is_external) { @@ -543,7 +539,7 @@ void VotedSensorsUpdate::parametersUpdate() } else { /* apply new scaling and offsets */ - config_ok = applyMagCalibration(h, &mscale, device_id); + config_ok = (px4_ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale) == 0); if (!config_ok) { PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag ", i); @@ -553,6 +549,8 @@ void VotedSensorsUpdate::parametersUpdate() break; } } + + px4_close(fd); } } @@ -848,7 +846,7 @@ void VotedSensorsUpdate::baroPoll(vehicle_air_data_s &airdata) orb_check(_baro.subscription[uorb_index], &baro_updated); if (baro_updated) { - sensor_baro_s baro_report; + sensor_baro_s baro_report{}; int ret = orb_copy(ORB_ID(sensor_baro), _baro.subscription[uorb_index], &baro_report); @@ -1050,52 +1048,6 @@ void VotedSensorsUpdate::printStatus() _temperature_compensation.print_status(); } -bool -VotedSensorsUpdate::applyGyroCalibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id) -{ -#if defined(__PX4_NUTTX) - - /* On most systems, we can just use the IOCTL call to set the calibration params. */ - return !h.ioctl(GYROIOCSSCALE, (long unsigned int)gcal); - -#else - /* On QURT, the params are read directly in the respective wrappers. */ - return true; -#endif -} - -bool -VotedSensorsUpdate::applyAccelCalibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id) -{ -#if defined(__PX4_NUTTX) - - /* On most systems, we can just use the IOCTL call to set the calibration params. */ - return !h.ioctl(ACCELIOCSSCALE, (long unsigned int)acal); - -#else - /* On QURT, the params are read directly in the respective wrappers. */ - return true; -#endif -} - -bool -VotedSensorsUpdate::applyMagCalibration(DevHandle &h, const struct mag_calibration_s *mcal, const int device_id) -{ -#if defined(__PX4_NUTTX) - - if (!h.isValid()) { - return false; - } - - /* On most systems, we can just use the IOCTL call to set the calibration params. */ - return !h.ioctl(MAGIOCSSCALE, (long unsigned int)mcal); - -#else - /* On QURT & POSIX, the params are read directly in the respective wrappers. */ - return true; -#endif -} - void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw, vehicle_air_data_s &airdata, vehicle_magnetometer_s &magnetometer) { diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 630c32b53e..5e4a0994ed 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -62,8 +62,6 @@ #include #include -#include - #include "temperature_compensation.h" #include "common.h" @@ -211,37 +209,6 @@ private: */ bool checkFailover(SensorData &sensor, const char *sensor_name, const uint64_t type); - /** - * Apply a gyro calibration. - * - * @param h: reference to the DevHandle in use - * @param gscale: the calibration data. - * @param device: the device id of the sensor. - * @return: true if config is ok - */ - bool applyGyroCalibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id); - - /** - * Apply a accel calibration. - * - * @param h: reference to the DevHandle in use - * @param ascale: the calibration data. - * @param device: the device id of the sensor. - * @return: true if config is ok - */ - bool applyAccelCalibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, - const int device_id); - - /** - * Apply a mag calibration. - * - * @param h: reference to the DevHandle in use - * @param gscale: the calibration data. - * @param device: the device id of the sensor. - * @return: true if config is ok - */ - bool applyMagCalibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); - SensorData _accel {}; SensorData _gyro {}; SensorData _mag {}; diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index a80904df7b..4e5fcfd40e 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -64,6 +64,21 @@ void Simulator::parameters_update(bool force) } } +void Simulator::print_status() +{ + PX4_INFO("accelerometer"); + _px4_accel.print_status(); + + PX4_INFO("gyroscope"); + _px4_gyro.print_status(); + + PX4_INFO("magnetometer"); + _px4_mag.print_status(); + + PX4_INFO("barometer"); + _px4_baro.print_status(); +} + int Simulator::start(int argc, char *argv[]) { _instance = new Simulator(); @@ -79,9 +94,8 @@ int Simulator::start(int argc, char *argv[]) _instance->set_port(atoi(argv[3])); } -#ifndef __PX4_QURT _instance->run(); -#endif + return 0; } else { @@ -92,7 +106,7 @@ int Simulator::start(int argc, char *argv[]) static void usage() { - PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}"); + PX4_INFO("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop|status}"); PX4_INFO("Start simulator: simulator start"); PX4_INFO("Connect using UDP: simulator start -u udp_port"); PX4_INFO("Connect using TCP: simulator start -c tcp_port"); @@ -119,7 +133,7 @@ int simulator_main(int argc, char *argv[]) Simulator::start, argv); -#if !defined(__PX4_QURT) && defined(ENABLE_LOCKSTEP_SCHEDULER) +#if defined(ENABLE_LOCKSTEP_SCHEDULER) // We want to prevent the rest of the startup script from running until time // is initialized by the HIL_SENSOR messages from the simulator. @@ -143,6 +157,15 @@ int simulator_main(int argc, char *argv[]) g_sim_task = -1; } + } else if (argc == 2 && strcmp(argv[1], "status") == 0) { + if (g_sim_task < 0) { + PX4_WARN("Simulator not running"); + return 1; + + } else { + Simulator::getInstance()->print_status(); + } + } else { usage(); return 1; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index d65e918656..361f86d199 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -95,9 +95,11 @@ public: void set_port(unsigned port) { _port = port; } #if defined(ENABLE_LOCKSTEP_SCHEDULER) - bool has_initialized() {return _has_initialized.load(); } + bool has_initialized() { return _has_initialized.load(); } #endif + void print_status(); + private: Simulator() : ModuleParams(nullptr) { @@ -172,8 +174,6 @@ private: } } _battery; -#ifndef __PX4_QURT - void run(); void handle_message(const mavlink_message_t *msg); void handle_message_distance_sensor(const mavlink_message_t *msg); @@ -248,6 +248,4 @@ private: (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id ) - -#endif }; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 8b383c29a5..849c2a0fc2 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -33,22 +33,25 @@ * ****************************************************************************/ -#include +#include "simulator.h" +#include + #include #include #include -#include "simulator.h" -#include -#include "errno.h" #include #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include +#include +#include +#include + #include #ifdef ENABLE_UART_RC_INPUT @@ -957,7 +960,7 @@ int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink _flow_pub.publish(flow); - return OK; + return PX4_OK; } int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) @@ -1062,7 +1065,7 @@ int Simulator::publish_odometry_topic(const mavlink_message_t *odom_mavlink) /** @note: frame_id == MAV_FRAME_VISION_NED) */ _visual_odometry_pub.publish(odom); - return OK; + return PX4_OK; } int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavlink) @@ -1087,5 +1090,5 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl _dist_pub.publish(dist); - return OK; + return PX4_OK; } diff --git a/src/modules/uORB/uORBDeviceMaster.cpp b/src/modules/uORB/uORBDeviceMaster.cpp index 48b216ac82..b118ed6e95 100644 --- a/src/modules/uORB/uORBDeviceMaster.cpp +++ b/src/modules/uORB/uORBDeviceMaster.cpp @@ -43,6 +43,10 @@ #include #include +#ifndef __PX4_QURT // QuRT has no poll() +#include +#endif // PX4_QURT + uORB::DeviceMaster::DeviceMaster() { px4_sem_init(&_lock, 0, 1); @@ -330,7 +334,7 @@ void uORB::DeviceMaster::showTop(char **topic_filter, int num_filters) PX4_ERR("addNewDeviceNodes failed (%i)", ret); } -#ifdef __PX4_QURT //QuRT has no poll() +#ifdef __PX4_QURT // QuRT has no poll() only_once = true; #else const int stdin_fileno = 0; diff --git a/src/modules/uORB/uORBDeviceNode.cpp b/src/modules/uORB/uORBDeviceNode.cpp index 7aee1b6984..b974c9f98f 100644 --- a/src/modules/uORB/uORBDeviceNode.cpp +++ b/src/modules/uORB/uORBDeviceNode.cpp @@ -480,7 +480,7 @@ int16_t uORB::DeviceNode::topic_unadvertised(const orb_metadata *meta, int prior */ #endif /* ORB_COMMUNICATOR */ -pollevent_t +px4_pollevent_t uORB::DeviceNode::poll_state(cdev::file_t *filp) { SubscriberData *sd = filp_to_sd(filp); @@ -496,7 +496,7 @@ uORB::DeviceNode::poll_state(cdev::file_t *filp) } void -uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) +uORB::DeviceNode::poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) { SubscriberData *sd = filp_to_sd((cdev::file_t *)fds->priv); diff --git a/src/modules/uORB/uORBDeviceNode.hpp b/src/modules/uORB/uORBDeviceNode.hpp index 1a1d0c9b99..e0efaa7ea3 100644 --- a/src/modules/uORB/uORBDeviceNode.hpp +++ b/src/modules/uORB/uORBDeviceNode.hpp @@ -235,9 +235,9 @@ public: protected: - pollevent_t poll_state(cdev::file_t *filp) override; + px4_pollevent_t poll_state(cdev::file_t *filp) override; - void poll_notify_one(px4_pollfd_struct_t *fds, pollevent_t events) override; + void poll_notify_one(px4_pollfd_struct_t *fds, px4_pollevent_t events) override; private: diff --git a/src/modules/uORB/uORBMain.cpp b/src/modules/uORB/uORBMain.cpp index 8039e0a390..62c0da24e0 100644 --- a/src/modules/uORB/uORBMain.cpp +++ b/src/modules/uORB/uORBMain.cpp @@ -113,7 +113,7 @@ uorb_main(int argc, char *argv[]) return -errno; } -#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) +#if !defined(__PX4_QURT) /* FIXME: this fails on Snapdragon (see https://github.com/PX4/Firmware/issues/5406), * so we disable logging messages to the ulog for now. This needs further investigations. */ diff --git a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp index b96d5a3ed5..5883899728 100644 --- a/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp +++ b/src/modules/uORB/uORB_tests/uORBTest_UnitTest.cpp @@ -37,7 +37,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp index df078f3f72..37abf6758b 100644 --- a/src/modules/uORB/uORB_tests/uORB_tests_main.cpp +++ b/src/modules/uORB/uORB_tests/uORB_tests_main.cpp @@ -36,9 +36,7 @@ #include "../uORB.h" #include "../uORBCommon.hpp" -#ifndef __PX4_QURT #include "uORBTest_UnitTest.hpp" -#endif extern "C" { __EXPORT int uorb_tests_main(int argc, char *argv[]); } @@ -50,9 +48,6 @@ static void usage() int uorb_tests_main(int argc, char *argv[]) { - -#ifndef __PX4_QURT - /* * Test the driver/device. */ @@ -88,8 +83,6 @@ uorb_tests_main(int argc, char *argv[]) } } -#endif - usage(); return -EINVAL; } diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index b80d16db16..262f6b0dbc 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -65,26 +65,20 @@ __EXPORT int config_main(int argc, char *argv[]); static int do_gyro(int argc, char *argv[]); static int do_accel(int argc, char *argv[]); static int do_mag(int argc, char *argv[]); -static int do_device(int argc, char *argv[]); static void print_usage(void); int config_main(int argc, char *argv[]) { - bool is_device_cmd = argc >= 2 && (!strcmp(argv[1], "block") || !strcmp(argv[1], "unblock")); - if (argc >= 3) { - if (!is_device_cmd && !strncmp(argv[2], "/dev/gyro", 9)) { + if (!strncmp(argv[2], "/dev/gyro", 9)) { return do_gyro(argc - 1, argv + 1); - } else if (!is_device_cmd && !strncmp(argv[2], "/dev/accel", 10)) { + } else if (!strncmp(argv[2], "/dev/accel", 10)) { return do_accel(argc - 1, argv + 1); - } else if (!is_device_cmd && !strncmp(argv[2], "/dev/mag", 8)) { + } else if (!strncmp(argv[2], "/dev/mag", 8)) { return do_mag(argc - 1, argv + 1); - - } else { - return do_device(argc - 1, argv + 1); } } @@ -114,55 +108,6 @@ print_usage(void) PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); } -static int -do_device(int argc, char *argv[]) -{ - if (argc < 2) { - print_usage(); - return 1; - } - - int fd; - - fd = open(argv[1], 0); - - if (fd < 0) { - PX4_ERR("open %s failed (%i)", argv[1], errno); - return 1; - - } else { - - int ret; - - if (argc == 2 && !strcmp(argv[0], "block")) { - - /* disable the device publications */ - ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); - - if (ret) { - PX4_ERR("uORB publications could not be blocked"); - return 1; - } - - } else if (argc == 2 && !strcmp(argv[0], "unblock")) { - - /* enable the device publications */ - ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); - - if (ret) { - PX4_ERR("uORB publications could not be unblocked"); - return 1; - } - - } else { - print_usage(); - return 1; - } - } - - return 0; -} - static int do_gyro(int argc, char *argv[]) { diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c index 2890e64ce3..f5445f4160 100644 --- a/src/systemcmds/esc_calib/esc_calib.c +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -53,7 +53,7 @@ #include #include -#include + #include "drivers/drv_pwm_output.h" diff --git a/src/systemcmds/motor_ramp/motor_ramp.cpp b/src/systemcmds/motor_ramp/motor_ramp.cpp index 7f2b9b5333..9062ac9214 100644 --- a/src/systemcmds/motor_ramp/motor_ramp.cpp +++ b/src/systemcmds/motor_ramp/motor_ramp.cpp @@ -52,7 +52,7 @@ #include #include -#include + #include #include diff --git a/src/systemcmds/param/param.cpp b/src/systemcmds/param/param.cpp index 32285f3203..a5f90562db 100644 --- a/src/systemcmds/param/param.cpp +++ b/src/systemcmds/param/param.cpp @@ -56,8 +56,6 @@ #include #include -#include - #include #include "systemlib/err.h" diff --git a/src/systemcmds/pwm/pwm.cpp b/src/systemcmds/pwm/pwm.cpp index bb039a4fdd..6ea75290ff 100644 --- a/src/systemcmds/pwm/pwm.cpp +++ b/src/systemcmds/pwm/pwm.cpp @@ -60,8 +60,6 @@ #include #endif -#include - #include "systemlib/err.h" #include #include "drivers/drv_pwm_output.h" diff --git a/src/systemcmds/shutdown/shutdown.c b/src/systemcmds/shutdown/shutdown.c index 8230a86e2e..e6f6c10de0 100644 --- a/src/systemcmds/shutdown/shutdown.c +++ b/src/systemcmds/shutdown/shutdown.c @@ -37,8 +37,6 @@ */ #include -//#include - __EXPORT int shutdown_main(int argc, char *argv[]); diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c index 9cea47aedd..fbc29f73ec 100644 --- a/src/systemcmds/tests/test_dataman.c +++ b/src/systemcmds/tests/test_dataman.c @@ -47,8 +47,6 @@ #include #include -#include - #include #include #include diff --git a/src/systemcmds/tests/test_led.c b/src/systemcmds/tests/test_led.c index 71110a915e..0f26c5bd07 100644 --- a/src/systemcmds/tests/test_led.c +++ b/src/systemcmds/tests/test_led.c @@ -48,8 +48,6 @@ #include #include -#include - #include #include "tests_main.h" diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index fb7c7b2db3..0aeff563e7 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -47,7 +47,6 @@ #include #include -#include #include #include #include diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 402c11ed3a..7b0692394a 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -47,7 +47,6 @@ #include #include -#include #include #include #include diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index 9bfc457d56..47e949eefc 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -48,7 +48,6 @@ #include #include -#include #include #include diff --git a/src/systemcmds/tests/test_sleep.c b/src/systemcmds/tests/test_sleep.c index 42c3476575..422fac7a48 100644 --- a/src/systemcmds/tests/test_sleep.c +++ b/src/systemcmds/tests/test_sleep.c @@ -48,8 +48,6 @@ #include #include -#include - #include "tests_main.h" int test_sleep(int argc, char *argv[]) diff --git a/src/systemcmds/tests/test_time.c b/src/systemcmds/tests/test_time.c index 1f0989a24c..a299bda0d6 100644 --- a/src/systemcmds/tests/test_time.c +++ b/src/systemcmds/tests/test_time.c @@ -47,8 +47,6 @@ #include #include -#include - #include "tests_main.h" #include diff --git a/src/systemcmds/tests/test_uart_baudchange.c b/src/systemcmds/tests/test_uart_baudchange.c index 8fb6eea1a0..90a086d125 100644 --- a/src/systemcmds/tests/test_uart_baudchange.c +++ b/src/systemcmds/tests/test_uart_baudchange.c @@ -49,8 +49,6 @@ #include #include -#include - #include "tests_main.h" #include diff --git a/src/systemcmds/tests/test_uart_break.c b/src/systemcmds/tests/test_uart_break.c index 7c955c52b1..86d2ad2ffb 100644 --- a/src/systemcmds/tests/test_uart_break.c +++ b/src/systemcmds/tests/test_uart_break.c @@ -51,8 +51,6 @@ #include #include -#include - #include "tests_main.h" #include diff --git a/src/systemcmds/tests/test_uart_console.c b/src/systemcmds/tests/test_uart_console.c index 5d4cedbc3f..f44417102d 100644 --- a/src/systemcmds/tests/test_uart_console.c +++ b/src/systemcmds/tests/test_uart_console.c @@ -50,8 +50,6 @@ #include #include -#include - #include "tests_main.h" #include diff --git a/src/systemcmds/tests/test_uart_loopback.c b/src/systemcmds/tests/test_uart_loopback.c index 73cb6c58f0..78100d67a6 100644 --- a/src/systemcmds/tests/test_uart_loopback.c +++ b/src/systemcmds/tests/test_uart_loopback.c @@ -49,8 +49,6 @@ #include #include -#include - #include "tests_main.h" #include diff --git a/src/systemcmds/tests/test_uart_send.c b/src/systemcmds/tests/test_uart_send.c index c860c70a91..ae37aa1e9b 100644 --- a/src/systemcmds/tests/test_uart_send.c +++ b/src/systemcmds/tests/test_uart_send.c @@ -48,8 +48,6 @@ #include #include -#include - #include "tests_main.h" #include diff --git a/src/systemcmds/usb_connected/usb_connected.c b/src/systemcmds/usb_connected/usb_connected.c index fd51a8cc9b..76411d743a 100644 --- a/src/systemcmds/usb_connected/usb_connected.c +++ b/src/systemcmds/usb_connected/usb_connected.c @@ -42,7 +42,7 @@ #include #include #include -#include + #include __EXPORT int usb_connected_main(int argc, char *argv[]);