dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

simulator move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer, PX4Barometer helpers (#12081)

This commit is contained in:
Daniel Agar 2019-05-30 21:07:26 -04:00 committed by GitHub
parent 4a4d323a97
commit 43e3fc707d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
30 changed files with 156 additions and 4460 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
ROMFS/px4fmu_common/init.d-posix/rcS
View File

@ -94,15 +94,16 @@ then
param set BAT_N_CELLS 3 param set BAT_N_CELLS 3
param set CAL_ACC0_ID 1376264 param set CAL_ACC0_ID 1311244
param set CAL_ACC1_ID 1310728 param set CAL_ACC_PRIME 1311244
param set CAL_ACC_PRIME 1376264
param set CAL_GYRO0_ID 2293768 param set CAL_GYRO0_ID 2294028
param set CAL_GYRO_PRIME 2293768 param set CAL_GYRO_PRIME 2294028
param set CAL_MAG0_ID 196616 param set CAL_MAG0_ID 197388
param set CAL_MAG_PRIME 196616 param set CAL_MAG_PRIME 197388
param set CAL_BARO_PRIME 6620172
param set CBRK_AIRSPD_CHK 0 param set CBRK_AIRSPD_CHK 0
@ -201,11 +202,8 @@ sh "$autostart_file"
dataman start dataman start
replay tryapplyparams replay tryapplyparams
simulator start -s -c $simulator_tcp_port simulator start -c $simulator_tcp_port
tone_alarm start tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start gpssim start
sensors start sensors start
commander start commander start
@ -227,15 +225,6 @@ then
camera_feedback start camera_feedback start
fi 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. # Configure vehicle type specific parameters.
# Note: rc.vehicle_setup is the entry point for rc.interface, # Note: rc.vehicle_setup is the entry point for rc.interface,
# rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps. # rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps.

6
posix-configs/SITL/init/test/cmd_template.in
View File

@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0
dataman start dataman start
simulator start -t simulator start
tone_alarm start tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start gpssim start
measairspeedsim start
pwm_out_sim start pwm_out_sim start
ver all ver all

6
posix-configs/SITL/init/test/test_mavlink
View File

@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0
dataman start dataman start
simulator start -t simulator start
tone_alarm start tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start gpssim start
measairspeedsim start
pwm_out_sim start pwm_out_sim start
ver all ver all

10
posix-configs/SITL/init/test/test_shutdown
View File

@ -14,13 +14,9 @@ param set SYS_RESTART_TYPE 0
dataman start dataman start
simulator start -t simulator start
tone_alarm start tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start gpssim start
measairspeedsim start
pwm_out_sim start pwm_out_sim start
sensors start sensors start
@ -83,11 +79,7 @@ sleep 2
simulator stop simulator stop
tone_alarm stop tone_alarm stop
gyrosim stop
#accelsim stop
#barosim stop
gpssim stop gpssim stop
measairspeedsim stop
dataman stop dataman stop
#uorb stop #uorb stop

6
posix-configs/SITL/init/test/test_template.in
View File

@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0
dataman start dataman start
simulator start -t simulator start
tone_alarm start tone_alarm start
gyrosim start
accelsim start
barosim start
gpssim start gpssim start
measairspeedsim start
pwm_out_sim start pwm_out_sim start
ver all ver all

2
posix-configs/eagle/hil/mainapphil.config
View File

@ -8,4 +8,4 @@ sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50 mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink boot_complete mavlink boot_complete
simulator start -p simulator start

4
src/drivers/drv_sensor.h
View File

@ -54,7 +54,7 @@
#define DRV_MAG_DEVTYPE_HMC5883 0x01 #define DRV_MAG_DEVTYPE_HMC5883 0x01
#define DRV_MAG_DEVTYPE_LSM303D 0x02 #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_MPU9250 0x04
#define DRV_MAG_DEVTYPE_LIS3MDL 0x05 #define DRV_MAG_DEVTYPE_LIS3MDL 0x05
#define DRV_MAG_DEVTYPE_IST8310 0x06 #define DRV_MAG_DEVTYPE_IST8310 0x06
@ -65,7 +65,6 @@
#define DRV_ACC_DEVTYPE_BMA180 0x12 #define DRV_ACC_DEVTYPE_BMA180 0x12
#define DRV_ACC_DEVTYPE_MPU6000 0x13 #define DRV_ACC_DEVTYPE_MPU6000 0x13
#define DRV_ACC_DEVTYPE_ACCELSIM 0x14 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14
#define DRV_ACC_DEVTYPE_GYROSIM 0x15
#define DRV_ACC_DEVTYPE_MPU9250 0x16 #define DRV_ACC_DEVTYPE_MPU9250 0x16
#define DRV_ACC_DEVTYPE_BMI160 0x17 #define DRV_ACC_DEVTYPE_BMI160 0x17
#define DRV_GYR_DEVTYPE_MPU6000 0x21 #define DRV_GYR_DEVTYPE_MPU6000 0x21
@ -113,6 +112,7 @@
#define DRV_MAG_DEVTYPE_LSM303AGR 0x62 #define DRV_MAG_DEVTYPE_LSM303AGR 0x62
#define DRV_ACC_DEVTYPE_ADIS16497 0x63 #define DRV_ACC_DEVTYPE_ADIS16497 0x63
#define DRV_GYR_DEVTYPE_ADIS16497 0x64 #define DRV_GYR_DEVTYPE_ADIS16497 0x64
#define DRV_BARO_DEVTYPE_BAROSIM 0x65
/* /*
* ioctl() definitions * ioctl() definitions

1
src/lib/drivers/CMakeLists.txt
View File

@ -33,6 +33,7 @@
add_subdirectory(accelerometer) add_subdirectory(accelerometer)
add_subdirectory(airspeed) add_subdirectory(airspeed)
add_subdirectory(barometer)
add_subdirectory(device) add_subdirectory(device)
add_subdirectory(gyroscope) add_subdirectory(gyroscope)
add_subdirectory(led) add_subdirectory(led)

1
src/lib/drivers/accelerometer/CMakeLists.txt
View File

@ -32,3 +32,4 @@
############################################################################ ############################################################################
px4_add_library(drivers_accelerometer PX4Accelerometer.cpp) px4_add_library(drivers_accelerometer PX4Accelerometer.cpp)
target_link_libraries(drivers_accelerometer PRIVATE drivers__device)

6
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
View File

@ -98,6 +98,12 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
_sensor_accel_pub.get().device_id = device_id.devid; _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) void PX4Accelerometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
{ {
sensor_accel_s &report = _sensor_accel_pub.get(); sensor_accel_s &report = _sensor_accel_pub.get();

10
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
View File

@ -47,7 +47,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
{ {
public: 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; ~PX4Accelerometer() override;
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) 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_scale(float scale) { _sensor_accel_pub.get().scaling = scale; }
void set_temperature(float temperature) { _sensor_accel_pub.get().temperature = temperature; } 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 set_sample_rate(unsigned rate);
void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z); void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
@ -67,6 +65,8 @@ public:
private: private:
void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
uORB::Publication<sensor_accel_s> _sensor_accel_pub; uORB::Publication<sensor_accel_s> _sensor_accel_pub;
math::LowPassFilter2pVector3f _filter{1000, 100}; math::LowPassFilter2pVector3f _filter{1000, 100};
@ -79,7 +79,7 @@ private:
int _class_device_instance{-1}; int _class_device_instance{-1};
unsigned _sample_rate{1000}; unsigned _sample_rate{1000};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff (ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff

4
src/lib/drivers/device/Device.hpp
View File

@ -130,6 +130,7 @@ public:
DeviceBusType_I2C = 1, DeviceBusType_I2C = 1,
DeviceBusType_SPI = 2, DeviceBusType_SPI = 2,
DeviceBusType_UAVCAN = 3, DeviceBusType_UAVCAN = 3,
DeviceBusType_SIMULATION = 4,
}; };
/** /**
@ -160,6 +161,9 @@ public:
case DeviceBusType_UAVCAN: case DeviceBusType_UAVCAN:
return "UAVCAN"; return "UAVCAN";
case DeviceBusType_SIMULATION:
return "SIMULATION";
case DeviceBusType_UNKNOWN: case DeviceBusType_UNKNOWN:
default: default:
return "UNKNOWN"; return "UNKNOWN";

1
src/lib/drivers/gyroscope/CMakeLists.txt
View File

@ -32,3 +32,4 @@
############################################################################ ############################################################################
px4_add_library(drivers_gyroscope PX4Gyroscope.cpp) px4_add_library(drivers_gyroscope PX4Gyroscope.cpp)
target_link_libraries(drivers_gyroscope PRIVATE drivers__device)

6
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -98,6 +98,12 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
_sensor_gyro_pub.get().device_id = device_id.devid; _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) void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
{ {
sensor_gyro_s &report = _sensor_gyro_pub.get(); sensor_gyro_s &report = _sensor_gyro_pub.get();

10
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -47,7 +47,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
{ {
public: 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; ~PX4Gyroscope() override;
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) 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_scale(float scale) { _sensor_gyro_pub.get().scaling = scale; }
void set_temperature(float temperature) { _sensor_gyro_pub.get().temperature = temperature; } 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 set_sample_rate(unsigned rate);
void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z); void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
@ -67,6 +65,8 @@ public:
private: private:
void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
uORB::Publication<sensor_gyro_s> _sensor_gyro_pub; uORB::Publication<sensor_gyro_s> _sensor_gyro_pub;
math::LowPassFilter2pVector3f _filter{1000, 100}; math::LowPassFilter2pVector3f _filter{1000, 100};
@ -79,7 +79,7 @@ private:
int _class_device_instance{-1}; int _class_device_instance{-1};
unsigned _sample_rate{1000}; unsigned _sample_rate{1000};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff (ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff

10
src/modules/simulator/CMakeLists.txt
View File

@ -41,7 +41,7 @@ if(ENABLE_UART_RC_INPUT)
elseif (WIN32) elseif (WIN32)
set(PIXHAWK_DEVICE "COM3") set(PIXHAWK_DEVICE "COM3")
endif() endif()
set(PIXHAWK_DEVICE_BAUD 115200) set(PIXHAWK_DEVICE_BAUD 115200)
endif() endif()
configure_file(simulator_config.h.in simulator_config.h @ONLY) configure_file(simulator_config.h.in simulator_config.h @ONLY)
@ -73,11 +73,11 @@ px4_add_module(
drivers__ledsim drivers__ledsim
git_ecl git_ecl
ecl_geo ecl_geo
drivers_accelerometer
drivers_barometer
drivers_gyroscope
drivers_magnetometer
) )
target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink) 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(gpssim)
add_subdirectory(gyrosim)

45
src/modules/simulator/accelsim/CMakeLists.txt
View File

@ -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
)

1052
src/modules/simulator/accelsim/accelsim.cpp
View File

File diff suppressed because it is too large Load Diff

46
src/modules/simulator/airspeedsim/CMakeLists.txt
View File

@ -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
)

352
src/modules/simulator/airspeedsim/airspeedsim.cpp
View File

@ -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 <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <simulator/simulator.h>
#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<differential_pressure_s *>(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;
}

172
src/modules/simulator/airspeedsim/airspeedsim.h
View File

@ -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 <px4_config.h>
#include <px4_defines.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <px4_workqueue.h>
#include <arch/board/board.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <lib/cdev/CDev.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
/* 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);
};

549
src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp
View File

@ -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 <lorenz@px4.io>
* @author Sarthak Kaingade
* @author Simon Wilks
* @author Thomas Gubler
* @author Roman Bapst
*
* Driver for a simulated airspeed sensor.
*
*/
#include <px4_config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdbool.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <parameters/param.h>
#include <perf/perf_counter.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <drivers/drv_airspeed.h>
#include <drivers/drv_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/system_power.h>
#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;
}

