diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-21 18:13:01 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-21 18:13:01 +0200 |
commit | fab110d21f147e5064ff140aadac649017fa466e (patch) | |
tree | b41291188db4e5734353f0d09a0af306c58ed9f5 | |
parent | 309ea8146055528c22395fca06b7a70b660c22b3 (diff) | |
download | px4-firmware-fab110d21f147e5064ff140aadac649017fa466e.tar.gz px4-firmware-fab110d21f147e5064ff140aadac649017fa466e.tar.bz2 px4-firmware-fab110d21f147e5064ff140aadac649017fa466e.zip |
Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs
-rw-r--r-- | ROMFS/px4fmu_common/init.d/rc.hil | 12 | ||||
-rw-r--r-- | makefiles/setup.mk | 4 | ||||
-rw-r--r-- | src/drivers/hott/messages.cpp | 2 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 17 | ||||
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 10 | ||||
-rw-r--r-- | src/examples/fixedwing_control/main.c | 2 | ||||
-rw-r--r-- | src/lib/geo/geo.c (renamed from src/modules/systemlib/geo/geo.c) | 2 | ||||
-rw-r--r-- | src/lib/geo/geo.h (renamed from src/modules/systemlib/geo/geo.h) | 0 | ||||
-rw-r--r-- | src/lib/geo/module.mk | 38 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h (renamed from src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h (renamed from src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/arm_common_tables.h (renamed from src/modules/mathlib/CMSIS/Include/arm_common_tables.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/arm_const_structs.h (renamed from src/modules/mathlib/CMSIS/Include/arm_const_structs.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/arm_math.h (renamed from src/modules/mathlib/CMSIS/Include/arm_math.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/core_cm3.h (renamed from src/modules/mathlib/CMSIS/Include/core_cm3.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/core_cm4.h (renamed from src/modules/mathlib/CMSIS/Include/core_cm4.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/core_cm4_simd.h (renamed from src/modules/mathlib/CMSIS/Include/core_cm4_simd.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/core_cmFunc.h (renamed from src/modules/mathlib/CMSIS/Include/core_cmFunc.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/Include/core_cmInstr.h (renamed from src/modules/mathlib/CMSIS/Include/core_cmInstr.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a (renamed from src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a) | bin | 3039508 -> 3039508 bytes | |||
-rwxr-xr-x | src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a (renamed from src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a) | bin | 3049684 -> 3049684 bytes | |||
-rwxr-xr-x | src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a (renamed from src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a) | bin | 2989192 -> 2989192 bytes | |||
-rw-r--r-- | src/lib/mathlib/CMSIS/library.mk (renamed from src/modules/mathlib/CMSIS/library.mk) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/CMSIS/license.txt (renamed from src/modules/mathlib/CMSIS/license.txt) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Dcm.cpp (renamed from src/modules/mathlib/math/Dcm.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Dcm.hpp (renamed from src/modules/mathlib/math/Dcm.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/EulerAngles.cpp (renamed from src/modules/mathlib/math/EulerAngles.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/EulerAngles.hpp (renamed from src/modules/mathlib/math/EulerAngles.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Limits.cpp (renamed from src/modules/mathlib/math/Limits.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Limits.hpp (renamed from src/modules/mathlib/math/Limits.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix.cpp (renamed from src/modules/mathlib/math/Matrix.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Matrix.hpp (renamed from src/modules/mathlib/math/Matrix.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.cpp (renamed from src/modules/mathlib/math/Quaternion.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Quaternion.hpp (renamed from src/modules/mathlib/math/Quaternion.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.cpp (renamed from src/modules/mathlib/math/Vector.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector.hpp (renamed from src/modules/mathlib/math/Vector.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector2f.cpp (renamed from src/modules/mathlib/math/Vector2f.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector2f.hpp (renamed from src/modules/mathlib/math/Vector2f.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector3.cpp (renamed from src/modules/mathlib/math/Vector3.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/Vector3.hpp (renamed from src/modules/mathlib/math/Vector3.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/arm/Matrix.cpp (renamed from src/modules/mathlib/math/arm/Matrix.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/arm/Matrix.hpp (renamed from src/modules/mathlib/math/arm/Matrix.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/arm/Vector.cpp (renamed from src/modules/mathlib/math/arm/Vector.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/arm/Vector.hpp (renamed from src/modules/mathlib/math/arm/Vector.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/filter/LowPassFilter2p.cpp (renamed from src/modules/mathlib/math/filter/LowPassFilter2p.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/filter/LowPassFilter2p.hpp (renamed from src/modules/mathlib/math/filter/LowPassFilter2p.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/filter/module.mk (renamed from src/modules/mathlib/math/filter/module.mk) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/generic/Matrix.cpp (renamed from src/modules/mathlib/math/generic/Matrix.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/generic/Matrix.hpp (renamed from src/modules/mathlib/math/generic/Matrix.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/generic/Vector.cpp (renamed from src/modules/mathlib/math/generic/Vector.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/generic/Vector.hpp (renamed from src/modules/mathlib/math/generic/Vector.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/nasa_rotation_def.pdf (renamed from src/modules/mathlib/math/nasa_rotation_def.pdf) | bin | 709235 -> 709235 bytes | |||
-rw-r--r-- | src/lib/mathlib/math/test/test.cpp (renamed from src/modules/mathlib/math/test/test.cpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/test/test.hpp (renamed from src/modules/mathlib/math/test/test.hpp) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/math/test_math.sce (renamed from src/modules/mathlib/math/test_math.sce) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/mathlib.h (renamed from src/modules/mathlib/mathlib.h) | 0 | ||||
-rw-r--r-- | src/lib/mathlib/module.mk (renamed from src/modules/mathlib/module.mk) | 0 | ||||
-rw-r--r-- | src/modules/commander/accelerometer_calibration.cpp | 2 | ||||
-rw-r--r-- | src/modules/controllib/uorb/blocks.hpp | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink.c | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 88 | ||||
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 50 | ||||
-rw-r--r-- | src/modules/mavlink/orb_topics.h | 1 | ||||
-rw-r--r-- | src/modules/position_estimator_inav/position_estimator_inav_main.c | 3 | ||||
-rw-r--r-- | src/modules/systemlib/airspeed.c | 16 | ||||
-rw-r--r-- | src/modules/systemlib/airspeed.h | 8 | ||||
-rw-r--r-- | src/modules/systemlib/conversions.c | 97 | ||||
-rw-r--r-- | src/modules/systemlib/conversions.h | 29 | ||||
-rw-r--r-- | src/modules/systemlib/module.mk | 1 |
69 files changed, 216 insertions, 171 deletions
diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil index 3517a5bd8..a843b7ffb 100644 --- a/ROMFS/px4fmu_common/init.d/rc.hil +++ b/ROMFS/px4fmu_common/init.d/rc.hil @@ -8,7 +8,7 @@ echo "[HIL] starting.." uorb start # Tell MAVLink that this link is "fast" -mavlink start -b 230400 -d /dev/console +mavlink start -b 230400 -d /dev/ttyS1 # Create a fake HIL /dev/pwm_output interface hil mode_pwm @@ -38,13 +38,13 @@ commander start # # Check if we got an IO # -if [ px4io start ] +if px4io start then echo "IO started" else fmu mode_serial echo "FMU started" -end +fi # # Start the sensors (depends on orb, px4io) @@ -60,9 +60,7 @@ att_pos_estimator_ekf start # Load mixer and start controllers (depends on px4io) # mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix -fixedwing_backside start +fw_pos_control_l1 start +fw_att_control start echo "[HIL] setup done, running" - -# Exit shell to make it available to MAVLink -exit diff --git a/makefiles/setup.mk b/makefiles/setup.mk index 168e41a5c..42f9a8a7f 100644 --- a/makefiles/setup.mk +++ b/makefiles/setup.mk @@ -43,6 +43,7 @@ # export PX4_INCLUDE_DIR = $(abspath $(PX4_BASE)/src/include)/ export PX4_MODULE_SRC = $(abspath $(PX4_BASE)/src)/ +export PX4_LIB_DIR = $(abspath $(PX4_BASE)/src/lib)/ export PX4_MK_DIR = $(abspath $(PX4_BASE)/makefiles)/ export NUTTX_SRC = $(abspath $(PX4_BASE)/NuttX/nuttx)/ export NUTTX_APP_SRC = $(abspath $(PX4_BASE)/NuttX/apps)/ @@ -57,7 +58,8 @@ export ARCHIVE_DIR = $(abspath $(PX4_BASE)/Archives)/ # export INCLUDE_DIRS := $(PX4_MODULE_SRC) \ $(PX4_MODULE_SRC)/modules/ \ - $(PX4_INCLUDE_DIR) + $(PX4_INCLUDE_DIR) \ + $(PX4_LIB_DIR) # # Tools diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 57c256339..bb8d45bea 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -42,7 +42,7 @@ #include <math.h> #include <stdio.h> #include <string.h> -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <unistd.h> #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index efe7cf8eb..cf5f8d94c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -54,7 +54,6 @@ #include <systemlib/perf_counter.h> #include <systemlib/err.h> -#include <systemlib/geo/geo.h> #include <nuttx/arch.h> #include <nuttx/clock.h> @@ -178,6 +177,8 @@ static const int ERROR = -1; #define LSM303D_MAG_DEFAULT_RANGE_GA 2 #define LSM303D_MAG_DEFAULT_RATE 100 +#define LSM303D_ONE_G 9.80665f + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -778,7 +779,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGRANGE: /* convert to m/s^2 and return rounded in G */ - return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f); case ACCELIOCGSCALE: /* copy scale out */ @@ -1015,27 +1016,27 @@ LSM303D::accel_set_range(unsigned max_g) max_g = 16; if (max_g <= 2) { - _accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 2.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_2G_A; new_scale_g_digit = 0.061e-3f; } else if (max_g <= 4) { - _accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 4.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_4G_A; new_scale_g_digit = 0.122e-3f; } else if (max_g <= 6) { - _accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 6.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_6G_A; new_scale_g_digit = 0.183e-3f; } else if (max_g <= 8) { - _accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 8.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_8G_A; new_scale_g_digit = 0.244e-3f; } else if (max_g <= 16) { - _accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G; + _accel_range_m_s2 = 16.0f*LSM303D_ONE_G; setbits |= REG2_FULL_SCALE_16G_A; new_scale_g_digit = 0.732e-3f; @@ -1043,7 +1044,7 @@ LSM303D::accel_set_range(unsigned max_g) return -EINVAL; } - _accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G; + _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; modify_reg(ADDR_CTRL_REG2, clearbits, setbits); diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 0e65923db..14f8f44b8 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -159,6 +159,8 @@ #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42 +#define MPU6000_ONE_G 9.80665f + class MPU6000_gyro; class MPU6000 : public device::SPI @@ -543,8 +545,8 @@ void MPU6000::reset() // Correct accel scale factors of 4096 LSB/g // scale to m/s^2 ( 1g = 9.81 m/s^2) - _accel_range_scale = (CONSTANTS_ONE_G / 4096.0f); - _accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G; + _accel_range_scale = (MPU6000_ONE_G / 4096.0f); + _accel_range_m_s2 = 8.0f * MPU6000_ONE_G; usleep(1000); @@ -906,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; case ACCELIOCGRANGE: - return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f); + return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f); case ACCELIOCSELFTEST: return accel_self_test(); @@ -1409,7 +1411,7 @@ test() warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw); warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw); warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2, - (double)(a_report.range_m_s2 / CONSTANTS_ONE_G)); + (double)(a_report.range_m_s2 / MPU6000_ONE_G)); /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index e29f76877..b286e0007 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -66,7 +66,7 @@ #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/pid/pid.h> -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <systemlib/err.h> diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c index 6463e6489..63792dda5 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -44,7 +44,7 @@ * @author Lorenz Meier <lm@inf.ethz.ch> */ -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <nuttx/config.h> #include <unistd.h> #include <pthread.h> diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h index dadec51ec..dadec51ec 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/lib/geo/geo.h diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk new file mode 100644 index 000000000..30a2dc99f --- /dev/null +++ b/src/lib/geo/module.mk @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Geo library +# + +SRCS = geo.c diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h index 8f39acd9d..8f39acd9d 100644 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h +++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h index 181b7e433..181b7e433 100644 --- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h +++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h index 9c37ab4e5..9c37ab4e5 100644 --- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h +++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h index 406f737dc..406f737dc 100644 --- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h +++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h index 6f66f9ee3..6f66f9ee3 100644 --- a/src/modules/mathlib/CMSIS/Include/arm_math.h +++ b/src/lib/mathlib/CMSIS/Include/arm_math.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h index 8ac6dc078..8ac6dc078 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cm3.h +++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h index 93efd3a7a..93efd3a7a 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cm4.h +++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h index af1831ee1..af1831ee1 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h +++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h index 139bc3c5e..139bc3c5e 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h +++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h index 8946c2c49..8946c2c49 100644 --- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h +++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a Binary files differindex 6898bc27d..6898bc27d 100644 --- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a +++ b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a Binary files differindex a0185eaa9..a0185eaa9 100755 --- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a +++ b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a Binary files differindex 94525528e..94525528e 100755 --- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a +++ b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk index 0cc1b559d..0cc1b559d 100644 --- a/src/modules/mathlib/CMSIS/library.mk +++ b/src/lib/mathlib/CMSIS/library.mk diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt index 31afac1ec..31afac1ec 100644 --- a/src/modules/mathlib/CMSIS/license.txt +++ b/src/lib/mathlib/CMSIS/license.txt diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp index f509f7081..f509f7081 100644 --- a/src/modules/mathlib/math/Dcm.cpp +++ b/src/lib/mathlib/math/Dcm.cpp diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp index df8970d3a..df8970d3a 100644 --- a/src/modules/mathlib/math/Dcm.hpp +++ b/src/lib/mathlib/math/Dcm.hpp diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp index e733d23bb..e733d23bb 100644 --- a/src/modules/mathlib/math/EulerAngles.cpp +++ b/src/lib/mathlib/math/EulerAngles.cpp diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp index 399eecfa7..399eecfa7 100644 --- a/src/modules/mathlib/math/EulerAngles.hpp +++ b/src/lib/mathlib/math/EulerAngles.hpp diff --git a/src/modules/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp index d4c892d8a..d4c892d8a 100644 --- a/src/modules/mathlib/math/Limits.cpp +++ b/src/lib/mathlib/math/Limits.cpp diff --git a/src/modules/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp index fb778dd66..fb778dd66 100644 --- a/src/modules/mathlib/math/Limits.hpp +++ b/src/lib/mathlib/math/Limits.hpp diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp index ebd1aeda3..ebd1aeda3 100644 --- a/src/modules/mathlib/math/Matrix.cpp +++ b/src/lib/mathlib/math/Matrix.cpp diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index f19db15ec..f19db15ec 100644 --- a/src/modules/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp index 02fec4ca6..02fec4ca6 100644 --- a/src/modules/mathlib/math/Quaternion.cpp +++ b/src/lib/mathlib/math/Quaternion.cpp diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 048a55d33..048a55d33 100644 --- a/src/modules/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp index 35158a396..35158a396 100644 --- a/src/modules/mathlib/math/Vector.cpp +++ b/src/lib/mathlib/math/Vector.cpp diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index 73de793d5..73de793d5 100644 --- a/src/modules/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp index 68e741817..68e741817 100644 --- a/src/modules/mathlib/math/Vector2f.cpp +++ b/src/lib/mathlib/math/Vector2f.cpp diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp index ecd62e81c..ecd62e81c 100644 --- a/src/modules/mathlib/math/Vector2f.hpp +++ b/src/lib/mathlib/math/Vector2f.hpp diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp index dcb85600e..dcb85600e 100644 --- a/src/modules/mathlib/math/Vector3.cpp +++ b/src/lib/mathlib/math/Vector3.cpp diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp index 568d9669a..568d9669a 100644 --- a/src/modules/mathlib/math/Vector3.hpp +++ b/src/lib/mathlib/math/Vector3.hpp diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp index 21661622a..21661622a 100644 --- a/src/modules/mathlib/math/arm/Matrix.cpp +++ b/src/lib/mathlib/math/arm/Matrix.cpp diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp index 715fd3a5e..715fd3a5e 100644 --- a/src/modules/mathlib/math/arm/Matrix.hpp +++ b/src/lib/mathlib/math/arm/Matrix.hpp diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp index 7ea6496bb..7ea6496bb 100644 --- a/src/modules/mathlib/math/arm/Vector.cpp +++ b/src/lib/mathlib/math/arm/Vector.cpp diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp index 4155800e8..4155800e8 100644 --- a/src/modules/mathlib/math/arm/Vector.hpp +++ b/src/lib/mathlib/math/arm/Vector.hpp diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index efb17225d..efb17225d 100644 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4..208ec98d4 100644 --- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp diff --git a/src/modules/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk index fe92c8c70..fe92c8c70 100644 --- a/src/modules/mathlib/math/filter/module.mk +++ b/src/lib/mathlib/math/filter/module.mk diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp index 21661622a..21661622a 100644 --- a/src/modules/mathlib/math/generic/Matrix.cpp +++ b/src/lib/mathlib/math/generic/Matrix.cpp diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp index 5601a3447..5601a3447 100644 --- a/src/modules/mathlib/math/generic/Matrix.hpp +++ b/src/lib/mathlib/math/generic/Matrix.hpp diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp index 7ea6496bb..7ea6496bb 100644 --- a/src/modules/mathlib/math/generic/Vector.cpp +++ b/src/lib/mathlib/math/generic/Vector.cpp diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp index 8cfdc676d..8cfdc676d 100644 --- a/src/modules/mathlib/math/generic/Vector.hpp +++ b/src/lib/mathlib/math/generic/Vector.hpp diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf Binary files differindex eb67a4bfc..eb67a4bfc 100644 --- a/src/modules/mathlib/math/nasa_rotation_def.pdf +++ b/src/lib/mathlib/math/nasa_rotation_def.pdf diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index 2fa2f7e7c..2fa2f7e7c 100644 --- a/src/modules/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp index 2027bb827..2027bb827 100644 --- a/src/modules/mathlib/math/test/test.hpp +++ b/src/lib/mathlib/math/test/test.hpp diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce index c3fba4729..c3fba4729 100644 --- a/src/modules/mathlib/math/test_math.sce +++ b/src/lib/mathlib/math/test_math.sce diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index 40ffb22bc..40ffb22bc 100644 --- a/src/modules/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk index 2146a1413..2146a1413 100644 --- a/src/modules/mathlib/module.mk +++ b/src/lib/mathlib/module.mk diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index b6217a414..ddd2e23d9 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -116,7 +116,7 @@ #include <drivers/drv_hrt.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_accel.h> -#include <systemlib/conversions.h> +#include <geo/geo.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <mavlink/mavlink_log.h> diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 9c0720aa5..46dc1bec2 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -58,7 +58,7 @@ #include <poll.h> extern "C" { -#include <systemlib/geo/geo.h> +#include <geo/geo.h> } #include "../blocks.hpp" diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 3d3434670..f39bbaa72 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -279,6 +279,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m orb_set_interval(subs->act_2_sub, min_interval); orb_set_interval(subs->act_3_sub, min_interval); orb_set_interval(subs->actuators_sub, min_interval); + orb_set_interval(subs->actuators_effective_sub, min_interval); + orb_set_interval(subs->spa_sub, min_interval); + orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval); break; case MAVLINK_MSG_ID_MANUAL_CONTROL: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..28f7af33c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -50,6 +50,10 @@ #include <mqueue.h> #include <string.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> +#include <drivers/drv_gyro.h> +#include <drivers/drv_mag.h> +#include <drivers/drv_baro.h> #include <time.h> #include <float.h> #include <unistd.h> @@ -101,6 +105,10 @@ static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; +static orb_advert_t pub_hil_gyro = -1; +static orb_advert_t pub_hil_accel = -1; +static orb_advert_t pub_hil_mag = -1; +static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t cmd_pub = -1; @@ -412,12 +420,12 @@ handle_message(mavlink_message_t *msg) /* airspeed from differential pressure, ambient pressure and temp */ struct airspeed_s airspeed; - airspeed.timestamp = hrt_absolute_time(); float ias = calc_indicated_airspeed(imu.diff_pressure); // XXX need to fix this float tas = ias; + airspeed.timestamp = hrt_absolute_time(); airspeed.indicated_airspeed_m_s = ias; airspeed.true_airspeed_m_s = tas; @@ -428,7 +436,67 @@ handle_message(mavlink_message_t *msg) } //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); - /* publish */ + /* individual sensor publications */ + struct gyro_report gyro; + gyro.x_raw = imu.xgyro / mrad2rad; + gyro.y_raw = imu.ygyro / mrad2rad; + gyro.z_raw = imu.zgyro / mrad2rad; + gyro.x = imu.xgyro; + gyro.y = imu.ygyro; + gyro.z = imu.zgyro; + gyro.temperature = imu.temperature; + gyro.timestamp = hrt_absolute_time(); + + if (pub_hil_gyro < 0) { + pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { + orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); + } + + struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } + + struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); + + if (pub_hil_mag < 0) { + pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { + orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); + } + + struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); + + if (pub_hil_baro < 0) { + pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { + orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); + } + + /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); } else { @@ -552,6 +620,22 @@ handle_message(mavlink_message_t *msg) } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } + + struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); + + if (pub_hil_accel < 0) { + pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { + orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); + } } if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d..c711b1803 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,8 @@ struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; -struct actuator_controls_effective_s actuators_0; +struct actuator_controls_effective_s actuators_effective_0; +struct actuator_controls_s actuators_0; struct vehicle_attitude_s att; struct mavlink_subscriptions mavlink_subs; @@ -112,6 +113,7 @@ static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); +static void l_vehicle_attitude_controls_effective(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); static void l_optical_flow(const struct listener *l); @@ -138,6 +140,7 @@ static const struct listener listeners[] = { {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, + {l_vehicle_attitude_controls_effective, &mavlink_subs.actuators_effective_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, {l_optical_flow, &mavlink_subs.optical_flow, 0}, {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, @@ -567,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l) } void -l_vehicle_attitude_controls(const struct listener *l) +l_vehicle_attitude_controls_effective(const struct listener *l) { - orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0); if (gcs_link) { /* send, add spaces so that string buffer is at least 10 chars long */ mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl0 ", - actuators_0.control_effective[0]); + actuators_effective_0.control_effective[0]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl1 ", - actuators_0.control_effective[1]); + actuators_effective_0.control_effective[1]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl2 ", - actuators_0.control_effective[2]); + actuators_effective_0.control_effective[2]); mavlink_msg_named_value_float_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000, "eff ctrl3 ", - actuators_0.control_effective[3]); + actuators_effective_0.control_effective[3]); + } +} + +void +l_vehicle_attitude_controls(const struct listener *l) +{ + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0); + + if (gcs_link) { + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl0 ", + actuators_0.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl1 ", + actuators_0.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl2 ", + actuators_0.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + "ctrl3 ", + actuators_0.control[3]); } } @@ -637,7 +666,7 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); float alt = global_pos.alt; float climb = global_pos.vz; @@ -773,7 +802,10 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.man_control_sp_sub, 100); /* 10Hz updates */ /* --- ACTUATOR CONTROL VALUE --- */ - mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + orb_set_interval(mavlink_subs.actuators_effective_sub, 100); /* 10Hz updates */ + + mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); orb_set_interval(mavlink_subs.actuators_sub, 100); /* 10Hz updates */ /* --- DEBUG VALUE OUTPUT --- */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 506f73105..e2e630046 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -80,6 +80,7 @@ struct mavlink_subscriptions { int safety_sub; int actuators_sub; int armed_sub; + int actuators_effective_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3466841c4..760e598bc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -64,9 +64,8 @@ #include <mavlink/mavlink_log.h> #include <poll.h> #include <systemlib/err.h> -#include <systemlib/geo/geo.h> +#include <geo/geo.h> #include <systemlib/systemlib.h> -#include <systemlib/conversions.h> #include <drivers/drv_hrt.h> #include "position_estimator_inav_params.h" diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index e01cc4dda..310fbf60f 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -42,7 +42,7 @@ #include <stdio.h> #include <math.h> -#include "conversions.h" +#include <geo/geo.h> #include "airspeed.h" @@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { - density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; -// printf("[airspeed] Invalid air density, using density at sea level\n"); + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; } float pressure_difference = total_pressure - static_pressure; - if(pressure_difference > 0) { + if (pressure_difference > 0) { return sqrtf((2.0f*(pressure_difference)) / density); - } else - { + } else { return -sqrtf((2.0f*fabsf(pressure_difference)) / density); } } + +float get_air_density(float static_pressure, float temperature_celsius) +{ + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); +} diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h index def53f0c1..8dccaab9c 100644 --- a/src/modules/systemlib/airspeed.h +++ b/src/modules/systemlib/airspeed.h @@ -85,6 +85,14 @@ */ __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); + /** + * Calculates air density. + * + * @param static_pressure ambient pressure in millibar + * @param temperature_celcius air / ambient temperature in celcius + */ +__EXPORT float get_air_density(float static_pressure, float temperature_celsius); + __END_DECLS #endif diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c index ac94252c5..9105d83cb 100644 --- a/src/modules/systemlib/conversions.c +++ b/src/modules/systemlib/conversions.c @@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[]) return u.w; } - -void rot2quat(const float R[9], float Q[4]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - int32_t idx; - - /* conversion of rotation matrix to quaternion - * choose the largest component to begin with */ - q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F; - q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F; - q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F; - q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F; - - idx = 0; - - if (q0_2 < q1_2) { - q0_2 = q1_2; - - idx = 1; - } - - if (q0_2 < q2_2) { - q0_2 = q2_2; - idx = 2; - } - - if (q0_2 < q3_2) { - q0_2 = q3_2; - idx = 3; - } - - q0_2 = sqrtf(q0_2); - - /* solve for the remaining three components */ - if (idx == 0) { - q1_2 = q0_2; - q2_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[6] - R[2]) / 4.0F / q0_2; - q0_2 = (R[1] - R[3]) / 4.0F / q0_2; - - } else if (idx == 1) { - q2_2 = q0_2; - q1_2 = (R[5] - R[7]) / 4.0F / q0_2; - q3_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[6] + R[2]) / 4.0F / q0_2; - - } else if (idx == 2) { - q3_2 = q0_2; - q1_2 = (R[6] - R[2]) / 4.0F / q0_2; - q2_2 = (R[3] + R[1]) / 4.0F / q0_2; - q0_2 = (R[7] + R[5]) / 4.0F / q0_2; - - } else { - q1_2 = (R[1] - R[3]) / 4.0F / q0_2; - q2_2 = (R[6] + R[2]) / 4.0F / q0_2; - q3_2 = (R[7] + R[5]) / 4.0F / q0_2; - } - - /* return values */ - Q[0] = q1_2; - Q[1] = q2_2; - Q[2] = q3_2; - Q[3] = q0_2; -} - -void quat2rot(const float Q[4], float R[9]) -{ - float q0_2; - float q1_2; - float q2_2; - float q3_2; - - memset(&R[0], 0, 9U * sizeof(float)); - - q0_2 = Q[0] * Q[0]; - q1_2 = Q[1] * Q[1]; - q2_2 = Q[2] * Q[2]; - q3_2 = Q[3] * Q[3]; - - R[0] = ((q0_2 + q1_2) - q2_2) - q3_2; - R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]); - R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]); - R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]); - R[4] = ((q0_2 + q2_2) - q1_2) - q3_2; - R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]); - R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]); - R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]); - R[8] = ((q0_2 + q3_2) - q1_2) - q2_2; -} - -float get_air_density(float static_pressure, float temperature_celsius) -{ - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); -} diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h index 064426f21..dc383e770 100644 --- a/src/modules/systemlib/conversions.h +++ b/src/modules/systemlib/conversions.h @@ -43,7 +43,6 @@ #define CONVERSIONS_H_ #include <float.h> #include <stdint.h> -#include <systemlib/geo/geo.h> __BEGIN_DECLS @@ -57,34 +56,6 @@ __BEGIN_DECLS */ __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]); -/** - * Converts a 3 x 3 rotation matrix to an unit quaternion. - * - * All orientations are expressed in NED frame. - * - * @param R rotation matrix to convert - * @param Q quaternion to write back to - */ -__EXPORT void rot2quat(const float R[9], float Q[4]); - -/** - * Converts an unit quaternion to a 3 x 3 rotation matrix. - * - * All orientations are expressed in NED frame. - * - * @param Q quaternion to convert - * @param R rotation matrix to write back to - */ -__EXPORT void quat2rot(const float Q[4], float R[9]); - -/** - * Calculates air density. - * - * @param static_pressure ambient pressure in millibar - * @param temperature_celcius air / ambient temperature in celcius - */ -__EXPORT float get_air_density(float static_pressure, float temperature_celsius); - __END_DECLS #endif /* CONVERSIONS_H_ */ diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index cbf829122..94c744c03 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -45,7 +45,6 @@ SRCS = err.c \ getopt_long.c \ up_cxxinitialize.c \ pid/pid.c \ - geo/geo.c \ systemlib.c \ airspeed.c \ system_params.c \ |