diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 1f275576923..2191d48e42e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -94,15 +94,16 @@ then param set BAT_N_CELLS 3 - param set CAL_ACC0_ID 1376264 - param set CAL_ACC1_ID 1310728 - param set CAL_ACC_PRIME 1376264 + param set CAL_ACC0_ID 1311244 + param set CAL_ACC_PRIME 1311244 - param set CAL_GYRO0_ID 2293768 - param set CAL_GYRO_PRIME 2293768 + param set CAL_GYRO0_ID 2294028 + param set CAL_GYRO_PRIME 2294028 - param set CAL_MAG0_ID 196616 - param set CAL_MAG_PRIME 196616 + param set CAL_MAG0_ID 197388 + param set CAL_MAG_PRIME 197388 + + param set CAL_BARO_PRIME 6620172 param set CBRK_AIRSPD_CHK 0 @@ -201,11 +202,8 @@ sh "$autostart_file" dataman start replay tryapplyparams -simulator start -s -c $simulator_tcp_port +simulator start -c $simulator_tcp_port tone_alarm start -gyrosim start -accelsim start -barosim start gpssim start sensors start commander start @@ -227,15 +225,6 @@ then camera_feedback start fi - -if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ] -then - if param compare CBRK_AIRSPD_CHK 0 - then - measairspeedsim start - fi -fi - # Configure vehicle type specific parameters. # Note: rc.vehicle_setup is the entry point for rc.interface, # rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps. diff --git a/posix-configs/SITL/init/test/cmd_template.in b/posix-configs/SITL/init/test/cmd_template.in index d5f06d72c60..fdc256c9100 100644 --- a/posix-configs/SITL/init/test/cmd_template.in +++ b/posix-configs/SITL/init/test/cmd_template.in @@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0 dataman start -simulator start -t +simulator start tone_alarm start -gyrosim start -accelsim start -barosim start gpssim start -measairspeedsim start pwm_out_sim start ver all diff --git a/posix-configs/SITL/init/test/test_mavlink b/posix-configs/SITL/init/test/test_mavlink index f7179608687..7fe67fc8f53 100644 --- a/posix-configs/SITL/init/test/test_mavlink +++ b/posix-configs/SITL/init/test/test_mavlink @@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0 dataman start -simulator start -t +simulator start tone_alarm start -gyrosim start -accelsim start -barosim start gpssim start -measairspeedsim start pwm_out_sim start ver all diff --git a/posix-configs/SITL/init/test/test_shutdown b/posix-configs/SITL/init/test/test_shutdown index 86274883cac..8c6d76b6110 100644 --- a/posix-configs/SITL/init/test/test_shutdown +++ b/posix-configs/SITL/init/test/test_shutdown @@ -14,13 +14,9 @@ param set SYS_RESTART_TYPE 0 dataman start -simulator start -t +simulator start tone_alarm start -gyrosim start -accelsim start -barosim start gpssim start -measairspeedsim start pwm_out_sim start sensors start @@ -83,11 +79,7 @@ sleep 2 simulator stop tone_alarm stop -gyrosim stop -#accelsim stop -#barosim stop gpssim stop -measairspeedsim stop dataman stop #uorb stop diff --git a/posix-configs/SITL/init/test/test_template.in b/posix-configs/SITL/init/test/test_template.in index 47f84eb9b95..762ee48b2c0 100644 --- a/posix-configs/SITL/init/test/test_template.in +++ b/posix-configs/SITL/init/test/test_template.in @@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0 dataman start -simulator start -t +simulator start tone_alarm start -gyrosim start -accelsim start -barosim start gpssim start -measairspeedsim start pwm_out_sim start ver all diff --git a/posix-configs/eagle/hil/mainapphil.config b/posix-configs/eagle/hil/mainapphil.config index ad8989c4ad9..ec5a8b30d54 100644 --- a/posix-configs/eagle/hil/mainapphil.config +++ b/posix-configs/eagle/hil/mainapphil.config @@ -8,4 +8,4 @@ sleep 1 mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50 mavlink boot_complete -simulator start -p +simulator start diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 6ba876c4658..5428e0871a6 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -54,7 +54,7 @@ #define DRV_MAG_DEVTYPE_HMC5883 0x01 #define DRV_MAG_DEVTYPE_LSM303D 0x02 -#define DRV_MAG_DEVTYPE_ACCELSIM 0x03 +#define DRV_MAG_DEVTYPE_MAGSIM 0x03 #define DRV_MAG_DEVTYPE_MPU9250 0x04 #define DRV_MAG_DEVTYPE_LIS3MDL 0x05 #define DRV_MAG_DEVTYPE_IST8310 0x06 @@ -65,7 +65,6 @@ #define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_MPU6000 0x13 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14 -#define DRV_ACC_DEVTYPE_GYROSIM 0x15 #define DRV_ACC_DEVTYPE_MPU9250 0x16 #define DRV_ACC_DEVTYPE_BMI160 0x17 #define DRV_GYR_DEVTYPE_MPU6000 0x21 @@ -113,6 +112,7 @@ #define DRV_MAG_DEVTYPE_LSM303AGR 0x62 #define DRV_ACC_DEVTYPE_ADIS16497 0x63 #define DRV_GYR_DEVTYPE_ADIS16497 0x64 +#define DRV_BARO_DEVTYPE_BAROSIM 0x65 /* * ioctl() definitions diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index 30ecbfc1d01..dbbfa015be5 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(accelerometer) add_subdirectory(airspeed) +add_subdirectory(barometer) add_subdirectory(device) add_subdirectory(gyroscope) add_subdirectory(led) diff --git a/src/lib/drivers/accelerometer/CMakeLists.txt b/src/lib/drivers/accelerometer/CMakeLists.txt index 38dd1bd4f58..0ed79b55ebe 100644 --- a/src/lib/drivers/accelerometer/CMakeLists.txt +++ b/src/lib/drivers/accelerometer/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ px4_add_library(drivers_accelerometer PX4Accelerometer.cpp) +target_link_libraries(drivers_accelerometer PRIVATE drivers__device) diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp index ba6b11e1763..9d08dfd0132 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.cpp @@ -98,6 +98,12 @@ void PX4Accelerometer::set_device_type(uint8_t devtype) _sensor_accel_pub.get().device_id = device_id.devid; } +void PX4Accelerometer::set_sample_rate(unsigned rate) +{ + _sample_rate = rate; + _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); +} + void PX4Accelerometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z) { sensor_accel_s &report = _sensor_accel_pub.get(); diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp index ff70f88ec22..c0cdc4ab8d2 100644 --- a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp +++ b/src/lib/drivers/accelerometer/PX4Accelerometer.hpp @@ -47,7 +47,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams { public: - PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation); + PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); ~PX4Accelerometer() override; int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; @@ -57,9 +57,7 @@ public: void set_scale(float scale) { _sensor_accel_pub.get().scaling = scale; } void set_temperature(float temperature) { _sensor_accel_pub.get().temperature = temperature; } - void set_sample_rate(unsigned rate) { _sample_rate = rate; _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); } - - void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } + void set_sample_rate(unsigned rate); void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z); @@ -67,6 +65,8 @@ public: private: + void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } + uORB::Publication _sensor_accel_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; @@ -79,7 +79,7 @@ private: int _class_device_instance{-1}; - unsigned _sample_rate{1000}; + unsigned _sample_rate{1000}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_accel_cutoff diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index ff3c65e864a..baa7c8017a3 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -130,6 +130,7 @@ public: DeviceBusType_I2C = 1, DeviceBusType_SPI = 2, DeviceBusType_UAVCAN = 3, + DeviceBusType_SIMULATION = 4, }; /** @@ -160,6 +161,9 @@ public: case DeviceBusType_UAVCAN: return "UAVCAN"; + case DeviceBusType_SIMULATION: + return "SIMULATION"; + case DeviceBusType_UNKNOWN: default: return "UNKNOWN"; diff --git a/src/lib/drivers/gyroscope/CMakeLists.txt b/src/lib/drivers/gyroscope/CMakeLists.txt index 8755e7d6719..f221e179c1c 100644 --- a/src/lib/drivers/gyroscope/CMakeLists.txt +++ b/src/lib/drivers/gyroscope/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ px4_add_library(drivers_gyroscope PX4Gyroscope.cpp) +target_link_libraries(drivers_gyroscope PRIVATE drivers__device) diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index aaa8188b9d7..facd9a93356 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp @@ -98,6 +98,12 @@ void PX4Gyroscope::set_device_type(uint8_t devtype) _sensor_gyro_pub.get().device_id = device_id.devid; } +void PX4Gyroscope::set_sample_rate(unsigned rate) +{ + _sample_rate = rate; + _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); +} + void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z) { sensor_gyro_s &report = _sensor_gyro_pub.get(); diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index dc71fde5d89..8d3295688e6 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -47,7 +47,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams { public: - PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation); + PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE); ~PX4Gyroscope() override; int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override; @@ -57,9 +57,7 @@ public: void set_scale(float scale) { _sensor_gyro_pub.get().scaling = scale; } void set_temperature(float temperature) { _sensor_gyro_pub.get().temperature = temperature; } - void set_sample_rate(unsigned rate) { _sample_rate = rate; _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); } - - void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } + void set_sample_rate(unsigned rate); void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z); @@ -67,6 +65,8 @@ public: private: + void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); } + uORB::Publication _sensor_gyro_pub; math::LowPassFilter2pVector3f _filter{1000, 100}; @@ -79,7 +79,7 @@ private: int _class_device_instance{-1}; - unsigned _sample_rate{1000}; + unsigned _sample_rate{1000}; DEFINE_PARAMETERS( (ParamFloat) _param_imu_gyro_cutoff diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt index bffe0bbecb4..28c77956679 100644 --- a/src/modules/simulator/CMakeLists.txt +++ b/src/modules/simulator/CMakeLists.txt @@ -41,7 +41,7 @@ if(ENABLE_UART_RC_INPUT) elseif (WIN32) set(PIXHAWK_DEVICE "COM3") endif() - + set(PIXHAWK_DEVICE_BAUD 115200) endif() configure_file(simulator_config.h.in simulator_config.h @ONLY) @@ -73,11 +73,11 @@ px4_add_module( drivers__ledsim git_ecl ecl_geo + drivers_accelerometer + drivers_barometer + drivers_gyroscope + drivers_magnetometer ) target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink) -add_subdirectory(accelsim) -add_subdirectory(airspeedsim) -add_subdirectory(barosim) add_subdirectory(gpssim) -add_subdirectory(gyrosim) diff --git a/src/modules/simulator/accelsim/CMakeLists.txt b/src/modules/simulator/accelsim/CMakeLists.txt deleted file mode 100644 index d24e562327a..00000000000 --- a/src/modules/simulator/accelsim/CMakeLists.txt +++ /dev/null @@ -1,45 +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 drivers__accelsim - MAIN accelsim - COMPILE_FLAGS - -Wno-double-promotion - -Wno-cast-align # TODO: fix and enable - -Wno-address-of-packed-member # TODO: fix in c_library_v2 - SRCS - accelsim.cpp - DEPENDS - modules__simulator - ) diff --git a/src/modules/simulator/accelsim/accelsim.cpp b/src/modules/simulator/accelsim/accelsim.cpp deleted file mode 100644 index a3f1b592f2d..00000000000 --- a/src/modules/simulator/accelsim/accelsim.cpp +++ /dev/null @@ -1,1052 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014-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. - * - ****************************************************************************/ - -/** - * @file accelsim.cpp - * Driver for a simulated accelerometer / magnetometer. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" -#define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" - -#define ADDR_WHO_AM_I 0x0F - -#define ACCELSIM_ACCEL_DEFAULT_RATE 250 -#define ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define DIR_READ (1<<7) -#define DIR_WRITE (0<<7) -#define ACC_READ (1<<5) -#define MAG_READ (1<<6) - -extern "C" { __EXPORT int accelsim_main(int argc, char *argv[]); } - -using namespace DriverFramework; - -class ACCELSIM_mag; - -class ACCELSIM : public VirtDevObj -{ -public: - ACCELSIM(const char *path, enum Rotation rotation); - virtual ~ACCELSIM(); - - virtual int init(); - - virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, unsigned long arg); - - /** - * dump register values - */ - void print_registers(); - -protected: - friend class ACCELSIM_mag; - - ssize_t mag_read(void *buffer, size_t buflen); - int mag_ioctl(unsigned long cmd, unsigned long arg); - - int transfer(uint8_t *send, uint8_t *recv, unsigned len); -private: - - ACCELSIM_mag *_mag; - - ringbuffer::RingBuffer *_accel_reports; - ringbuffer::RingBuffer *_mag_reports; - - struct accel_calibration_s _accel_scale; - unsigned _accel_range_m_s2; - float _accel_range_scale; - unsigned _accel_samplerate; - unsigned _accel_onchip_filter_bandwith; - - struct mag_calibration_s _mag_scale; - unsigned _mag_range_ga; - float _mag_range_scale; - unsigned _mag_samplerate; - - orb_advert_t _accel_topic{nullptr}; - int _accel_orb_class_instance{-1}; - int _accel_class_instance{-1}; - - unsigned _accel_read; - unsigned _mag_read; - - perf_counter_t _accel_sample_perf; - perf_counter_t _mag_sample_perf; - perf_counter_t _accel_reschedules; - perf_counter_t _bad_registers; - perf_counter_t _bad_values; - - math::LowPassFilter2p _accel_filter_x; - math::LowPassFilter2p _accel_filter_y; - math::LowPassFilter2p _accel_filter_z; - - enum Rotation _rotation; - - // values used to - float _last_accel[3]; - uint8_t _constant_accel_count; - - // last temperature value - float _last_temperature; - - /** - * Override Start automatic measurement. - */ - virtual int start(); - - /** - * Override Stop automatic measurement. - */ - virtual int stop(); - - /** - * Fetch accel measurements from the sensor and update the report ring. - */ - virtual void _measure(); - - /** - * Fetch mag measurements from the sensor and update the report ring. - */ - void mag_measure(); - - /** - * Set the ACCELSIM accel measurement range. - * - * @param max_g The measurement range of the accel is in g (9.81m/s^2) - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_range(unsigned max_g); - - /** - * Set the ACCELSIM mag measurement range. - * - * @param max_ga The measurement range of the mag is in Ga - * Zero selects the maximum supported range. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int mag_set_range(unsigned max_g); - - /** - * Set the driver lowpass filter bandwidth. - * - * @param bandwidth The anti-alias filter bandwidth in Hz - * Zero selects the highest bandwidth - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int accel_set_driver_lowpass_filter(float samplerate, float bandwidth); - - /* this class cannot be copied */ - ACCELSIM(const ACCELSIM &); - ACCELSIM operator=(const ACCELSIM &); -}; - -/** - * Helper class implementing the mag driver node. - */ -class ACCELSIM_mag : public VirtDevObj -{ -public: - ACCELSIM_mag(ACCELSIM *parent); - ~ACCELSIM_mag() = default; - - virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, unsigned long arg); - - virtual int start(); - virtual int stop(); - -protected: - friend class ACCELSIM; - -private: - ACCELSIM *_parent; - - orb_advert_t _mag_topic{nullptr}; - int _mag_orb_class_instance{-1}; - int _mag_class_instance{-1}; - - virtual void _measure(); - - /* this class does not allow copying due to ptr data members */ - ACCELSIM_mag(const ACCELSIM_mag &) = delete; - ACCELSIM_mag operator=(const ACCELSIM_mag &) = delete; -}; - - -ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : - VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), - _mag(new ACCELSIM_mag(this)), - _accel_reports(nullptr), - _mag_reports(nullptr), - _accel_scale{}, - _accel_range_m_s2(0.0f), - _accel_range_scale(0.0f), - _accel_samplerate(0), - _accel_onchip_filter_bandwith(0), - _mag_scale{}, - _mag_range_ga(0.0f), - _mag_range_scale(0.0f), - _mag_samplerate(0), - _accel_read(0), - _mag_read(0), - _accel_sample_perf(perf_alloc(PC_ELAPSED, "sim_accel_read")), - _mag_sample_perf(perf_alloc(PC_ELAPSED, "sim_mag_read")), - _accel_reschedules(perf_alloc(PC_COUNT, "sim_accel_resched")), - _bad_registers(perf_alloc(PC_COUNT, "sim_bad_registers")), - _bad_values(perf_alloc(PC_COUNT, "sim_bad_values")), - _accel_filter_x(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_y(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _rotation(rotation), - _constant_accel_count(0), - _last_temperature(0) -{ - m_id.dev_id_s.bus = 1; - m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; - - /* Prime _mag with parents devid. */ - _mag->m_id.dev_id = m_id.dev_id; - _mag->m_id.dev_id_s.devtype = DRV_MAG_DEVTYPE_ACCELSIM; - - // default scale factors - _accel_scale.x_offset = 0.0f; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0.0f; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0.0f; - _accel_scale.z_scale = 1.0f; - - _mag_scale.x_offset = 0.0f; - _mag_scale.x_scale = 1.0f; - _mag_scale.y_offset = 0.0f; - _mag_scale.y_scale = 1.0f; - _mag_scale.z_offset = 0.0f; - _mag_scale.z_scale = 1.0f; -} - -ACCELSIM::~ACCELSIM() -{ - /* make sure we are truly inactive */ - _mag->stop(); - stop(); - - /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; - } - - if (_mag_reports != nullptr) { - delete _mag_reports; - } - - delete _mag; - - /* delete the perf counter */ - perf_free(_accel_sample_perf); - perf_free(_mag_sample_perf); - perf_free(_bad_registers); - perf_free(_bad_values); - perf_free(_accel_reschedules); -} - -int -ACCELSIM::init() -{ - /* do SIM init first */ - int ret = VirtDevObj::init(); - - if (ret != PX4_OK) { - PX4_WARN("SIM init failed"); - return ret; - } - - /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - - if (_accel_reports == nullptr) { - PX4_WARN("_accel_reports creation failed"); - return -ENOMEM; - } - - _mag_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_mag_s)); - - if (_mag_reports == nullptr) { - PX4_WARN("_mag_reports creation failed"); - return -ENOMEM; - } - - /* do init for the mag device node */ - ret = _mag->init(); - - if (ret != PX4_OK) { - PX4_WARN("MAG init failed"); - return ret; - } - - return PX4_OK; -} - -int -ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) -{ - uint8_t cmd = send[0]; - - if (cmd & DIR_READ) { - // Get data from the simulator - Simulator *sim = Simulator::getInstance(); - - if (sim == nullptr) { - return ENODEV; - } - - // FIXME - not sure what interrupt status should be - recv[1] = 0; - - // skip cmd and status bytes - if (cmd & ACC_READ) { - sim->getRawAccelReport(&recv[2], len - 2); - - } else if (cmd & MAG_READ) { - sim->getMagReport(&recv[2], len - 2); - } - } - - return PX4_OK; -} - -ssize_t -ACCELSIM::devRead(void *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_accel_s); - sensor_accel_s *arb = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (m_sample_interval_usecs > 0) { - /* - * While there is space in the caller's buffer, and reports, copy them. - */ - while (count--) { - if (_accel_reports->get(arb)) { - ret += sizeof(*arb); - arb++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _measure(); - - /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) { - ret = sizeof(*arb); - } - - return ret; -} - -ssize_t -ACCELSIM::mag_read(void *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(struct mag_report); - mag_report *mrb = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_mag->m_sample_interval_usecs > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - */ - while (count--) { - if (_mag_reports->get(mrb)) { - ret += sizeof(*mrb); - mrb++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement */ - _mag_reports->flush(); - _mag->_measure(); - - /* measurement will have generated a report, copy it out */ - if (_mag_reports->get(mrb)) { - ret = sizeof(*mrb); - } - - return ret; -} - -int -ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) -{ - unsigned long ul_arg = (unsigned long)arg; - - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (ul_arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - return devIOCTL(SENSORIOCSPOLLRATE, ACCELSIM_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / ul_arg; - - /* check against maximum sane rate */ - if (interval < 500) { - return -EINVAL; - } - - /* adjust filters */ - accel_set_driver_lowpass_filter((float)ul_arg, _accel_filter_x.get_cutoff_freq()); - - bool want_start = (m_sample_interval_usecs == 0); - - /* update interval for next measurement */ - setSampleInterval(interval); - - if (want_start) { - start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - // Nothing to do for simulator - return OK; - - case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - - } else { - return -EINVAL; - } - } - - default: - /* give it to the superclass */ - return VirtDevObj::devIOCTL(cmd, arg); - } -} - -int -ACCELSIM::mag_ioctl(unsigned long cmd, unsigned long arg) -{ - unsigned long ul_arg = (unsigned long)arg; - - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - /* 100 Hz is max for mag */ - return mag_ioctl(SENSORIOCSPOLLRATE, 100); - - /* adjust to a legal polling interval in Hz */ - default: { - /* convert hz to hrt interval via microseconds */ - unsigned interval = 1000000 / ul_arg; - - /* check against maximum sane rate (1ms) */ - if (interval < 10000) { - return -EINVAL; - } - - bool want_start = (_mag->m_sample_interval_usecs == 0); - - /* update interval for next measurement */ - _mag->setSampleInterval(interval); - - if (want_start) { - _mag->start(); - } - - return OK; - } - } - } - - case SENSORIOCRESET: - // Nothing to do for simulator - return OK; - - case MAGIOCSSCALE: - /* copy scale in */ - memcpy(&_mag_scale, (struct mag_calibration_s *) arg, sizeof(_mag_scale)); - return OK; - - case MAGIOCGSCALE: - /* copy scale out */ - memcpy((struct mag_calibration_s *) arg, &_mag_scale, sizeof(_mag_scale)); - return OK; - - case MAGIOCSRANGE: - return mag_set_range(arg); - - case MAGIOCGEXTERNAL: - /* Even if this sensor is on the "external" SPI bus - * it is still fixed to the autopilot assembly, - * so always return 0. - */ - return 0; - - default: - /* give it to the superclass */ - return VirtDevObj::devIOCTL(cmd, arg); - } -} - -int -ACCELSIM::accel_set_range(unsigned max_g) -{ - float new_scale_g_digit = 0.732e-3f; - - _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; - - return OK; -} - -int -ACCELSIM::mag_set_range(unsigned max_ga) -{ - float new_scale_ga_digit = 0.479e-3f; - - _mag_range_scale = new_scale_ga_digit; - - return OK; -} - -int -ACCELSIM::accel_set_driver_lowpass_filter(float samplerate, float bandwidth) -{ - _accel_filter_x.set_cutoff_frequency(samplerate, bandwidth); - _accel_filter_y.set_cutoff_frequency(samplerate, bandwidth); - _accel_filter_z.set_cutoff_frequency(samplerate, bandwidth); - - return OK; -} - -int -ACCELSIM::start() -{ - //PX4_INFO("ACCELSIM::start"); - /* make sure we are stopped first */ - stop(); - - /* reset the report ring */ - _accel_reports->flush(); - _mag_reports->flush(); - - int ret2 = VirtDevObj::start(); - - if (ret2 != 0) { - PX4_ERR("ACCELSIM::start base class start failed"); - } - - return (ret2 != 0) ? -1 : 0; -} - -int -ACCELSIM::stop() -{ - //PX4_INFO("ACCELSIM::stop"); - return VirtDevObj::stop(); -} - -void -ACCELSIM::_measure() -{ -#if 0 - static int x = 0; - - // Verify the samples are being taken at the expected rate - if (x == 99) { - x = 0; - PX4_INFO("ACCELSIM::measure %" PRIu64, hrt_absolute_time()); - - } else { - x++; - } - -#endif - - /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - float temperature; - float x; - float y; - float z; - } raw_accel_report; -#pragma pack(pop) - - sensor_accel_s accel_report = {}; - - /* start the performance counter */ - perf_begin(_accel_sample_perf); - - /* fetch data from the sensor */ - memset(&raw_accel_report, 0, sizeof(raw_accel_report)); - raw_accel_report.cmd = DIR_READ | ACC_READ; - - if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { - return; - } - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - accel_report.timestamp = hrt_absolute_time(); - accel_report.device_id = 1310728; - - // use the temperature from the last mag reading - accel_report.temperature = _last_temperature; - - // report the error count as the sum of the number of bad - // register reads and bad values. This allows the higher level - // code to decide if it should use this sensor based on - // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - - if (math::isZero(_accel_range_scale)) { - _accel_range_scale = FLT_EPSILON; - } - - accel_report.x_raw = math::constrainFloatToInt16(raw_accel_report.x / _accel_range_scale); - accel_report.y_raw = math::constrainFloatToInt16(raw_accel_report.y / _accel_range_scale); - accel_report.z_raw = math::constrainFloatToInt16(raw_accel_report.z / _accel_range_scale); - - accel_report.x = raw_accel_report.x; - accel_report.y = raw_accel_report.y; - accel_report.z = raw_accel_report.z; - - //accel_report.integral_dt = 0; - //accel_report.x_integral = 0.0f; - //accel_report.y_integral = 0.0f; - //accel_report.z_integral = 0.0f; - - accel_report.temperature = 25.0f; - - accel_report.scaling = _accel_range_scale; - - _accel_reports->force(&accel_report); - - orb_publish_auto(ORB_ID(sensor_accel), &_accel_topic, &accel_report, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); - - _accel_read++; - - /* stop the perf counter */ - perf_end(_accel_sample_perf); -} - -void -ACCELSIM::mag_measure() -{ - /* status register and data as read back from the device */ -#pragma pack(push, 1) - struct { - uint8_t cmd; - uint8_t status; - float temperature; - float x; - float y; - float z; - } raw_mag_report; -#pragma pack(pop) - - /* start the performance counter */ - perf_begin(_mag_sample_perf); - - /* fetch data from the sensor */ - memset(&raw_mag_report, 0, sizeof(raw_mag_report)); - raw_mag_report.cmd = DIR_READ | MAG_READ; - - if (OK != transfer((uint8_t *)&raw_mag_report, (uint8_t *)&raw_mag_report, sizeof(raw_mag_report))) { - return; - } - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - mag_report mag_report = {}; - mag_report.timestamp = hrt_absolute_time(); - mag_report.device_id = 196616; - mag_report.is_external = false; - - if (math::isZero(_mag_range_scale)) { - _mag_range_scale = FLT_EPSILON; - } - - float xraw_f = math::constrainFloatToInt16(raw_mag_report.x / _mag_range_scale); - float yraw_f = math::constrainFloatToInt16(raw_mag_report.y / _mag_range_scale); - float zraw_f = math::constrainFloatToInt16(raw_mag_report.z / _mag_range_scale); - - mag_report.x_raw = xraw_f; - mag_report.y_raw = yraw_f; - mag_report.z_raw = zraw_f; - - /* apply user specified rotation */ - rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - /* remember the temperature. The datasheet isn't clear, but it - * seems to be a signed offset from 25 degrees C in units of 0.125C - */ - _last_temperature = raw_mag_report.temperature; - mag_report.temperature = _last_temperature; - mag_report.x = raw_mag_report.x; - mag_report.y = raw_mag_report.y; - mag_report.z = raw_mag_report.z; - - _mag_reports->force(&mag_report); - - orb_publish_auto(ORB_ID(sensor_mag), &_mag->_mag_topic, &mag_report, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); - - _mag_read++; - - /* stop the perf counter */ - perf_end(_mag_sample_perf); -} - -ACCELSIM_mag::ACCELSIM_mag(ACCELSIM *parent) : - VirtDevObj("ACCELSIM_mag", ACCELSIM_DEVICE_PATH_MAG, MAG_BASE_DEVICE_PATH, 10000), - _parent(parent), - _mag_topic(nullptr), - _mag_orb_class_instance(-1), - _mag_class_instance(-1) -{ - m_id.dev_id_s.bus = 1; - m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_ACCELSIM; -} - -ssize_t -ACCELSIM_mag::devRead(void *buffer, size_t buflen) -{ - return _parent->mag_read(buffer, buflen); -} - -int -ACCELSIM_mag::devIOCTL(unsigned long cmd, unsigned long arg) -{ - int ret; - - switch (cmd) { - case DEVIOCGDEVICEID: - ret = (int)VirtDevObj::devIOCTL(cmd, arg); - //PX4_WARN("DEVICE ID: %d", ret); - return ret; - break; - - default: - return _parent->mag_ioctl(cmd, arg); - } -} - -int ACCELSIM_mag::start() -{ - //PX4_INFO("ACCELSIM_mag::start"); - return VirtDevObj::start(); -} - -int ACCELSIM_mag::stop() -{ - //PX4_INFO("ACCELSIM_mag::stop"); - return VirtDevObj::stop(); -} - -void ACCELSIM_mag::_measure() -{ - //PX4_INFO("ACCELSIM_mag::_measure"); - _parent->mag_measure(); -} - -/** - * Local functions in support of the shell command. - */ -namespace accelsim -{ - -ACCELSIM *g_dev; - -int start(enum Rotation rotation); -int info(); -void usage(); - -/** - * Start the driver. - * - * This function call only returns once the driver is - * up and running or failed to detect the sensor. - */ -int -start(enum Rotation rotation) -{ - if (g_dev != nullptr) { - PX4_WARN("already started"); - return 0; - } - - DevHandle h; - DevHandle h_mag; - - /* create the driver */ - g_dev = new ACCELSIM(ACCELSIM_DEVICE_PATH_ACCEL, rotation); - - if (g_dev == nullptr) { - PX4_ERR("failed instantiating ACCELSIM obj"); - goto fail; - } - - if (OK != g_dev->init()) { - PX4_ERR("failed init of ACCELSIM obj"); - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - DevMgr::getHandle(ACCELSIM_DEVICE_PATH_ACCEL, h); - - if (!h.isValid()) { - PX4_WARN("open %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - goto fail; - } - - if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_ACCEL); - DevMgr::releaseHandle(h); - goto fail; - } - - DevMgr::getHandle(ACCELSIM_DEVICE_PATH_MAG, h_mag); - - /* don't fail if mag dev cannot be opened */ - if (h_mag.isValid()) { - if (h_mag.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); - } - - } else { - PX4_ERR("ioctl SENSORIOCSPOLLRATE %s failed", ACCELSIM_DEVICE_PATH_MAG); - } - - DevMgr::releaseHandle(h); - DevMgr::releaseHandle(h_mag); - - return 0; -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - PX4_ERR("driver start failed"); - return 1; -} - -/** - * 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); - - unsigned dummy = 0; - PX4_WARN("device_id: %u", (unsigned int)g_dev->devIOCTL(DEVIOCGDEVICEID, dummy)); - - return 0; -} - -void -usage() -{ - PX4_WARN("Usage: accelsim 'start', 'info'"); - PX4_WARN("options:"); - PX4_WARN(" -R rotation"); -} - -} // namespace - -int -accelsim_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret; - int myoptind = 1; - const char *myoptarg = nullptr; - - /* 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: - accelsim::usage(); - return 0; - } - } - - if (myoptind >= argc) { - accelsim::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ret = accelsim::start(rotation); - } - - /* - * Print driver information. - */ - else if (!strcmp(verb, "info")) { - ret = accelsim::info(); - } - - else { - accelsim::usage(); - return 1; - } - - return ret; -} diff --git a/src/modules/simulator/airspeedsim/CMakeLists.txt b/src/modules/simulator/airspeedsim/CMakeLists.txt deleted file mode 100644 index 2227e5e8755..00000000000 --- a/src/modules/simulator/airspeedsim/CMakeLists.txt +++ /dev/null @@ -1,46 +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 drivers__airspeedsim - MAIN measairspeedsim - COMPILE_FLAGS - -Wno-double-promotion - -Wno-cast-align # TODO: fix and enable - -Wno-address-of-packed-member # TODO: fix in c_library_v2 - SRCS - airspeedsim.cpp - meas_airspeed_sim.cpp - DEPENDS - modules__simulator - ) diff --git a/src/modules/simulator/airspeedsim/airspeedsim.cpp b/src/modules/simulator/airspeedsim/airspeedsim.cpp deleted file mode 100644 index c3b0cbda4ef..00000000000 --- a/src/modules/simulator/airspeedsim/airspeedsim.cpp +++ /dev/null @@ -1,352 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file ets_airspeed.cpp - * @author Simon Wilks - * @author Mark Charlebois (simulator) - * @author Roman Bapst (simulator) - * Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include - -#include "airspeedsim.h" - -AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) : - CDev(path), - _reports(nullptr), - _retries(0), - _sensor_ok(false), - _measure_ticks(0), - _collect_phase(false), - _diff_pres_offset(0.0f), - _airspeed_pub(nullptr), - _class_instance(-1), - _conversion_interval(conversion_interval), - _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), - _comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")) -{ -} - -AirspeedSim::~AirspeedSim() -{ - /* make sure we are truly inactive */ - stop(); - - if (_class_instance != -1) { - unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); - } - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); -} - -int -AirspeedSim::init() -{ - int ret = ERROR; - - /* init base class */ - if (CDev::init() != OK) { - PX4_ERR("CDev init failed"); - goto out; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s)); - - if (_reports == nullptr) { - goto out; - } - - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); - - /* publication init */ - if (_class_instance == 0) { - - /* advertise sensor topic, measure manually to initialize valid report */ - struct differential_pressure_s arp; - measure(); - _reports->get(&arp); - - /* measurement will have generated a report, publish */ - _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); - - if (_airspeed_pub == nullptr) { - PX4_WARN("uORB started?"); - } - } - - ret = OK; - -out: - return ret; -} - -int -AirspeedSim::probe() -{ - /* on initial power up the device may need more than one retry - for detection. Once it is running the number of retries can - be reduced - */ - _retries = 4; - int ret = measure(); - - // drop back to 2 retries once initialised - _retries = 2; - return ret; -} - -int -AirspeedSim::ioctl(cdev::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* set interval for next measurement to minimum legal value */ - _measure_ticks = USEC2TICK(_conversion_interval); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - - /* adjust to a legal polling interval in Hz */ - default: { - /* do we need to start internal polling? */ - bool want_start = (_measure_ticks == 0); - - /* convert hz to tick interval via microseconds */ - int ticks = USEC2TICK(1000000 / arg); - - /* check against maximum rate */ - if (ticks < USEC2TICK(_conversion_interval)) { - return -EINVAL; - } - - /* update interval for next measurement */ - _measure_ticks = ticks; - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale *)arg; - _diff_pres_offset = s->offset_pa; - return OK; - } - - default: - /* give it to the superclass */ - //return I2C::ioctl(filp, cmd, arg); XXX SIM should be super class - return 0; - } -} - -ssize_t -AirspeedSim::read(cdev::file_t *filp, char *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(differential_pressure_s); - differential_pressure_s *abuf = reinterpret_cast(buffer); - int ret = 0; - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is enabled */ - if (_measure_ticks > 0) { - - /* - * While there is space in the caller's buffer, and reports, copy them. - * Note that we may be pre-empted by the workq thread while we are doing this; - * we are careful to avoid racing with them. - */ - while (count--) { - if (_reports->get(abuf)) { - ret += sizeof(*abuf); - abuf++; - } - } - - /* if there was no data, warn the caller */ - return ret ? ret : -EAGAIN; - } - - /* manual measurement - run one conversion */ - do { - _reports->flush(); - - /* trigger a measurement */ - if (OK != measure()) { - ret = -EIO; - break; - } - - /* wait for it to complete */ - px4_usleep(_conversion_interval); - - /* run the collection phase */ - if (OK != collect()) { - ret = -EIO; - break; - } - - /* state machine will have generated a report, copy it out */ - if (_reports->get(abuf)) { - ret = sizeof(*abuf); - } - - } while (0); - - return ret; -} - -void -AirspeedSim::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - _reports->flush(); - - /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1); -} - -void -AirspeedSim::stop() -{ - work_cancel(HPWORK, &_work); -} - -void -AirspeedSim::cycle_trampoline(void *arg) -{ - AirspeedSim *dev = (AirspeedSim *)arg; - - dev->cycle(); -} - -void -AirspeedSim::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - PX4_INFO("poll interval: %u ticks", _measure_ticks); - _reports->print_info("report queue"); -} - -void -AirspeedSim::new_report(const differential_pressure_s &report) -{ - _reports->force(&report); -} - -int -AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) -{ - if (recv_len > 0) { - // this is equivalent to the collect phase - Simulator *sim = Simulator::getInstance(); - - if (sim == nullptr) { - PX4_ERR("Error BARO_SIM::transfer no simulator"); - return -ENODEV; - } - - PX4_DEBUG("BARO_SIM::transfer getting sample"); - sim->getAirspeedSample(recv, recv_len); - - } else { - // we don't need measure phase - } - - return 0; -} diff --git a/src/modules/simulator/airspeedsim/airspeedsim.h b/src/modules/simulator/airspeedsim/airspeedsim.h deleted file mode 100644 index be1e204395c..00000000000 --- a/src/modules/simulator/airspeedsim/airspeedsim.h +++ /dev/null @@ -1,172 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file airspeed.h - * @author Simon Wilks - * - * Generic driver for airspeed sensors connected via I2C. - */ - -#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 - -/* Default I2C bus */ -#define PX4_I2C_BUS_DEFAULT PX4_I2C_BUS_EXPANSION - -#ifndef CONFIG_SCHED_WORKQUEUE -# error This requires CONFIG_SCHED_WORKQUEUE. -#endif - -class __EXPORT AirspeedSim : public cdev::CDev -{ -public: - AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path); - virtual ~AirspeedSim(); - - virtual int init(); - - virtual ssize_t read(cdev::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(cdev::file_t *filp, int cmd, unsigned long arg); - - /** - * Diagnostics - print some basic information about the driver. - */ - virtual void print_info(); - -private: - ringbuffer::RingBuffer *_reports; - - unsigned _retries; // XXX this should come from the SIM class - - /* this class has pointer data members and should not be copied */ - AirspeedSim(const AirspeedSim &); - AirspeedSim &operator=(const AirspeedSim &); - -protected: - virtual int probe(); - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - virtual void cycle() = 0; - virtual int measure() = 0; - virtual int collect() = 0; - - virtual int transfer(const uint8_t *send, unsigned send_len, - uint8_t *recv, unsigned recv_len); - - struct work_s _work {}; - bool _sensor_ok; - int _measure_ticks; - bool _collect_phase; - float _diff_pres_offset; - - orb_advert_t _airspeed_pub; - - int _class_instance; - - int _conversion_interval; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - - - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - - /** - * Initialise the automatic measurement state machine and start it. - * - * @note This function is called at open and error time. It might make sense - * to make it more aggressive about resetting the bus in case of errors. - */ - void start(); - - /** - * Stop the automatic measurement state machine. - */ - void stop(); - - /** - * Static trampoline from the workq context; because we don't have a - * generic workq wrapper yet. - * - * @param arg Instance pointer for the driver that is polling. - */ - static void cycle_trampoline(void *arg); - - /** - * add a new report to the reports queue - * - * @param report differential_pressure_s report - */ - void new_report(const differential_pressure_s &report); -}; - - diff --git a/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp b/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp deleted file mode 100644 index 8209afc9ab0..00000000000 --- a/src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp +++ /dev/null @@ -1,549 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014, 2017 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 meas_airspeed_sim.cpp - * @author Lorenz Meier - * @author Sarthak Kaingade - * @author Simon Wilks - * @author Thomas Gubler - * @author Roman Bapst - * - * Driver for a simulated airspeed sensor. - * - */ - - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include - -#include "airspeedsim.h" - -/* I2C bus address is 1010001x */ -#define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */ -#define PATH_MS4525 "/dev/ms4525" -/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ -#define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ -#define PATH_MS5525 "/dev/ms5525" - -/* Register address */ -#define ADDR_READ_MR 0x00 /* write to this address to start conversion */ - -/* Measurement rate is 100Hz */ -#define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 1.2f -#define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ - -class MEASAirspeedSim : public AirspeedSim -{ -public: - MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); - -protected: - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - virtual void cycle(); - virtual int measure(); - virtual int collect(); - - math::LowPassFilter2p _filter; - - /** - * Correct for 5V rail voltage variations - */ - void voltage_correction(float &diff_pres_pa, float &temperature); - - int _t_system_power; - struct system_power_s system_power; -}; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]); - -MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address, - CONVERSION_INTERVAL, path), - _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), - _t_system_power(-1), - system_power{} -{} - -int -MEASAirspeedSim::measure() -{ - int ret; - - /* - * Send the command to begin a measurement. - */ - uint8_t cmd = 0; - ret = transfer(&cmd, 1, nullptr, 0); - - if (OK != ret) { - perf_count(_comms_errors); - } - - return ret; -} - -int -MEASAirspeedSim::collect() -{ - int ret = -EIO; - - /* read from the sensor */ -#pragma pack(push, 1) - struct { - float temperature; - float diff_pressure; - } airspeed_report; -#pragma pack(pop) - - - perf_begin(_sample_perf); - - ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report)); - - if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } - - uint8_t status = 0; - - switch (status) { - case 0: - break; - - case 1: - - /* fallthrough */ - case 2: - - /* fallthrough */ - case 3: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - } - - float temperature = airspeed_report.temperature; - float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar - - differential_pressure_s report = {}; - - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.temperature = temperature; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); - report.differential_pressure_raw_pa = diff_press_pa_raw; - - int instance; - orb_publish_auto(ORB_ID(differential_pressure), &_airspeed_pub, &report, &instance, ORB_PRIO_DEFAULT); - - new_report(report); - - /* notify anyone waiting for data */ - poll_notify(POLLIN); - - ret = OK; - - perf_end(_sample_perf); - - return ret; -} - -void -MEASAirspeedSim::cycle() -{ - int ret; - - /* collection phase? */ - if (_collect_phase) { - - /* perform collection */ - ret = collect(); - - if (OK != ret) { - /* restart the measurement state machine */ - start(); - _sensor_ok = false; - return; - } - - /* next phase is measurement */ - _collect_phase = false; - - /* - * Is there a collect->measure gap? - */ - if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) { - - /* schedule a fresh cycle call when we are ready to measure again */ - work_queue(HPWORK, - &_work, - (worker_t)&AirspeedSim::cycle_trampoline, - this, - _measure_ticks - USEC2TICK(CONVERSION_INTERVAL)); - - return; - } - } - - /* measurement phase */ - ret = measure(); - - if (OK != ret) { - PX4_ERR("measure error"); - } - - _sensor_ok = (ret == OK); - - /* next phase is collection */ - _collect_phase = true; - - /* schedule a fresh cycle call when the measurement is done */ - work_queue(HPWORK, - &_work, - (worker_t)&AirspeedSim::cycle_trampoline, - this, - USEC2TICK(CONVERSION_INTERVAL)); -} - -/** - * Local functions in support of the shell command. - */ -namespace meas_airspeed_sim -{ - -MEASAirspeedSim *g_dev = nullptr; - -int start(int i2c_bus); -int stop(); -int test(); -int reset(); -int info(); - -/** - * Start the driver. - * - * This function call only returns once the driver is up and running - * or failed to detect the sensor. - */ -int -start(int i2c_bus) -{ - int fd; - - if (g_dev != nullptr) { - PX4_WARN("already started"); - } - - /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); - - /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) { - goto fail; - } - - /* try the MS5525DSO next if init fails */ - if (OK != g_dev->AirspeedSim::init()) { - delete g_dev; - g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); - - /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) { - goto fail; - } - - /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->AirspeedSim::init()) { - goto fail; - } - } - - /* set the poll rate to default, starts automatic data collection */ - fd = px4_open(PATH_MS4525, O_RDONLY); - - if (fd < 0) { - goto fail; - } - - if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - goto fail; - } - - return 0; - -fail: - - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - } - - PX4_ERR("no MS4525 airspeedSim sensor connected"); - return 1; -} - -/** - * Stop the driver - */ -int -stop() -{ - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; - - } else { - PX4_ERR("driver not running"); - } - - return 0; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - struct differential_pressure_s report; - ssize_t sz; - int ret; - - int fd = px4_open(PATH_MS4525, O_RDONLY); - - if (fd < 0) { - PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525); - return 1; - } - - /* do a simple demand read */ - sz = px4_read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_ERR("immediate read failed"); - return 1; - } - - print_message(report); - - /* start the sensor polling at 2Hz */ - if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) { - PX4_WARN("failed to set 2Hz poll rate"); - } - - /* read the sensor 5x and report each value */ - for (unsigned i = 0; i < 5; i++) { - struct pollfd fds; - - /* wait for data to be ready */ - fds.fd = fd; - fds.events = POLLIN; - ret = poll(&fds, 1, 2000); - - if (ret != 1) { - warnx("timed out"); - } - - /* now go get it */ - sz = read(fd, &report, sizeof(report)); - - if (sz != sizeof(report)) { - PX4_WARN("periodic read failed"); - } - - print_message(report); - } - - /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { - PX4_ERR("failed to set default rate"); - return 1; - } - - PX4_WARN("PASS"); - - return 0; -} - -/** - * Reset the driver. - */ -int -reset() -{ - int fd = open(PATH_MS4525, O_RDONLY); - - if (fd < 0) { - PX4_ERR("failed "); - return 1; - } - - if (ioctl(fd, SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - close(fd); - return 1; - } - - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - close(fd); - return 1; - } - - close(fd); - - return 0; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - if (g_dev == nullptr) { - PX4_WARN("driver not running"); - return -1; - } - - printf("state @ %p\n", g_dev); - g_dev->print_info(); - - return 0; -} - -} // namespace - - -static void -meas_airspeed_usage() -{ - PX4_WARN("usage: measairspeedsim command [options]"); - PX4_WARN("options:"); - PX4_WARN("\t-b --bus i2cbus (%d)", 1); - PX4_WARN("command:"); - PX4_WARN("\tstart|stop|reset|test|info"); -} - -int -measairspeedsim_main(int argc, char *argv[]) -{ - int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT; - - int i; - - for (i = 1; i < argc; i++) { - if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) { - if (argc > i + 1) { - i2c_bus = atoi(argv[i + 1]); - } - } - } - - int ret = 1; - - /* - * Start/load the driver. - */ - if (!strcmp(argv[1], "start")) { - return meas_airspeed_sim::start(i2c_bus); - } - - /* - * Stop the driver - */ - if (!strcmp(argv[1], "stop")) { - return meas_airspeed_sim::stop(); - } - - /* - * Test the driver/device. - */ - if (!strcmp(argv[1], "test")) { - return meas_airspeed_sim::test(); - } - - /* - * Reset the driver. - */ - if (!strcmp(argv[1], "reset")) { - return meas_airspeed_sim::reset(); - } - - /* - * Print driver information. - */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { - return meas_airspeed_sim::info(); - } - - meas_airspeed_usage(); - return ret; -} diff --git a/src/modules/simulator/barosim/CMakeLists.txt b/src/modules/simulator/barosim/CMakeLists.txt deleted file mode 100644 index 0ad5c0a024b..00000000000 --- a/src/modules/simulator/barosim/CMakeLists.txt +++ /dev/null @@ -1,45 +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 drivers__barosim - MAIN barosim - COMPILE_FLAGS - -Wno-double-promotion - -Wno-cast-align # TODO: fix and enable - -Wno-address-of-packed-member # TODO: fix in c_library_v2 - SRCS - baro.cpp - DEPENDS - modules__simulator - ) diff --git a/src/modules/simulator/barosim/baro.cpp b/src/modules/simulator/barosim/baro.cpp deleted file mode 100644 index 86a370fbacc..00000000000 --- a/src/modules/simulator/barosim/baro.cpp +++ /dev/null @@ -1,347 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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. - * - ****************************************************************************/ - -/** - * @file baro.cpp - * Driver for the simulated barometric pressure sensor - */ - -#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 - -#include "barosim.h" -#include "VirtDevObj.hpp" - -/* helper macro for arithmetic - returns the square of the argument */ -#define POW2(_x) ((_x) * (_x)) - -#define BAROSIM_DEV_PATH "/dev/barosim" - -#define BAROSIM_MEASURE_INTERVAL_US (10000) - -using namespace DriverFramework; - -class BAROSIM : public VirtDevObj -{ -public: - BAROSIM(const char *path); - virtual ~BAROSIM(); - - virtual int init() override; - - virtual int devIOCTL(unsigned long cmd, unsigned long arg) override; - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - -protected: - - ringbuffer::RingBuffer *_reports; - - /* last report */ - sensor_baro_s report; - - orb_advert_t _baro_topic; - int _orb_class_instance; - - perf_counter_t _sample_perf; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } - - virtual void _measure() override; - - /** - * Collect the result of the most recent measurement. - */ - virtual int collect(); -}; - -static BAROSIM *g_barosim = nullptr; - -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int barosim_main(int argc, char *argv[]); - -BAROSIM::BAROSIM(const char *path) : - VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, BAROSIM_MEASURE_INTERVAL_US), - _reports(nullptr), - report{}, - _baro_topic(nullptr), - _orb_class_instance(-1), - _sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")), - _measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")), - _comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors")) -{ -} - -BAROSIM::~BAROSIM() -{ - /* make sure we are truly inactive */ - stop(); - setSampleInterval(0); - - /* free any existing reports */ - if (_reports != nullptr) { - delete _reports; - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_measure_perf); - perf_free(_comms_errors); -} - -int -BAROSIM::init() -{ - int ret; - sensor_baro_s brp = {}; - - ret = VirtDevObj::init(); - - if (ret != OK) { - PX4_ERR("VirtDevObj init failed"); - goto out; - } - - /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s)); - - if (_reports == nullptr) { - PX4_ERR("can't get memory for reports"); - ret = -ENOMEM; - goto out; - } - - _reports->flush(); - - _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); - - if (_baro_topic == nullptr) { - PX4_ERR("failed to create sensor_baro publication"); - return -ENODEV; - } - - /* fill report structures */ - start(); - -out: - return ret; -} - -int -BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg) -{ - /* give it to the bus-specific superclass */ - // return bus_ioctl(filp, cmd, arg); - return VirtDevObj::devIOCTL(cmd, arg); -} - -void -BAROSIM::_measure() -{ - collect(); -} - -int -BAROSIM::collect() -{ - bool status; - - simulator::RawBaroData raw_baro; - - perf_begin(_sample_perf); - - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* read requested */ - Simulator *sim = Simulator::getInstance(); - - if (sim == nullptr) { - PX4_ERR("Error BAROSIM_DEV::transfer no simulator"); - return -ENODEV; - } - - PX4_DEBUG("BAROSIM_DEV::transfer getting sample"); - status = sim->getBaroSample((uint8_t *)(&raw_baro), sizeof(raw_baro)); - - if (!status) { - perf_count(_comms_errors); - perf_end(_sample_perf); - return -1; - } - - report.pressure = raw_baro.pressure; - report.temperature = raw_baro.temperature; - - /* fake device ID */ - report.device_id = 478459; - - int instance; - orb_publish_auto(ORB_ID(sensor_baro), &_baro_topic, &report, &instance, ORB_PRIO_DEFAULT); - - _reports->force(&report); - - perf_end(_sample_perf); - - return OK; -} - -void -BAROSIM::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_comms_errors); - PX4_INFO("poll interval: %u usec", m_sample_interval_usecs); - _reports->print_info("report queue"); - PX4_INFO("TEMP: %f", (double)report.temperature); - PX4_INFO("P: %.3f", (double)report.pressure); -} - -namespace barosim -{ - -/** - * Start the driver. - * - * This function call only returns once the driver - * is either successfully up and running or failed to start. - */ -static int -start() -{ - g_barosim = new BAROSIM(BAROSIM_DEV_PATH); - - if (g_barosim != nullptr && OK != g_barosim->init()) { - delete g_barosim; - g_barosim = nullptr; - PX4_ERR("bus init failed"); - return false; - } - - return 0; -} - -/** - * Print a little info about the driver. - */ -static int -info() -{ - if (g_barosim != nullptr) { - PX4_INFO("%s", BAROSIM_DEV_PATH); - g_barosim->print_info(); - } - - return 0; -} - -static void -usage() -{ - PX4_WARN("missing command: try 'start', 'info'"); -} - -}; // namespace barosim - -int -barosim_main(int argc, char *argv[]) -{ - int ret; - - if (argc < 2) { - barosim::usage(); - return 1; - } - - const char *verb = argv[1]; - - /* - * Start/load the driver. - */ - if (!strcmp(verb, "start")) { - ret = barosim::start(); - } - - /* - * Print driver information. - */ - else if (!strcmp(verb, "info")) { - ret = barosim::info(); - - } else { - barosim::usage(); - return 1; - } - - return ret; -} diff --git a/src/modules/simulator/barosim/barosim.h b/src/modules/simulator/barosim/barosim.h deleted file mode 100644 index 825e464a07f..00000000000 --- a/src/modules/simulator/barosim/barosim.h +++ /dev/null @@ -1,42 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 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 barosim.h - * - * A simulated Barometer. - */ -#pragma once - -#include "VirtDevObj.hpp" - diff --git a/src/modules/simulator/gyrosim/CMakeLists.txt b/src/modules/simulator/gyrosim/CMakeLists.txt deleted file mode 100644 index d9e26f6f13a..00000000000 --- a/src/modules/simulator/gyrosim/CMakeLists.txt +++ /dev/null @@ -1,46 +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 drivers__gyrosim - MAIN gyrosim - COMPILE_FLAGS - -Wno-double-promotion - -Wno-cast-align # TODO: fix and enable - -Wno-address-of-packed-member # TODO: fix in c_library_v2 - STACK_MAIN 1200 - SRCS - gyrosim.cpp - DEPENDS - modules__simulator -) diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp deleted file mode 100644 index 1b46283944b..00000000000 --- a/src/modules/simulator/gyrosim/gyrosim.cpp +++ /dev/null @@ -1,1291 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-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. - * - ****************************************************************************/ - -/** - * @file gyrosim.cpp - * - * Driver for the simulated gyro - * - * @author Andrew Tridgell - * @author Pat Hickey - * @author Mark Charlebois - */ - -#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 -#include -#include -#include - -#include "VirtDevObj.hpp" - -using namespace DriverFramework; - -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -#define MPU_DEVICE_PATH_ACCEL "/dev/gyrosim_accel" -#define MPU_DEVICE_PATH_GYRO "/dev/gyrosim_gyro" - -// MPU 6000 registers -#define MPUREG_SMPLRT_DIV 0x19 -#define MPUREG_CONFIG 0x1A -#define MPUREG_GYRO_CONFIG 0x1B -#define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_INT_STATUS 0x3A -#define MPUREG_PRODUCT_ID 0x0C - -// Product ID Description for GYROSIM -// high 4 bits low 4 bits -// Product Name Product Revision -#define GYROSIMES_REV_C4 0x14 - -#define GYROSIM_ACCEL_DEFAULT_RATE 400 - -#define GYROSIM_GYRO_DEFAULT_RATE 400 - -#ifdef PX4_SPI_BUS_EXT -#define EXTERNAL_BUS PX4_SPI_BUS_EXT -#else -#define EXTERNAL_BUS 0 -#endif - -/* - the GYROSIM can only handle high SPI bus speeds on the sensor and - interrupt status registers. All other registers have a maximum 1MHz - SPI speed - */ -class GYROSIM_gyro; - -class GYROSIM : public VirtDevObj -{ -public: - GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation); - virtual ~GYROSIM(); - - int init(); - virtual int start(); - - virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, unsigned long arg); - int transfer(uint8_t *send, uint8_t *recv, unsigned len); - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_info(); - - void print_registers(); - -protected: - friend class GYROSIM_gyro; - - virtual ssize_t gyro_read(void *buffer, size_t buflen); - virtual int gyro_ioctl(unsigned long cmd, unsigned long arg); - -private: - GYROSIM_gyro *_gyro; - uint8_t _product; /** product code */ - - unsigned _call_interval; - - ringbuffer::RingBuffer *_accel_reports; - - struct accel_calibration_s _accel_scale; - float _accel_range_scale; - float _accel_range_m_s2; - orb_advert_t _accel_topic; - int _accel_orb_class_instance; - - ringbuffer::RingBuffer *_gyro_reports; - - struct gyro_calibration_s _gyro_scale; - float _gyro_range_scale; - - perf_counter_t _accel_reads; - perf_counter_t _gyro_reads; - perf_counter_t _sample_perf; - perf_counter_t _good_transfers; - perf_counter_t _reset_retries; - - Integrator _accel_int; - Integrator _gyro_int; - - // last temperature reading for print_info() - float _last_temperature; - - /** - * Reset chip. - * - * Resets the chip and measurements ranges, but not scale and offset. - */ - int reset(); - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - virtual void _measure(); - - /** - * Read a register from the GYROSIM - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the GYROSIM - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Set the GYROSIM measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_accel_range(unsigned max_g); - - /** - * Swap a 16-bit value read from the GYROSIM to native byte order. - */ - uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } - - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - - /* - set sample rate (approximate) - 1kHz to 5Hz - */ - void _set_sample_rate(unsigned desired_sample_rate_hz); - - /* do not allow to copy this class due to pointer data members */ - GYROSIM(const GYROSIM &) = delete; - GYROSIM operator=(const GYROSIM &) = delete; - -#pragma pack(push, 1) - /** - * Report conversation within the GYROSIM, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - float accel_x; - float accel_y; - float accel_z; - float temp; - float gyro_x; - float gyro_y; - float gyro_z; - }; -#pragma pack(pop) - - uint8_t _regdata[108]; - -}; - -/** - * Helper class implementing the gyro driver node. - */ -class GYROSIM_gyro : public VirtDevObj -{ -public: - GYROSIM_gyro(GYROSIM *parent, const char *path); - virtual ~GYROSIM_gyro() = default; - - virtual ssize_t devRead(void *buffer, size_t buflen); - virtual int devIOCTL(unsigned long cmd, unsigned long arg); - - virtual int init(); - -protected: - friend class GYROSIM; - - virtual void _measure() {} -private: - GYROSIM *_parent; - orb_advert_t _gyro_topic; - int _gyro_orb_class_instance; - - /* do not allow to copy this class due to pointer data members */ - GYROSIM_gyro(const GYROSIM_gyro &) = delete; - GYROSIM_gyro operator=(const GYROSIM_gyro &) = delete; -}; - -/** driver 'main' command */ -extern "C" { __EXPORT int gyrosim_main(int argc, char *argv[]); } - -GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation rotation) : - VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), - _gyro(new GYROSIM_gyro(this, path_gyro)), - _product(GYROSIMES_REV_C4), - _accel_reports(nullptr), - _accel_scale{}, - _accel_range_scale(0.0f), - _accel_range_m_s2(0.0f), - _accel_topic(nullptr), - _accel_orb_class_instance(-1), - _gyro_reports(nullptr), - _gyro_scale{}, - _gyro_range_scale(0.0f), - _accel_reads(perf_alloc(PC_COUNT, "gyrosim_accel_read")), - _gyro_reads(perf_alloc(PC_COUNT, "gyrosim_gyro_read")), - _sample_perf(perf_alloc(PC_ELAPSED, "gyrosim_read")), - _good_transfers(perf_alloc(PC_COUNT, "gyrosim_good_transfers")), - _reset_retries(perf_alloc(PC_COUNT, "gyrosim_reset_retries")), - _accel_int(1000000 / GYROSIM_ACCEL_DEFAULT_RATE, true), - _gyro_int(1000000 / GYROSIM_GYRO_DEFAULT_RATE, true), - _last_temperature(0) -{ - - m_id.dev_id_s.bus = 1; - m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_GYROSIM; - - /* Prime _gyro with parents devid. */ - _gyro->m_id.dev_id = m_id.dev_id; - _gyro->m_id.dev_id_s.devtype = DRV_GYR_DEVTYPE_GYROSIM; - - // default accel scale factors - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - // default gyro scale factors - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; -} - -GYROSIM::~GYROSIM() -{ - /* make sure we are truly inactive */ - stop(); - - /* delete the gyro subdriver */ - delete _gyro; - - /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; - } - - if (_gyro_reports != nullptr) { - delete _gyro_reports; - } - - /* delete the perf counter */ - perf_free(_sample_perf); - perf_free(_accel_reads); - perf_free(_gyro_reads); - perf_free(_good_transfers); -} - -int -GYROSIM::init() -{ - int ret = VirtDevObj::init(); - - if (ret != 0) { - PX4_WARN("Base class init failed"); - return ret; - } - - /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - - if (_accel_reports == nullptr) { - PX4_WARN("_accel_reports creation failed"); - return -ENOMEM; - } - - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - - if (_gyro_reports == nullptr) { - PX4_WARN("_gyro_reports creation failed"); - return -ENOMEM; - } - - if (reset() != OK) { - PX4_WARN("reset failed"); - return PX4_ERROR; - } - - /* Initialize offsets and scales */ - _accel_scale.x_offset = 0; - _accel_scale.x_scale = 1.0f; - _accel_scale.y_offset = 0; - _accel_scale.y_scale = 1.0f; - _accel_scale.z_offset = 0; - _accel_scale.z_scale = 1.0f; - - _gyro_scale.x_offset = 0; - _gyro_scale.x_scale = 1.0f; - _gyro_scale.y_offset = 0; - _gyro_scale.y_scale = 1.0f; - _gyro_scale.z_offset = 0; - _gyro_scale.z_scale = 1.0f; - - /* do init for the gyro device node, keep it optional */ - ret = _gyro->init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - PX4_ERR("gyro init failed"); - return ret; - } - - ret = start(); - - if (ret != OK) { - PX4_ERR("gyro accel start failed (%d)", ret); - return ret; - } - - return PX4_OK; -} - -int GYROSIM::reset() -{ - return OK; -} - -int -GYROSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) -{ - uint8_t cmd = send[0]; - uint8_t reg = cmd & 0x7F; - const uint8_t MPUREAD = MPUREG_INT_STATUS | DIR_READ; - - if (cmd == MPUREAD) { - // Get data from the simulator - Simulator *sim = Simulator::getInstance(); - - if (sim == nullptr) { - PX4_WARN("failed accessing simulator"); - return ENODEV; - } - - // FIXME - not sure what interrupt status should be - recv[1] = 0; - - // skip cmd and status bytes - if (len > 2) { - sim->getMPUReport(&recv[2], len - 2); - } - - } else if (cmd & DIR_READ) { - PX4_DEBUG("Reading %u bytes from register %u", len - 1, reg); - memcpy(&_regdata[reg - MPUREG_PRODUCT_ID], &send[1], len - 1); - - } else { - PX4_DEBUG("Writing %u bytes to register %u", len - 1, reg); - - if (recv) { - memcpy(&recv[1], &_regdata[reg - MPUREG_PRODUCT_ID], len - 1); - } - } - - return PX4_OK; -} - -/* - set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro -*/ -void -GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) -{ - PX4_DEBUG("_set_sample_rate %u Hz", desired_sample_rate_hz); - - if (desired_sample_rate_hz == 0) { - desired_sample_rate_hz = GYROSIM_GYRO_DEFAULT_RATE; - } - - uint8_t div = 1000 / desired_sample_rate_hz; - - if (div > 200) { div = 200; } - - if (div < 1) { div = 1; } - - // This does nothing in the simulator but writes the value in the "register" so - // register dumps look correct - write_reg(MPUREG_SMPLRT_DIV, div - 1); - - unsigned sample_rate = 1000 / div; - PX4_DEBUG("Changed sample rate to %uHz", sample_rate); - setSampleInterval(1000000 / sample_rate); - _gyro->setSampleInterval(1000000 / sample_rate); -} - -ssize_t -GYROSIM::devRead(void *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_accel_s); - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ - if (_call_interval == 0) { - _accel_reports->flush(); - _measure(); - } - - /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { - return -EAGAIN; - } - - perf_count(_accel_reads); - - /* copy reports out of our buffer to the caller */ - sensor_accel_s *arp = reinterpret_cast(buffer); - int transferred = 0; - - while (count--) { - if (!_accel_reports->get(arp)) { - break; - } - - transferred++; - arp++; - } - - /* return the number of bytes transferred */ - return (transferred * sizeof(sensor_accel_s)); -} - -int -GYROSIM::self_test() -{ - if (perf_event_count(_sample_perf) == 0) { - _measure(); - } - - /* return 0 on success, 1 else */ - return (perf_event_count(_sample_perf) > 0) ? 0 : 1; -} - -ssize_t -GYROSIM::gyro_read(void *buffer, size_t buflen) -{ - unsigned count = buflen / sizeof(sensor_gyro_s); - - /* buffer must be large enough */ - if (count < 1) { - return -ENOSPC; - } - - /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ - if (_call_interval == 0) { - _gyro_reports->flush(); - _measure(); - } - - /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { - return -EAGAIN; - } - - perf_count(_gyro_reads); - - /* copy reports out of our buffer to the caller */ - sensor_gyro_s *grp = reinterpret_cast(buffer); - int transferred = 0; - - while (count--) { - if (!_gyro_reports->get(grp)) { - break; - } - - transferred++; - grp++; - } - - /* return the number of bytes transferred */ - return (transferred * sizeof(sensor_gyro_s)); -} - -int -GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) -{ - - switch (cmd) { - - case SENSORIOCRESET: - return reset(); - - case SENSORIOCSPOLLRATE: { - switch (arg) { - - /* zero would be bad */ - case 0: - return -EINVAL; - - /* set default polling rate */ - case SENSOR_POLLRATE_DEFAULT: - return devIOCTL(SENSORIOCSPOLLRATE, GYROSIM_ACCEL_DEFAULT_RATE); - - /* adjust to a legal polling interval in Hz */ - default: { - /* convert hz to hrt interval via microseconds */ - unsigned ticks = 1000000 / arg; - - /* check against maximum sane rate */ - if (ticks < 1000) { - return -EINVAL; - } - - /* update interval for next measurement */ - _call_interval = ticks; - - /* do we need to start internal polling? */ - bool want_start = (_call_interval == 0); - - /* if we need to start the poll state machine, do it */ - if (want_start) { - start(); - } - - return OK; - } - } - } - - case ACCELIOCSSCALE: { - /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; - float sum = s->x_scale + s->y_scale + s->z_scale; - - if (sum > 2.0f && sum < 4.0f) { - memcpy(&_accel_scale, s, sizeof(_accel_scale)); - return OK; - - } else { - return -EINVAL; - } - } - - default: - /* give it to the superclass */ - return VirtDevObj::devIOCTL(cmd, arg); - } -} - -int -GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg) -{ - switch (cmd) { - - /* these are shared with the accel side */ - case SENSORIOCSPOLLRATE: - case SENSORIOCRESET: - return devIOCTL(cmd, arg); - - case GYROIOCSSCALE: - /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); - return OK; - - default: - /* give it to the superclass */ - return VirtDevObj::devIOCTL(cmd, arg); - } -} - -uint8_t -GYROSIM::read_reg(unsigned reg) -{ - uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; - - // general register transfer at low clock speed - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -GYROSIM::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - // general register transfer at low clock speed - transfer(cmd, nullptr, sizeof(cmd)); -} - -int -GYROSIM::set_accel_range(unsigned max_g_in) -{ - // workaround for bugged versions of MPU6k (rev C) - switch (_product) { - case GYROSIMES_REV_C4: - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); - _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; - return OK; - } - - uint8_t afs_sel; - float lsb_per_g; - float max_accel_g; - - if (max_g_in > 8) { // 16g - AFS_SEL = 3 - afs_sel = 3; - lsb_per_g = 2048; - max_accel_g = 16; - - } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 - afs_sel = 2; - lsb_per_g = 4096; - max_accel_g = 8; - - } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 - afs_sel = 1; - lsb_per_g = 8192; - max_accel_g = 4; - - } else { // 2g - AFS_SEL = 0 - afs_sel = 0; - lsb_per_g = 16384; - max_accel_g = 2; - } - - write_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); - _accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g); - _accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G; - - return OK; -} - -int -GYROSIM::start() -{ - /* make sure we are stopped first */ - stop(); - - /* discard any stale data in the buffers */ - _accel_reports->flush(); - _gyro_reports->flush(); - - /* start polling at the specified rate */ - return DevObj::start(); -} - -void -GYROSIM::_measure() -{ - -#if 0 - static int x = 0; - - // Verify the samples are being taken at the expected rate - if (x == 99) { - x = 0; - PX4_INFO("GYROSIM::measure %" PRIu64, hrt_absolute_time()); - - } else { - x++; - } - -#endif - struct MPUReport mpu_report = {}; - - /* start measuring */ - perf_begin(_sample_perf); - - /* - * Fetch the full set of measurements from the GYROSIM in one pass. - */ - mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; - - // sensor transfer at high clock speed - //set_frequency(GYROSIM_HIGH_BUS_SPEED); - if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) { - return; - } - - /* - * Report buffers. - */ - sensor_accel_s arb = {}; - sensor_gyro_s grb = {}; - - // for now use local time but this should be the timestamp of the simulator - grb.timestamp = hrt_absolute_time(); - arb.timestamp = grb.timestamp; - // report the error count as the sum of the number of bad - // transfers and bad register reads. This allows the higher - // level code to decide if it should use this sensor based on - // whether it has had failures - grb.error_count = arb.error_count = 0; // FIXME - - /* - * 1) Scale raw value to SI units using scaling from datasheet. - * 2) Subtract static offset (in SI units) - * 3) Scale the statically calibrated values with a linear - * dynamically obtained factor - * - * Note: the static sensor offset is the number the sensor outputs - * at a nominally 'zero' input. Therefore the offset has to - * be subtracted. - * - * Example: A gyro outputs a value of 74 at zero angular rate - * the offset is 74 from the origin and subtracting - * 74 from all measurements centers them around zero. - */ - - - /* NOTE: Axes have been swapped to match the board a few lines above. */ - - if (math::isZero(_accel_range_scale)) { - _accel_range_scale = FLT_EPSILON; - } - - arb.x_raw = math::constrainFloatToInt16(mpu_report.accel_x / _accel_range_scale); - arb.y_raw = math::constrainFloatToInt16(mpu_report.accel_y / _accel_range_scale); - arb.z_raw = math::constrainFloatToInt16(mpu_report.accel_z / _accel_range_scale); - - arb.scaling = _accel_range_scale; - - _last_temperature = mpu_report.temp; - - arb.temperature = _last_temperature; - - arb.x = mpu_report.accel_x; - arb.y = mpu_report.accel_y; - arb.z = mpu_report.accel_z; - - matrix::Vector3f aval(mpu_report.accel_x, mpu_report.accel_y, mpu_report.accel_z); - matrix::Vector3f aval_integrated; - - bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt); - arb.x_integral = aval_integrated(0); - arb.y_integral = aval_integrated(1); - arb.z_integral = aval_integrated(2); - - /* fake device ID */ - arb.device_id = 1376264; - - if (math::isZero(_gyro_range_scale)) { - _gyro_range_scale = FLT_EPSILON; - } - - grb.x_raw = math::constrainFloatToInt16(mpu_report.gyro_x / _gyro_range_scale); - grb.y_raw = math::constrainFloatToInt16(mpu_report.gyro_y / _gyro_range_scale); - grb.z_raw = math::constrainFloatToInt16(mpu_report.gyro_z / _gyro_range_scale); - - grb.scaling = _gyro_range_scale; - - grb.temperature = _last_temperature; - - grb.x = mpu_report.gyro_x; - grb.y = mpu_report.gyro_y; - grb.z = mpu_report.gyro_z; - - matrix::Vector3f gval(mpu_report.gyro_x, mpu_report.gyro_y, mpu_report.gyro_z); - matrix::Vector3f gval_integrated; - - bool gyro_notify = _gyro_int.put(grb.timestamp, gval, gval_integrated, grb.integral_dt); - grb.x_integral = gval_integrated(0); - grb.y_integral = gval_integrated(1); - grb.z_integral = gval_integrated(2); - - /* fake device ID */ - grb.device_id = 2293768; - - _accel_reports->force(&arb); - _gyro_reports->force(&grb); - - if (accel_notify) { - orb_publish_auto(ORB_ID(sensor_accel), &_accel_topic, &arb, &_accel_orb_class_instance, ORB_PRIO_HIGH); - } - - if (gyro_notify) { - orb_publish_auto(ORB_ID(sensor_gyro), &_gyro->_gyro_topic, &grb, &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); - } - - /* stop measuring */ - perf_end(_sample_perf); -} - -void -GYROSIM::print_info() -{ - perf_print_counter(_sample_perf); - perf_print_counter(_accel_reads); - perf_print_counter(_gyro_reads); - perf_print_counter(_good_transfers); - perf_print_counter(_reset_retries); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); - PX4_INFO("temperature: %.1f", (double)_last_temperature); -} - -void -GYROSIM::print_registers() -{ - char buf[6 * 13 + 1]; - int i = 0; - - buf[0] = '\0'; - PX4_INFO("GYROSIM registers"); - - for (uint8_t reg = MPUREG_PRODUCT_ID; reg <= 108; reg++) { - uint8_t v = read_reg(reg); - sprintf(&buf[i * 6], "%02x:%02x ", (unsigned)reg, (unsigned)v); - i++; - - if ((i + 1) % 13 == 0) { - PX4_INFO("%s", buf); - i = 0; - buf[i] = '\0'; - } - } - - PX4_INFO("%s", buf); -} - - -GYROSIM_gyro::GYROSIM_gyro(GYROSIM *parent, const char *path) : - // Set sample interval to 0 since device is read by parent - VirtDevObj("GYROSIM_gyro", path, GYRO_BASE_DEVICE_PATH, 0), - _parent(parent), - _gyro_topic(nullptr), - _gyro_orb_class_instance(-1) -{ -} - - -int -GYROSIM_gyro::init() -{ - int ret = VirtDevObj::init(); - PX4_DEBUG("GYROSIM_gyro::init base class ret: %d", ret); - return ret; -} - -ssize_t -GYROSIM_gyro::devRead(void *buffer, size_t buflen) -{ - return _parent->gyro_read(buffer, buflen); -} - -int -GYROSIM_gyro::devIOCTL(unsigned long cmd, unsigned long arg) -{ - - switch (cmd) { - case DEVIOCGDEVICEID: - return (int)VirtDevObj::devIOCTL(cmd, arg); - break; - - default: - return _parent->gyro_ioctl(cmd, arg); - } -} - -/** - * Local functions in support of the shell command. - */ -namespace gyrosim -{ - -GYROSIM *g_dev_sim; // on simulated bus - -int start(enum Rotation /*rotation*/); -int stop(); -int test(); -int reset(); -int info(); -int regdump(); -void usage(); - -/** - * Start the driver. - * - * This function only returns if the driver is up and running - * or failed to detect the sensor. - */ -int -start(enum Rotation rotation) -{ - GYROSIM **g_dev_ptr = &g_dev_sim; - const char *path_accel = MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = MPU_DEVICE_PATH_GYRO; - DevHandle h; - - if (*g_dev_ptr != nullptr) { - /* if already started, the still command succeeded */ - PX4_WARN("already started"); - return 0; - } - - /* create the driver */ - *g_dev_ptr = new GYROSIM(path_accel, path_gyro, rotation); - - if (*g_dev_ptr == nullptr) { - goto fail; - } - - if (OK != (*g_dev_ptr)->init()) { - goto fail; - } - - /* set the poll rate to default, starts automatic data collection */ - DevMgr::getHandle(path_accel, h); - - if (!h.isValid()) { - goto fail; - } - - if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - DevMgr::releaseHandle(h); - goto fail; - } - - DevMgr::releaseHandle(h); - return 0; -fail: - - if (*g_dev_ptr != nullptr) { - delete *g_dev_ptr; - *g_dev_ptr = nullptr; - } - - PX4_WARN("driver start failed"); - return 1; -} - -int -stop() -{ - GYROSIM **g_dev_ptr = &g_dev_sim; - - if (*g_dev_ptr != nullptr) { - delete *g_dev_ptr; - *g_dev_ptr = nullptr; - - } else { - /* warn, but not an error */ - PX4_WARN("already stopped."); - } - - return 0; -} - -/** - * Perform some basic functional tests on the driver; - * make sure we can collect data from the sensor in polled - * and automatic modes. - */ -int -test() -{ - const char *path_accel = MPU_DEVICE_PATH_ACCEL; - const char *path_gyro = MPU_DEVICE_PATH_GYRO; - sensor_accel_s a_report; - sensor_gyro_s g_report; - ssize_t sz; - - /* get the driver */ - DevHandle h_accel; - DevMgr::getHandle(path_accel, h_accel); - - if (!h_accel.isValid()) { - PX4_ERR("%s open failed (try 'gyrosim start')", path_accel); - return 1; - } - - /* get the driver */ - DevHandle h_gyro; - DevMgr::getHandle(path_gyro, h_gyro); - - if (!h_gyro.isValid()) { - PX4_ERR("%s open failed", path_gyro); - return 1; - } - - /* do a simple demand read */ - sz = h_accel.read(&a_report, sizeof(a_report)); - - if (sz != sizeof(a_report)) { - PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(a_report)); - PX4_ERR("immediate acc read failed"); - return 1; - } - - PX4_INFO("single read"); - PX4_INFO("time: %lld", (long long)a_report.timestamp); - PX4_INFO("acc x: \t%8.4f\tm/s^2", (double)a_report.x); - PX4_INFO("acc y: \t%8.4f\tm/s^2", (double)a_report.y); - PX4_INFO("acc z: \t%8.4f\tm/s^2", (double)a_report.z); - PX4_INFO("acc x: \t%d\traw 0x%0x", (short)a_report.x_raw, (unsigned short)a_report.x_raw); - PX4_INFO("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); - PX4_INFO("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); - - /* do a simple demand read */ - sz = h_gyro.read(&g_report, sizeof(g_report)); - - if (sz != sizeof(g_report)) { - PX4_WARN("ret: %zd, expected: %zd", sz, sizeof(g_report)); - PX4_ERR("immediate gyro read failed"); - return 1; - } - - PX4_INFO("gyro x: \t% 9.5f\trad/s", (double)g_report.x); - PX4_INFO("gyro y: \t% 9.5f\trad/s", (double)g_report.y); - PX4_INFO("gyro z: \t% 9.5f\trad/s", (double)g_report.z); - PX4_INFO("gyro x: \t%d\traw", (int)g_report.x_raw); - PX4_INFO("gyro y: \t%d\traw", (int)g_report.y_raw); - PX4_INFO("gyro z: \t%d\traw", (int)g_report.z_raw); - - PX4_INFO("temp: \t%8.4f\tdeg celsius", (double)a_report.temperature); - - - /* XXX add poll-rate tests here too */ - - // Destructor would clean the up too - DevMgr::releaseHandle(h_accel); - DevMgr::releaseHandle(h_gyro); - reset(); - PX4_INFO("PASS"); - - return 0; -} - -/** - * Reset the driver. - */ -int -reset() -{ - DevHandle h; - DevMgr::getHandle(MPU_DEVICE_PATH_ACCEL, h); - - if (!h.isValid()) { - PX4_ERR("reset failed"); - return 1; - } - - - if (h.ioctl(SENSORIOCRESET, 0) < 0) { - PX4_ERR("driver reset failed"); - goto reset_fail; - } - - if (h.ioctl(SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { - PX4_ERR("driver poll restart failed"); - goto reset_fail; - } - - return 0; - -reset_fail: - return 1; -} - -/** - * Print a little info about the driver. - */ -int -info() -{ - GYROSIM **g_dev_ptr = &g_dev_sim; - - if (*g_dev_ptr == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_INFO("state @ %p", *g_dev_ptr); - (*g_dev_ptr)->print_info(); - unsigned dummy = 0; - PX4_INFO("device_id: %u", (unsigned int)(*g_dev_ptr)->devIOCTL(DEVIOCGDEVICEID, dummy)); - - return 0; -} - -/** - * Dump the register information - */ -int -regdump() -{ - GYROSIM **g_dev_ptr = &g_dev_sim; - - if (*g_dev_ptr == nullptr) { - PX4_ERR("driver not running"); - return 1; - } - - PX4_INFO("regdump @ %p", *g_dev_ptr); - (*g_dev_ptr)->print_registers(); - - return 0; -} - -void -usage() -{ - PX4_INFO("missing command: try 'start', 'info', 'test', 'stop', 'reset', 'regdump'"); - PX4_INFO("options:"); - PX4_INFO(" -R rotation"); -} - -} // namespace - -int -gyrosim_main(int argc, char *argv[]) -{ - int ch; - enum Rotation rotation = ROTATION_NONE; - int ret; - - /* jump over start/off/etc and look at options first */ - int myoptind = 1; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'R': - rotation = (enum Rotation)atoi(myoptarg); - break; - - default: - gyrosim::usage(); - return 0; - } - } - - if (myoptind >= argc) { - gyrosim::usage(); - return 1; - } - - const char *verb = argv[myoptind]; - - /* - * Start/load the driver. - - */ - if (!strcmp(verb, "start")) { - ret = gyrosim::start(rotation); - } - - else if (!strcmp(verb, "stop")) { - ret = gyrosim::stop(); - } - - /* - * Test the driver/device. - */ - else if (!strcmp(verb, "test")) { - ret = gyrosim::test(); - } - - /* - * Reset the driver. - */ - else if (!strcmp(verb, "reset")) { - ret = gyrosim::reset(); - } - - /* - * Print driver information. - */ - else if (!strcmp(verb, "info")) { - ret = gyrosim::info(); - } - - /* - * Print register information. - */ - else if (!strcmp(verb, "regdump")) { - ret = gyrosim::regdump(); - } - - else { - gyrosim::usage(); - ret = 1; - } - - return ret; -} diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index e1042c6470a..a7fde33cd63 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -65,66 +65,16 @@ Simulator *Simulator::getInstance() return _instance; } -bool Simulator::getMPUReport(uint8_t *buf, int len) -{ - return _mpu.copyData(buf, len); -} - -bool Simulator::getRawAccelReport(uint8_t *buf, int len) -{ - return _accel.copyData(buf, len); -} - -bool Simulator::getMagReport(uint8_t *buf, int len) -{ - return _mag.copyData(buf, len); -} - -bool Simulator::getBaroSample(uint8_t *buf, int len) -{ - return _baro.copyData(buf, len); -} - bool Simulator::getGPSSample(uint8_t *buf, int len) { return _gps.copyData(buf, len); } -bool Simulator::getAirspeedSample(uint8_t *buf, int len) -{ - return _airspeed.copyData(buf, len); -} - -void Simulator::write_MPU_data(void *buf) -{ - _mpu.writeData(buf); -} - -void Simulator::write_accel_data(void *buf) -{ - _accel.writeData(buf); -} - -void Simulator::write_mag_data(void *buf) -{ - _mag.writeData(buf); -} - -void Simulator::write_baro_data(void *buf) -{ - _baro.writeData(buf); -} - void Simulator::write_gps_data(void *buf) { _gps.writeData(buf); } -void Simulator::write_airspeed_data(void *buf) -{ - _airspeed.writeData(buf); -} - void Simulator::parameters_update(bool force) { bool updated; @@ -149,33 +99,21 @@ int Simulator::start(int argc, char *argv[]) if (_instance) { drv_led_start(); - if (argc == 5 && strcmp(argv[3], "-u") == 0) { + if (argc == 4 && strcmp(argv[2], "-u") == 0) { _instance->set_ip(InternetProtocol::UDP); - _instance->set_port(atoi(argv[4])); + _instance->set_port(atoi(argv[3])); } - if (argc == 5 && strcmp(argv[3], "-c") == 0) { + if (argc == 4 && strcmp(argv[2], "-c") == 0) { _instance->set_ip(InternetProtocol::TCP); - _instance->set_port(atoi(argv[4])); + _instance->set_port(atoi(argv[3])); } - if (argv[2][1] == 's') { - _instance->initialize_sensor_data(); #ifndef __PX4_QURT - // Update sensor data - _instance->poll_for_MAVLink_messages(); + // Update sensor data + _instance->poll_for_MAVLink_messages(); #endif - } else if (argv[2][1] == 'p') { - // Update sensor data - _instance->set_publish(true); - _instance->poll_for_MAVLink_messages(); - - } else { - _instance->initialize_sensor_data(); - _instance->_initialized = true; - } - return 0; } else { @@ -197,11 +135,9 @@ void Simulator::set_port(unsigned port) static void usage() { PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}"); - PX4_WARN("Simulate raw sensors: simulator start -s"); - PX4_WARN("Publish sensors combined: simulator start -p"); + PX4_WARN("Start simulator: simulator start"); PX4_WARN("Connect using UDP: simulator start -u udp_port"); PX4_WARN("Connect using TCP: simulator start -c tcp_port"); - PX4_WARN("Dummy unit test data: simulator start -t"); } __BEGIN_DECLS @@ -212,35 +148,27 @@ extern "C" { int simulator_main(int argc, char *argv[]) { - if (argc > 2 && strcmp(argv[1], "start") == 0) { - if (strcmp(argv[2], "-s") == 0 || - strcmp(argv[2], "-p") == 0 || - strcmp(argv[2], "-t") == 0) { + if (argc > 1 && strcmp(argv[1], "start") == 0) { - if (g_sim_task >= 0) { - PX4_WARN("Simulator already started"); - return 0; + if (g_sim_task >= 0) { + PX4_WARN("Simulator already started"); + return 0; + } + + g_sim_task = px4_task_spawn_cmd("simulator", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 1500, + Simulator::start, + argv); + + while (true) { + if (Simulator::getInstance()) { + break; + + } else { + system_sleep(1); } - - g_sim_task = px4_task_spawn_cmd("simulator", - SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, - 1500, - Simulator::start, - argv); - - while (true) { - if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) { - break; - - } else { - system_sleep(1); - } - } - - } else { - usage(); - return 1; } } else if (argc == 2 && strcmp(argv[1], "stop") == 0) { diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index ec7551ac9ed..a88fbd54cdc 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -42,80 +42,38 @@ #pragma once -#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 -#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 namespace simulator { -#pragma pack(push, 1) -struct RawAccelData { - float temperature; - float x; - float y; - float z; -}; -#pragma pack(pop) - -#pragma pack(push, 1) -struct RawMagData { - float temperature; - float x; - float y; - float z; -}; -#pragma pack(pop) - -#pragma pack(push, 1) -struct RawMPUData { - float accel_x; - float accel_y; - float accel_z; - float temp; - float gyro_x; - float gyro_y; - float gyro_z; -}; -#pragma pack(pop) - -#pragma pack(push, 1) -struct RawBaroData { - float pressure; - float temperature; -}; -#pragma pack(pop) - -#pragma pack(push, 1) -struct RawAirspeedData { - float temperature; - float diff_pressure; -}; -#pragma pack(pop) - #pragma pack(push, 1) struct RawGPSData { uint64_t timestamp; @@ -198,20 +156,6 @@ class Simulator : public ModuleParams public: static Simulator *getInstance(); - enum sim_dev_t { - SIM_GYRO, - SIM_ACCEL, - SIM_MAG - }; - - struct sample { - float x; - float y; - float z; - sample() : x(0), y(0), z(0) {} - sample(float a, float b, float c) : x(a), y(b), z(c) {} - }; - enum class InternetProtocol { TCP, UDP @@ -219,21 +163,9 @@ public: static int start(int argc, char *argv[]); - bool getAirspeedSample(uint8_t *buf, int len); - bool getBaroSample(uint8_t *buf, int len); bool getGPSSample(uint8_t *buf, int len); - bool getMagReport(uint8_t *buf, int len); - bool getMPUReport(uint8_t *buf, int len); - bool getRawAccelReport(uint8_t *buf, int len); - void write_airspeed_data(void *buf); - void write_accel_data(void *buf); - void write_baro_data(void *buf); void write_gps_data(void *buf); - void write_mag_data(void *buf); - void write_MPU_data(void *buf); - - bool isInitialized() { return _initialized; } void set_ip(InternetProtocol ip); void set_port(unsigned port); @@ -251,6 +183,9 @@ private: _param_sub = orb_subscribe(ORB_ID(parameter_update)); _battery_status.timestamp = hrt_absolute_time(); + + _px4_accel.set_sample_rate(250); + _px4_gyro.set_sample_rate(250); } ~Simulator() @@ -259,22 +194,15 @@ private: orb_unsubscribe(_param_sub); // free perf counters - perf_free(_perf_accel); - perf_free(_perf_airspeed); - perf_free(_perf_baro); perf_free(_perf_gps); - perf_free(_perf_mag); - perf_free(_perf_mpu); perf_free(_perf_sim_delay); perf_free(_perf_sim_interval); - _instance = NULL; + _instance = nullptr; } // class methods - void initialize_sensor_data(); - int publish_sensor_topics(const mavlink_hil_sensor_t *imu); int publish_flow_topic(const mavlink_hil_optical_flow_t *flow); int publish_odometry_topic(const mavlink_message_t *odom_mavlink); int publish_distance_topic(const mavlink_distance_sensor_t *dist); @@ -282,31 +210,23 @@ private: static Simulator *_instance; // simulated sensor instances - simulator::Report _airspeed{1}; - simulator::Report _accel{1}; - simulator::Report _baro{1}; - simulator::Report _gps{1}; - simulator::Report _mag{1}; - simulator::Report _mpu{1}; + PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{2294028, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + PX4Barometer _px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + + simulator::Report _gps{1}; - perf_counter_t _perf_accel{perf_alloc_once(PC_ELAPSED, "sim_accel_delay")}; - perf_counter_t _perf_airspeed{perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")}; - perf_counter_t _perf_baro{perf_alloc_once(PC_ELAPSED, "sim_baro_delay")}; perf_counter_t _perf_gps{perf_alloc_once(PC_ELAPSED, "sim_gps_delay")}; - perf_counter_t _perf_mag{perf_alloc_once(PC_ELAPSED, "sim_mag_delay")}; - perf_counter_t _perf_mpu{perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")}; perf_counter_t _perf_sim_delay{perf_alloc_once(PC_ELAPSED, "sim_network_delay")}; perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, "sim_network_interval")}; // uORB publisher handlers - orb_advert_t _accel_pub{nullptr}; - orb_advert_t _baro_pub{nullptr}; orb_advert_t _battery_pub{nullptr}; + orb_advert_t _differential_pressure_pub{nullptr}; orb_advert_t _dist_pub{nullptr}; orb_advert_t _flow_pub{nullptr}; - orb_advert_t _gyro_pub{nullptr}; orb_advert_t _irlock_report_pub{nullptr}; - orb_advert_t _mag_pub{nullptr}; orb_advert_t _visual_odometry_pub{nullptr}; int _param_sub{-1}; @@ -315,8 +235,6 @@ private: InternetProtocol _ip{InternetProtocol::UDP}; - bool _initialized{false}; - double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time hrt_abstime _last_sim_timestamp{0}; @@ -349,8 +267,7 @@ private: void send_controls(); void send_heartbeat(); void send_mavlink_message(const mavlink_message_t &aMsg); - void set_publish(const bool publish = true); - void update_sensors(const mavlink_hil_sensor_t *imu); + void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu); void update_gps(const mavlink_hil_gps_t *gps_sim); static void *sending_trampoline(void *); @@ -369,7 +286,6 @@ private: struct map_projection_reference_s _hil_local_proj_ref {}; bool _hil_local_proj_inited{false}; - bool _publish{false}; double _hil_ref_lat{0}; double _hil_ref_lon{0}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index e16890043f8..f11a8ea5904 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -220,51 +220,53 @@ static void fill_rc_input_msg(input_rc_s *rc, mavlink_rc_channels_t *rc_channels rc->values[17] = rc_channels->chan18_raw; } -void Simulator::update_sensors(const mavlink_hil_sensor_t *imu) +void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu) { - // write sensor data to memory so that drivers can copy data from there - RawMPUData mpu = {}; - mpu.accel_x = imu->xacc; - mpu.accel_y = imu->yacc; - mpu.accel_z = imu->zacc; - mpu.temp = imu->temperature; - mpu.gyro_x = imu->xgyro; - mpu.gyro_y = imu->ygyro; - mpu.gyro_z = imu->zgyro; + if ((imu.fields_updated & 0x1FFF) != 0x1FFF) { + PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x", imu.fields_updated); + } - write_MPU_data(&mpu); - perf_begin(_perf_mpu); + // gyro + { + static constexpr float scaling = 1000.0f; + _px4_gyro.set_scale(1 / scaling); + _px4_gyro.set_temperature(imu.temperature); + _px4_gyro.update(time, imu.xgyro * scaling, imu.ygyro * scaling, imu.zgyro * scaling); + } - RawAccelData accel = {}; - accel.x = imu->xacc; - accel.y = imu->yacc; - accel.z = imu->zacc; + // accel + { + static constexpr float scaling = 1000.0f; + _px4_accel.set_scale(1 / scaling); + _px4_accel.set_temperature(imu.temperature); + _px4_accel.update(time, imu.xacc * scaling, imu.yacc * scaling, imu.zacc * scaling); + } - write_accel_data(&accel); - perf_begin(_perf_accel); + // magnetometer + { + static constexpr float scaling = 1000.0f; + _px4_mag.set_scale(1 / scaling); + _px4_mag.set_temperature(imu.temperature); + _px4_mag.update(time, imu.xmag * scaling, imu.ymag * scaling, imu.zmag * scaling); + } - RawMagData mag = {}; - mag.x = imu->xmag; - mag.y = imu->ymag; - mag.z = imu->zmag; + // baro + { + _px4_baro.set_temperature(imu.temperature); + _px4_baro.update(time, imu.abs_pressure); + } - write_mag_data(&mag); - perf_begin(_perf_mag); + // differential pressure + { + differential_pressure_s report{}; + report.timestamp = time; + report.temperature = imu.temperature; + report.differential_pressure_filtered_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar; + report.differential_pressure_raw_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar; - RawBaroData baro = {}; - - // Get air pressure and pressure altitude - // valid for troposphere (below 11km AMSL) - baro.pressure = imu->abs_pressure; - baro.temperature = imu->temperature; - - write_baro_data(&baro); - - RawAirspeedData airspeed = {}; - airspeed.temperature = imu->temperature; - airspeed.diff_pressure = imu->diff_pressure; - - write_airspeed_data(&airspeed); + int instance; + orb_publish_auto(ORB_ID(differential_pressure), &_differential_pressure_pub, &report, &instance, ORB_PRIO_DEFAULT); + } } void Simulator::update_gps(const mavlink_hil_gps_t *gps_sim) @@ -340,10 +342,6 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) mavlink_hil_gps_t gps_sim; mavlink_msg_hil_gps_decode(msg, &gps_sim); - if (_publish) { - //PX4_WARN("FIXME: Need to publish GPS topic. Not done yet."); - } - update_gps(&gps_sim); } @@ -352,9 +350,6 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) mavlink_hil_sensor_t imu; mavlink_msg_hil_sensor_decode(msg, &imu); - // set temperature to a decent value - imu.temperature = 32.0f; - struct timespec ts; abstime_to_ts(&ts, imu.time_usec); px4_clock_settime(CLOCK_MONOTONIC, &ts); @@ -374,11 +369,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg) last_time = now_us; #endif - if (_publish) { - publish_sensor_topics(&imu); - } - - update_sensors(&imu); + update_sensors(now_us, imu); // battery simulation (limit update to 100Hz) if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { @@ -540,10 +531,8 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg) fill_rc_input_msg(&_rc_input, &rc_channels); // publish message - if (_publish) { - int rc_multi; - orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH); - } + int rc_multi; + orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH); } void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg) @@ -652,38 +641,6 @@ void Simulator::send_heartbeat() send_mavlink_message(message); } -void Simulator::initialize_sensor_data() -{ - // Write sensor data to memory so that drivers can copy data from there. - RawMPUData mpu = {}; - mpu.accel_z = CONSTANTS_ONE_G; - - write_MPU_data(&mpu); - - RawAccelData accel = {}; - accel.z = CONSTANTS_ONE_G; - - write_accel_data(&accel); - - RawMagData mag = {}; - mag.x = 0.4f; - mag.y = 0.0f; - mag.z = 0.6f; - - write_mag_data((void *)&mag); - - RawBaroData baro = {}; - // calculate air pressure from altitude (valid for low altitude) - baro.pressure = 120000.0f; - baro.temperature = 25.0f; - - write_baro_data(&baro); - - RawAirspeedData airspeed {}; - - write_airspeed_data(&airspeed); -} - void Simulator::poll_for_MAVLink_messages() { #ifdef __PX4_DARWIN @@ -812,8 +769,6 @@ void Simulator::poll_for_MAVLink_messages() // Request HIL_STATE_QUATERNION for ground truth. request_hil_state_quaternion(); - _initialized = true; - while (true) { // wait for new mavlink messages to arrive @@ -857,11 +812,7 @@ void Simulator::poll_for_MAVLink_messages() for (int i = 0; i < len; ++i) { if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { - // have a message, handle it - bool prev_publish = _publish; - set_publish(true); handle_message(&msg); - set_publish(prev_publish); } } } @@ -969,101 +920,6 @@ int openUart(const char *uart_name, int baud) } #endif -int Simulator::publish_sensor_topics(const mavlink_hil_sensor_t *imu) -{ - uint64_t timestamp = hrt_absolute_time(); - - if ((imu->fields_updated & 0x1FFF) != 0x1FFF) { - PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x", imu->fields_updated); - } - - /* - static int count=0; - static uint64_t last_timestamp=0; - count++; - if (!(count % 200)) { - PX4_WARN("TIME : %lu, dt: %lu", - (unsigned long) timestamp,(unsigned long) timestamp - (unsigned long) last_timestamp); - PX4_WARN("IMU : %f %f %f",imu->xgyro,imu->ygyro,imu->zgyro); - PX4_WARN("ACCEL: %f %f %f",imu->xacc,imu->yacc,imu->zacc); - PX4_WARN("MAG : %f %f %f",imu->xmag,imu->ymag,imu->zmag); - PX4_WARN("BARO : %f %f %f",imu->abs_pressure,imu->pressure_alt,imu->temperature); - } - last_timestamp = timestamp; - */ - /* gyro */ - { - sensor_gyro_s gyro = {}; - - gyro.timestamp = timestamp; - gyro.device_id = 2293768; - gyro.x_raw = imu->xgyro * 1000.0f; - gyro.y_raw = imu->ygyro * 1000.0f; - gyro.z_raw = imu->zgyro * 1000.0f; - gyro.x = imu->xgyro; - gyro.y = imu->ygyro; - gyro.z = imu->zgyro; - - gyro.temperature = imu->temperature; - - int gyro_multi; - orb_publish_auto(ORB_ID(sensor_gyro), &_gyro_pub, &gyro, &gyro_multi, ORB_PRIO_HIGH); - } - - /* accelerometer */ - { - sensor_accel_s accel = {}; - - accel.timestamp = timestamp; - accel.device_id = 1376264; - accel.x_raw = imu->xacc / (CONSTANTS_ONE_G / 1000.0f); - accel.y_raw = imu->yacc / (CONSTANTS_ONE_G / 1000.0f); - accel.z_raw = imu->zacc / (CONSTANTS_ONE_G / 1000.0f); - accel.x = imu->xacc; - accel.y = imu->yacc; - accel.z = imu->zacc; - - accel.temperature = imu->temperature; - - int accel_multi; - orb_publish_auto(ORB_ID(sensor_accel), &_accel_pub, &accel, &accel_multi, ORB_PRIO_HIGH); - } - - /* magnetometer */ - { - struct mag_report mag = {}; - - mag.timestamp = timestamp; - mag.device_id = 196616; - mag.x_raw = imu->xmag * 1000.0f; - mag.y_raw = imu->ymag * 1000.0f; - mag.z_raw = imu->zmag * 1000.0f; - mag.x = imu->xmag; - mag.y = imu->ymag; - mag.z = imu->zmag; - - mag.temperature = imu->temperature; - - int mag_multi; - orb_publish_auto(ORB_ID(sensor_mag), &_mag_pub, &mag, &mag_multi, ORB_PRIO_HIGH); - } - - /* baro */ - { - sensor_baro_s baro = {}; - - baro.timestamp = timestamp; - baro.device_id = 478459; - baro.pressure = imu->abs_pressure; - baro.temperature = imu->temperature; - - int baro_multi; - orb_publish_auto(ORB_ID(sensor_baro), &_baro_pub, &baro, &baro_multi, ORB_PRIO_HIGH); - } - - return OK; -} - int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink) { uint64_t timestamp = hrt_absolute_time(); @@ -1240,8 +1096,3 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl return OK; } - -void Simulator::set_publish(const bool publish) -{ - _publish = publish; -}