45
src/modules/simulator/barosim/CMakeLists.txt
View File

@ -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
)

347
src/modules/simulator/barosim/baro.cpp
View File

@ -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 <inttypes.h>
#include <px4_config.h>
#include <px4_defines.h>
#include <px4_time.h>
#include <px4_getopt.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <arch/board/board.h>
#include <board_config.h>
#include <drivers/device/device.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/ringbuffer.h>
#include <simulator/simulator.h>
#include <perf/perf_counter.h>
#include <systemlib/err.h>
#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;
}

42
src/modules/simulator/barosim/barosim.h
View File

@ -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"

46
src/modules/simulator/gyrosim/CMakeLists.txt
View File

@ -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
)

1291
src/modules/simulator/gyrosim/gyrosim.cpp
View File

File diff suppressed because it is too large Load Diff

124
src/modules/simulator/simulator.cpp
View File

@ -65,66 +65,16 @@ Simulator *Simulator::getInstance()
return _instance; 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) bool Simulator::getGPSSample(uint8_t *buf, int len)
{ {
return _gps.copyData(buf, 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) void Simulator::write_gps_data(void *buf)
{ {
_gps.writeData(buf); _gps.writeData(buf);
} }
void Simulator::write_airspeed_data(void *buf)
{
_airspeed.writeData(buf);
}
void Simulator::parameters_update(bool force) void Simulator::parameters_update(bool force)
{ {
bool updated; bool updated;
@ -149,33 +99,21 @@ int Simulator::start(int argc, char *argv[])
if (_instance) { if (_instance) {
drv_led_start(); 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_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_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 #ifndef __PX4_QURT
// Update sensor data // Update sensor data
_instance->poll_for_MAVLink_messages(); _instance->poll_for_MAVLink_messages();
#endif #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; return 0;
} else { } else {
@ -197,11 +135,9 @@ void Simulator::set_port(unsigned port)
static void usage() static void usage()
{ {
PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}"); PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");
PX4_WARN("Simulate raw sensors: simulator start -s"); PX4_WARN("Start simulator: simulator start");
PX4_WARN("Publish sensors combined: simulator start -p");
PX4_WARN("Connect using UDP: simulator start -u udp_port"); PX4_WARN("Connect using UDP: simulator start -u udp_port");
PX4_WARN("Connect using TCP: simulator start -c tcp_port"); PX4_WARN("Connect using TCP: simulator start -c tcp_port");
PX4_WARN("Dummy unit test data: simulator start -t");
} }
__BEGIN_DECLS __BEGIN_DECLS
@ -212,35 +148,27 @@ extern "C" {
int simulator_main(int argc, char *argv[]) int simulator_main(int argc, char *argv[])
{ {
if (argc > 2 && strcmp(argv[1], "start") == 0) { if (argc > 1 && strcmp(argv[1], "start") == 0) {
if (strcmp(argv[2], "-s") == 0 ||
strcmp(argv[2], "-p") == 0 ||
strcmp(argv[2], "-t") == 0) {
if (g_sim_task >= 0) { if (g_sim_task >= 0) {
PX4_WARN("Simulator already started"); PX4_WARN("Simulator already started");
return 0; 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) { } else if (argc == 2 && strcmp(argv[1], "stop") == 0) {

158
src/modules/simulator/simulator.h
View File

@ -42,80 +42,38 @@
#pragma once #pragma once
#include <px4_posix.h> #include <battery/battery.h>
#include <px4_module_params.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_baro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/drv_rc_input.h> #include <drivers/drv_rc_input.h>
#include <perf/perf_counter.h> #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <battery/battery.h> #include <lib/drivers/barometer/PX4Barometer.hpp>
#include <uORB/uORB.h> #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <uORB/topics/optical_flow.h> #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <uORB/topics/distance_sensor.h>
#include <v2.0/mavlink_types.h>
#include <v2.0/common/mavlink.h>
#include <lib/ecl/geo/geo.h> #include <lib/ecl/geo/geo.h>
#include <perf/perf_counter.h>
#include <px4_module_params.h>
#include <px4_posix.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/irlock_report.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/optical_flow.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/uORB.h>
#include <v2.0/common/mavlink.h>
#include <v2.0/mavlink_types.h>
namespace simulator 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) #pragma pack(push, 1)
struct RawGPSData { struct RawGPSData {
uint64_t timestamp; uint64_t timestamp;
@ -198,20 +156,6 @@ class Simulator : public ModuleParams
public: public:
static Simulator *getInstance(); 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 { enum class InternetProtocol {
TCP, TCP,
UDP UDP
@ -219,21 +163,9 @@ public:
static int start(int argc, char *argv[]); 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 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_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_ip(InternetProtocol ip);
void set_port(unsigned port); void set_port(unsigned port);
@ -251,6 +183,9 @@ private:
_param_sub = orb_subscribe(ORB_ID(parameter_update)); _param_sub = orb_subscribe(ORB_ID(parameter_update));
_battery_status.timestamp = hrt_absolute_time(); _battery_status.timestamp = hrt_absolute_time();
_px4_accel.set_sample_rate(250);
_px4_gyro.set_sample_rate(250);
} }
~Simulator() ~Simulator()
@ -259,22 +194,15 @@ private:
orb_unsubscribe(_param_sub); orb_unsubscribe(_param_sub);
// free perf counters // free perf counters
perf_free(_perf_accel);
perf_free(_perf_airspeed);
perf_free(_perf_baro);
perf_free(_perf_gps); perf_free(_perf_gps);
perf_free(_perf_mag);
perf_free(_perf_mpu);
perf_free(_perf_sim_delay); perf_free(_perf_sim_delay);
perf_free(_perf_sim_interval); perf_free(_perf_sim_interval);
_instance = NULL; _instance = nullptr;
} }
// class methods // 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_flow_topic(const mavlink_hil_optical_flow_t *flow);
int publish_odometry_topic(const mavlink_message_t *odom_mavlink); int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
int publish_distance_topic(const mavlink_distance_sensor_t *dist); int publish_distance_topic(const mavlink_distance_sensor_t *dist);
@ -282,31 +210,23 @@ private:
static Simulator *_instance; static Simulator *_instance;
// simulated sensor instances // simulated sensor instances
simulator::Report<simulator::RawAirspeedData> _airspeed{1}; PX4Accelerometer _px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
simulator::Report<simulator::RawAccelData> _accel{1}; PX4Gyroscope _px4_gyro{2294028, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
simulator::Report<simulator::RawBaroData> _baro{1}; PX4Magnetometer _px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
simulator::Report<simulator::RawGPSData> _gps{1}; PX4Barometer _px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
simulator::Report<simulator::RawMagData> _mag{1};
simulator::Report<simulator::RawMPUData> _mpu{1}; simulator::Report<simulator::RawGPSData> _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_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_delay{perf_alloc_once(PC_ELAPSED, "sim_network_delay")};
perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, "sim_network_interval")}; perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, "sim_network_interval")};
// uORB publisher handlers // uORB publisher handlers
orb_advert_t _accel_pub{nullptr};
orb_advert_t _baro_pub{nullptr};
orb_advert_t _battery_pub{nullptr}; orb_advert_t _battery_pub{nullptr};
orb_advert_t _differential_pressure_pub{nullptr};
orb_advert_t _dist_pub{nullptr}; orb_advert_t _dist_pub{nullptr};
orb_advert_t _flow_pub{nullptr}; orb_advert_t _flow_pub{nullptr};
orb_advert_t _gyro_pub{nullptr};
orb_advert_t _irlock_report_pub{nullptr}; orb_advert_t _irlock_report_pub{nullptr};
orb_advert_t _mag_pub{nullptr};
orb_advert_t _visual_odometry_pub{nullptr}; orb_advert_t _visual_odometry_pub{nullptr};
int _param_sub{-1}; int _param_sub{-1};
@ -315,8 +235,6 @@ private:
InternetProtocol _ip{InternetProtocol::UDP}; InternetProtocol _ip{InternetProtocol::UDP};
bool _initialized{false};
double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time double _realtime_factor{1.0}; ///< How fast the simulation runs in comparison to real system time
hrt_abstime _last_sim_timestamp{0}; hrt_abstime _last_sim_timestamp{0};
@ -349,8 +267,7 @@ private:
void send_controls(); void send_controls();
void send_heartbeat(); void send_heartbeat();
void send_mavlink_message(const mavlink_message_t &aMsg); void send_mavlink_message(const mavlink_message_t &aMsg);
void set_publish(const bool publish = true); void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu);
void update_sensors(const mavlink_hil_sensor_t *imu);
void update_gps(const mavlink_hil_gps_t *gps_sim); void update_gps(const mavlink_hil_gps_t *gps_sim);
static void *sending_trampoline(void *); static void *sending_trampoline(void *);
@ -369,7 +286,6 @@ private:
struct map_projection_reference_s _hil_local_proj_ref {}; struct map_projection_reference_s _hil_local_proj_ref {};
bool _hil_local_proj_inited{false}; bool _hil_local_proj_inited{false};
bool _publish{false};
double _hil_ref_lat{0}; double _hil_ref_lat{0};
double _hil_ref_lon{0}; double _hil_ref_lon{0};

235
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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; 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 if ((imu.fields_updated & 0x1FFF) != 0x1FFF) {
RawMPUData mpu = {}; PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x", imu.fields_updated);
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;
write_MPU_data(&mpu); // gyro
perf_begin(_perf_mpu); {
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
accel.x = imu->xacc; {
accel.y = imu->yacc; static constexpr float scaling = 1000.0f;
accel.z = imu->zacc; _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); // magnetometer
perf_begin(_perf_accel); {
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 = {}; // baro
mag.x = imu->xmag; {
mag.y = imu->ymag; _px4_baro.set_temperature(imu.temperature);
mag.z = imu->zmag; _px4_baro.update(time, imu.abs_pressure);
}
write_mag_data(&mag); // differential pressure
perf_begin(_perf_mag); {
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 = {}; int instance;
orb_publish_auto(ORB_ID(differential_pressure), &_differential_pressure_pub, &report, &instance, ORB_PRIO_DEFAULT);
// 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);
} }
void Simulator::update_gps(const mavlink_hil_gps_t *gps_sim) 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_hil_gps_t gps_sim;
mavlink_msg_hil_gps_decode(msg, &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); 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_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu); mavlink_msg_hil_sensor_decode(msg, &imu);
// set temperature to a decent value
imu.temperature = 32.0f;
struct timespec ts; struct timespec ts;
abstime_to_ts(&ts, imu.time_usec); abstime_to_ts(&ts, imu.time_usec);
px4_clock_settime(CLOCK_MONOTONIC, &ts); 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; last_time = now_us;
#endif #endif
if (_publish) { update_sensors(now_us, imu);
publish_sensor_topics(&imu);
}
update_sensors(&imu);
// battery simulation (limit update to 100Hz) // battery simulation (limit update to 100Hz)
if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) { 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); fill_rc_input_msg(&_rc_input, &rc_channels);
// publish message // publish message
if (_publish) { int rc_multi;
int rc_multi; orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH);
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) void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg)
@ -652,38 +641,6 @@ void Simulator::send_heartbeat()
send_mavlink_message(message); 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() void Simulator::poll_for_MAVLink_messages()
{ {
#ifdef __PX4_DARWIN #ifdef __PX4_DARWIN
@ -812,8 +769,6 @@ void Simulator::poll_for_MAVLink_messages()
// Request HIL_STATE_QUATERNION for ground truth. // Request HIL_STATE_QUATERNION for ground truth.
request_hil_state_quaternion(); request_hil_state_quaternion();
_initialized = true;
while (true) { while (true) {
// wait for new mavlink messages to arrive // wait for new mavlink messages to arrive
@ -857,11 +812,7 @@ void Simulator::poll_for_MAVLink_messages()
for (int i = 0; i < len; ++i) { for (int i = 0; i < len; ++i) {
if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) { 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); handle_message(&msg);
set_publish(prev_publish);
} }
} }
} }
@ -969,101 +920,6 @@ int openUart(const char *uart_name, int baud)
} }
#endif #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) int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
{ {
uint64_t timestamp = hrt_absolute_time(); uint64_t timestamp = hrt_absolute_time();
@ -1240,8 +1096,3 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
return OK; return OK;
} }
void Simulator::set_publish(const bool publish)
{
_publish = publish;
}