From ce1e9762b00c63e14b40c458090aa24ece4b306c Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 5 Jan 2020 13:48:01 -0500 Subject: [PATCH] deprecate df_hmc5883_wrapper and replace with in tree hmc5883 - deprecate DriverFramework hmc5883 driver (df_hmc5883_wrapper) - update aerotenna ocpoc and snapdragon flight eagle boards to use in tree hmc5883 --- Tools/serial/generate_config.py | 5 + boards/aerotenna/ocpoc/default.cmake | 9 +- boards/aerotenna/ocpoc/src/board_config.h | 28 +- boards/atlflight/eagle/qurt-default.cmake | 1 - boards/atlflight/eagle/src/board_config.h | 6 +- boards/atlflight/excelsior/qurt-default.cmake | 1 - boards/emlid/navio2/default.cmake | 1 - boards/emlid/navio2/src/board_config.h | 21 +- boards/px4/raspberrypi/default.cmake | 2 +- boards/px4/raspberrypi/src/board_config.h | 13 +- posix-configs/eagle/flight/px4.config | 2 +- posix-configs/eagle/init/rcS | 2 +- posix-configs/ocpoc/px4.config | 2 +- posix-configs/rpi/px4.config | 1 - posix-configs/rpi/px4_test.config | 1 - .../df_hmc5883_wrapper/CMakeLists.txt | 45 -- .../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 462 ------------------ 17 files changed, 60 insertions(+), 542 deletions(-) delete mode 100644 src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt delete mode 100644 src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp diff --git a/Tools/serial/generate_config.py b/Tools/serial/generate_config.py index 2b47d8247b..5593736c4e 100755 --- a/Tools/serial/generate_config.py +++ b/Tools/serial/generate_config.py @@ -107,6 +107,11 @@ serial_ports = { "index": 202, "default_baudrate": 0, }, + "GPS3": { + "label": "GPS 3", + "index": 203, + "default_baudrate": 0, + }, # RC Port "RC": { diff --git a/boards/aerotenna/ocpoc/default.cmake b/boards/aerotenna/ocpoc/default.cmake index 180d1d750f..e2485368f9 100644 --- a/boards/aerotenna/ocpoc/default.cmake +++ b/boards/aerotenna/ocpoc/default.cmake @@ -7,6 +7,14 @@ px4_add_board( ARCHITECTURE cortex-a9 TOOLCHAIN arm-linux-gnueabihf TESTING + SERIAL_PORTS + GPS1:/dev/ttyS3 # GPS/Compass #1 (OcPoC Port 6) + GPS2:/dev/ttyS7 # GPS/Compass #2 (OcPoC Port 7) + GPS3:/dev/ttyS1 # GPS/Compass #3 (OcPoC Port 9) + TEL1:/dev/ttyPS1 # Radio Telemetry (OcPoC Port 4) + TEL2:/dev/ttyS6 # uLanding Radar Altimeter (OcPoC Port 8) + TEL3:/dev/ttyS2 # (OcPoC Port 2) + TEL4:/dev/ttyS0 # uSharp-Patch (OcPoC Port 5) DRIVERS #barometer # all available barometer drivers barometer/ms5611 @@ -26,7 +34,6 @@ px4_add_board( pwm_out_sim #telemetry # all available telemetry drivers DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers - hmc5883 mpu9250 MODULES airspeed_selector diff --git a/boards/aerotenna/ocpoc/src/board_config.h b/boards/aerotenna/ocpoc/src/board_config.h index ca07365f2a..a01dee7db1 100644 --- a/boards/aerotenna/ocpoc/src/board_config.h +++ b/boards/aerotenna/ocpoc/src/board_config.h @@ -34,7 +34,7 @@ /** * @file board_config.h * - * OCPOC internal definitions + * Aerotenna Ocpoc internal definitions */ #pragma once @@ -43,22 +43,36 @@ #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_OCPOC #define BOARD_BATTERY1_V_DIV (10.177939394f) + #define BOARD_HAS_NO_RESET #define BOARD_HAS_NO_BOOTLOADER -#define BOARD_NUMBER_I2C_BUSES 4 #define BOARD_MAX_LEDS 1 // Number of external LED's this board has + +/* + * I2C busses + */ +#define PX4_I2C_BUS_EXPANSION 2 // i2c-2: Air Data Probe or I2C Splitter +#define PX4_I2C_BUS_EXPANSION1 4 // i2c-4: GPS/Compass #1 +#define PX4_I2C_BUS_EXPANSION2 5 // i2c-5: GPS/Compass #2 +#define PX4_I2C_BUS_EXPANSION3 3 // i2c-3: GPS/Compass #3 + +#define PX4_NUMBER_I2C_BUSES 4 + #define PX4_I2C_BUS_LED 1 -#define PX4_I2C_BUS_EXPANSION 1 + +// SPI +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) // spidev1.0 - mpu9250 +#define PX4_SPIDEV_BARO PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) // spidev1.1 - ms5611 +//#define PX4_SPIDEV_MPU2 PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) // TODO: where is the 2nd mpu9250? + +#define PX4_SPI_BUS_BARO PX4_SPI_BUS_SENSORS // Battery ADC channels #define ADC_BATTERY_VOLTAGE_CHANNEL 10 #define ADC_BATTERY_CURRENT_CHANNEL ((uint8_t)(-1)) #define ADC_AIRSPEED_VOLTAGE_CHANNEL 11 -// SPI -#define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) - #include #include diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake index 2b90fa90dc..7b4c297f17 100644 --- a/boards/atlflight/eagle/qurt-default.cmake +++ b/boards/atlflight/eagle/qurt-default.cmake @@ -52,7 +52,6 @@ px4_add_board( snapdragon_pwm_out spektrum_rc DF_DRIVERS - hmc5883 isl29501 ltc2946 mpu9250 diff --git a/boards/atlflight/eagle/src/board_config.h b/boards/atlflight/eagle/src/board_config.h index 88d2c5dda7..6be1185312 100644 --- a/boards/atlflight/eagle/src/board_config.h +++ b/boards/atlflight/eagle/src/board_config.h @@ -57,8 +57,10 @@ #define PX4_I2C_OBDEV_BMP280 0x76 // SPI -#define PX4_SPI_BUS_SENSORS 1 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) +#define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) // spi-1 - mpu9250 + +// Battery ADC channels #include #include diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake index d827c82691..47e5030afc 100644 --- a/boards/atlflight/excelsior/qurt-default.cmake +++ b/boards/atlflight/excelsior/qurt-default.cmake @@ -52,7 +52,6 @@ px4_add_board( snapdragon_pwm_out spektrum_rc DF_DRIVERS - hmc5883 isl29501 ltc2946 mpu9250 diff --git a/boards/emlid/navio2/default.cmake b/boards/emlid/navio2/default.cmake index bb444850a6..a5b5efd08a 100644 --- a/boards/emlid/navio2/default.cmake +++ b/boards/emlid/navio2/default.cmake @@ -25,7 +25,6 @@ px4_add_board( pwm_out_sim #telemetry # all available telemetry drivers DF_DRIVERS # NOTE: DriverFramework is migrating to intree PX4 drivers - hmc5883 isl29501 lsm9ds1 mpu9250 diff --git a/boards/emlid/navio2/src/board_config.h b/boards/emlid/navio2/src/board_config.h index 29e825fdb2..17b49b1aba 100644 --- a/boards/emlid/navio2/src/board_config.h +++ b/boards/emlid/navio2/src/board_config.h @@ -34,7 +34,7 @@ /** * @file board_config.h * - * Emlid Navio2 RPI internal definitions + * Emlid Navio2 internal definitions */ #pragma once @@ -42,9 +42,6 @@ #define BOARD_OVERRIDE_UUID "RPIID00000000000" // must be of length 16 #define PX4_SOC_ARCH_ID PX4_SOC_ARCH_ID_RPI -#define ADC_BATTERY_VOLTAGE_CHANNEL 2 -#define ADC_BATTERY_CURRENT_CHANNEL 3 - #define BOARD_BATTERY1_V_DIV (10.177939394f) #define BOARD_BATTERY1_A_PER_V (15.391030303f) @@ -56,12 +53,20 @@ /* * I2C busses */ -#define PX4_I2C_BUS_EXPANSION 1 -#define PX4_NUMBER_I2C_BUSES 1 +#define PX4_I2C_BUS_EXPANSION 1 + +#define PX4_NUMBER_I2C_BUSES 1 // SPI -#define PX4_SPI_BUS_SENSORS 0 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) +#define PX4_SPI_BUS_SENSORS 0 +#define PX4_SPIDEV_UBLOX PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) // spidev0.0 - ublox m8n +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 1) // spidev0.1 - mpu9250 +#define PX4_SPIDEV_LSM9DS1_M PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 2) // spidev0.2 - lsm9ds1 mag +#define PX4_SPIDEV_LSM9DS1_AG PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 3) // spidev0.3 - lsm9ds1 accel/gyro + +// Battery ADC channels +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 #include #include diff --git a/boards/px4/raspberrypi/default.cmake b/boards/px4/raspberrypi/default.cmake index 2fabad960f..d6c55ec15a 100644 --- a/boards/px4/raspberrypi/default.cmake +++ b/boards/px4/raspberrypi/default.cmake @@ -28,7 +28,7 @@ px4_add_board( MODULES airspeed_selector attitude_estimator_q - battery_status + #battery_status camera_feedback commander dataman diff --git a/boards/px4/raspberrypi/src/board_config.h b/boards/px4/raspberrypi/src/board_config.h index c8f3e257b1..de84c6fe11 100644 --- a/boards/px4/raspberrypi/src/board_config.h +++ b/boards/px4/raspberrypi/src/board_config.h @@ -48,17 +48,14 @@ /* * I2C busses */ -#define PX4_I2C_BUS_ONBOARD 1 -#define PX4_I2C_BUS_EXPANSION 0 +#define PX4_I2C_BUS_ONBOARD 1 +#define PX4_I2C_BUS_EXPANSION 0 -#define PX4_NUMBER_I2C_BUSES 2 +#define PX4_NUMBER_I2C_BUSES 2 // SPI -#define PX4_SPI_BUS_SENSORS 0 -#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) - -#define ADC_BATTERY_VOLTAGE_CHANNEL 0 -#define ADC_BATTERY_CURRENT_CHANNEL 1 +#define PX4_SPI_BUS_SENSORS 0 +#define PX4_SPIDEV_MPU PX4_MK_SPI_SEL(PX4_SPI_BUS_SENSORS, 0) // spidev0.0 #include #include diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 2864d0aa05..6ab46467c8 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -5,7 +5,7 @@ sleep 1 gps start -d /dev/tty-4 param set MAV_TYPE 2 sleep 1 -df_hmc5883_wrapper start +hmc5883 start df_mpu9250_wrapper start_without_mag bmp280 start df_trone_wrapper start diff --git a/posix-configs/eagle/init/rcS b/posix-configs/eagle/init/rcS index d48b375796..2e99309d78 100644 --- a/posix-configs/eagle/init/rcS +++ b/posix-configs/eagle/init/rcS @@ -19,7 +19,7 @@ then qshell pwm_out_rc_in start -d /dev/tty-2 qshell gps start -d /dev/tty-4 - qshell df_hmc5883_wrapper start + qshell hmc5883 start qshell df_trone_wrapper start #qshell df_isl29501_wrapper start diff --git a/posix-configs/ocpoc/px4.config b/posix-configs/ocpoc/px4.config index e4d296a2e0..27affaa6f3 100644 --- a/posix-configs/ocpoc/px4.config +++ b/posix-configs/ocpoc/px4.config @@ -16,7 +16,7 @@ param set MAV_TYPE 2 param set MAV_SYS_ID 1 df_mpu9250_wrapper start_without_mag -R 10 -df_hmc5883_wrapper start -D /dev/i2c-4 +hmc5883 start ms5611 start rgbled start -b 1 ocpoc_adc start diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 97feab349c..1ea16a82e6 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -19,7 +19,6 @@ param set BAT_A_PER_V 15.391030303 dataman start df_lsm9ds1_wrapper start -R 4 #df_mpu9250_wrapper start -R 10 -#df_hmc5883_wrapper start ms5611 start navio_rgbled start navio_adc start diff --git a/posix-configs/rpi/px4_test.config b/posix-configs/rpi/px4_test.config index 609d737ba9..b5478ec114 100644 --- a/posix-configs/rpi/px4_test.config +++ b/posix-configs/rpi/px4_test.config @@ -19,7 +19,6 @@ param set BAT_A_PER_V 15.391030303 dataman start df_lsm9ds1_wrapper start -R 4 #df_mpu9250_wrapper start -R 10 -#df_hmc5883_wrapper start ms5611 start navio_rgbled start navio_adc start diff --git a/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt b/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/CMakeLists.txt deleted file mode 100644 index b19f4eefc1..0000000000 --- a/src/drivers/driver_framework_wrapper/df_hmc5883_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_hmc5883_wrapper - MAIN df_hmc5883_wrapper - SRCS - df_hmc5883_wrapper.cpp - DEPENDS - git_driverframework - df_driver_framework - df_hmc5883 - ) diff --git a/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp deleted file mode 100644 index 4cf6bb5111..0000000000 --- a/src/drivers/driver_framework_wrapper/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp +++ /dev/null @@ -1,462 +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_hmc5883_wrapper.cpp - * Lightweight driver to access the HMC5883 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 - - -extern "C" { __EXPORT int df_hmc5883_wrapper_main(int argc, char *argv[]); } - -using namespace DriverFramework; - - -class DfHmc5883Wrapper : public HMC5883 -{ -public: - DfHmc5883Wrapper(enum Rotation rotation, const char *path); - ~DfHmc5883Wrapper(); - - - /** - * Start automatic measurement. - * - * @return 0 on success - */ - int start(); - - /** - * Stop automatic measurement. - * - * @return 0 on success - */ - int stop(); - -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; - -}; - -DfHmc5883Wrapper::DfHmc5883Wrapper(enum Rotation rotation, const char *path) : - HMC5883(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); -} - -DfHmc5883Wrapper::~DfHmc5883Wrapper() -{ - perf_free(_mag_sample_perf); -} - -int DfHmc5883Wrapper::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("HMC5883 init fail: %d", ret); - return ret; - } - - ret = HMC5883::start(); - - if (ret != 0) { - PX4_ERR("HMC5883 start fail: %d", ret); - return ret; - } - - /* Force getting the calibration values. */ - _update_mag_calibration(); - - return 0; -} - -int DfHmc5883Wrapper::stop() -{ - /* Stop sensor. */ - int ret = HMC5883::stop(); - - if (ret != 0) { - PX4_ERR("HMC5883 stop fail: %d", ret); - return ret; - } - - return 0; -} - -void DfHmc5883Wrapper::_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); - } - } -} - - -int DfHmc5883Wrapper::_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); - - mag_report mag_report = {}; - mag_report.timestamp = hrt_absolute_time(); - mag_report.is_external = true; - - /* The standard external mag by 3DR has x pointing to the - * right, y pointing backwards, and z down, therefore switch x - * and y and invert y. */ - const float tmp = data.field_x_ga; - data.field_x_ga = -data.field_y_ga; - data.field_y_ga = tmp; - - // 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; - - // Apply calibration after rotation. - 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_hmc5883_wrapper -{ - -DfHmc5883Wrapper *g_dev = nullptr; - -int start(enum Rotation rotation, const char *path); -int stop(); -int info(); -void usage(); - -int start(enum Rotation rotation, const char *path) -{ - g_dev = new DfHmc5883Wrapper(rotation, path); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating DfHmc5883Wrapper object"); - return -1; - } - - int ret = g_dev->start(); - - if (ret != 0) { - PX4_ERR("DfHmc5883Wrapper start failed"); - return ret; - } - - // Open the MAG sensor - DevHandle h; - DevMgr::getHandle(path, h); - - if (!h.isValid()) { - DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", - 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; -} - -void -usage() -{ - PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace df_hmc5883_wrapper - - -int -df_hmc5883_wrapper_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret = 0; - int myoptind = 1; - const char *myoptarg = NULL; - const char *device_path = MAG_DEVICE_PATH; - - /* jump over start/off/etc and look at options first */ - while ((ch = px4_getopt(argc, argv, "R:D:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - case 'D': - device_path = myoptarg; - break; - - default: - df_hmc5883_wrapper::usage(); - return 0; - } - } - - if (argc <= 1) { - df_hmc5883_wrapper::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - - if (!strcmp(verb, "start")) { - ret = df_hmc5883_wrapper::start(rotation, device_path); - } - - else if (!strcmp(verb, "stop")) { - ret = df_hmc5883_wrapper::stop(); - } - - else if (!strcmp(verb, "info")) { - ret = df_hmc5883_wrapper::info(); - } - - else { - df_hmc5883_wrapper::usage(); - return 1; - } - - return ret; -}