diff --git a/examples/sensor_fusion/Kconfig b/examples/sensor_fusion/Kconfig new file mode 100644 index 00000000000..ba1fd6674a5 --- /dev/null +++ b/examples/sensor_fusion/Kconfig @@ -0,0 +1,38 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config EXAMPLES_SENSOR_FUSION + bool "Sensor Fusion Example" + default n + ---help--- + Sensor fusion example that combines gyro and accelerometer data + using the Madgwick algorithm to provide yaw, pitch and roll angles. + +if EXAMPLES_SENSOR_FUSION + +config EXAMPLES_SENSOR_FUSION_SAMPLES + int "Number of samples to acquire" + default 100 + +config EXAMPLES_SENSOR_FUSION_SAMPLE_RATE + int "Sample rate in ms" + default 100 + +config EXAMPLES_SENSOR_FUSION_PROGNAME + string "Program name" + default "sensor_fusion" + ---help--- + This is the name of the program that will be used when the NSH ELF + program is installed. + +config EXAMPLES_SENSOR_FUSION_PRIORITY + int "Sensor Fusion task priority" + default 100 + +config EXAMPLES_SENSOR_FUSION_STACKSIZE + int "Sensor Fusion stack size" + default DEFAULT_TASK_STACKSIZE + +endif diff --git a/examples/sensor_fusion/Make.defs b/examples/sensor_fusion/Make.defs new file mode 100644 index 00000000000..f7499a972b1 --- /dev/null +++ b/examples/sensor_fusion/Make.defs @@ -0,0 +1,24 @@ +############################################################################ +# apps/examples/sensor_fusion/Make.defs +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +ifneq ($(CONFIG_EXAMPLES_SENSOR_FUSION),) +CONFIGURED_APPS += $(APPDIR)/examples/sensor_fusion + +endif diff --git a/examples/sensor_fusion/Makefile b/examples/sensor_fusion/Makefile new file mode 100644 index 00000000000..ce17e69dfe7 --- /dev/null +++ b/examples/sensor_fusion/Makefile @@ -0,0 +1,30 @@ +############################################################################ +# apps/examples/sensor_fusion/Makefile +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +include $(APPDIR)/Make.defs + +PROGNAME = $(CONFIG_EXAMPLES_SENSOR_FUSION_PROGNAME) +PRIORITY = $(CONFIG_EXAMPLES_SENSOR_FUSION_PRIORITY) +STACKSIZE = $(CONFIG_EXAMPLES_SENSOR_FUSION_STACKSIZE) +MODULE = $(CONFIG_EXAMPLES_SENSOR_FUSION) + +MAINSRC = sensor_fusion_main.c + +include $(APPDIR)/Application.mk diff --git a/examples/sensor_fusion/sensor_fusion_main.c b/examples/sensor_fusion/sensor_fusion_main.c new file mode 100644 index 00000000000..3046f244031 --- /dev/null +++ b/examples/sensor_fusion/sensor_fusion_main.c @@ -0,0 +1,162 @@ +/**************************************************************************** + * apps/examples/sensor_fusion/sensor_fusion_main.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include "Fusion/Fusion.h" + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define REG_LOW_MASK 0xFF00 +#define REG_HIGH_MASK 0x00FF +#define MPU6050_FS_SEL 65.5f +#define MPU6050_AFS_SEL 4096.0f + +/**************************************************************************** + * Public Types + ****************************************************************************/ + +struct mpu6050_imu_msg +{ + int16_t acc_x; + int16_t acc_y; + int16_t acc_z; + int16_t temp; + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; +}; + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +void read_mpu6050(int fd, struct sensor_accel *acc_data, + struct sensor_gyro *gyro_data); + +/**************************************************************************** + * sensor_fusion_main + ****************************************************************************/ + +int main(int argc, FAR char *argv[]) +{ + int fd; + int iterations = CONFIG_EXAMPLES_SENSOR_FUSION_SAMPLES; + float acq_period = CONFIG_EXAMPLES_SENSOR_FUSION_SAMPLE_RATE / 1000.0f; + + /* Minimal required data for Fusion library. Use of magnetometer data + * is optional and can improve performance. + */ + + struct sensor_accel imu_acc_data; + struct sensor_gyro imu_gyro_data; + + printf("Sensor Fusion example\n"); + printf("Sample Rate: %.2f Hz\n", 1.0 / acq_period); + + FusionAhrs ahrs; + FusionAhrsInitialise(&ahrs); + + fd = open("/dev/imu0", O_RDONLY); + if (fd < 0) + { + printf("Failed to open imu0\n"); + return EXIT_FAILURE; + } + + for (int i = 0; i < iterations; i++) + { + read_mpu6050(fd, &imu_acc_data, &imu_gyro_data); + + const FusionVector accelerometer = + { + {imu_acc_data.x, imu_acc_data.y, imu_acc_data.z} + }; + + const FusionVector gyroscope = + { + {imu_gyro_data.x, imu_gyro_data.y, imu_gyro_data.z} + }; + + FusionAhrsUpdateNoMagnetometer(&ahrs, + gyroscope, + accelerometer, + acq_period); + const FusionEuler euler = + FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs)); + + printf("Yaw: %.3f | Pitch: %.3f | Roll: %.3f\n", + euler.angle.yaw, euler.angle.pitch, euler.angle.roll); + usleep(CONFIG_EXAMPLES_SENSOR_FUSION_SAMPLE_RATE * 1000); + } + + close(fd); + + return EXIT_SUCCESS; +} + +void read_mpu6050(int fd, + struct sensor_accel *acc_data, + struct sensor_gyro *gyro_data) +{ + struct mpu6050_imu_msg raw_imu; + memset(&raw_imu, 0, sizeof(raw_imu)); + int16_t raw_data[7]; + + int ret = read(fd, &raw_data, sizeof(raw_data)); + if (ret != sizeof(raw_data)) + { + printf("Failed to read accelerometer data\n"); + } + else + { + raw_imu.acc_x = ((raw_data[0] & REG_HIGH_MASK) << 8) + + ((raw_data[0] & REG_LOW_MASK) >> 8); + raw_imu.acc_y = ((raw_data[1] & REG_HIGH_MASK) << 8) + + ((raw_data[1] & REG_LOW_MASK) >> 8); + raw_imu.acc_z = ((raw_data[2] & REG_HIGH_MASK) << 8) + + ((raw_data[2] & REG_LOW_MASK) >> 8); + raw_imu.gyro_x = ((raw_data[4] & REG_HIGH_MASK) << 8) + + ((raw_data[4] & REG_LOW_MASK) >> 8); + raw_imu.gyro_y = ((raw_data[5] & REG_HIGH_MASK) << 8) + + ((raw_data[5] & REG_LOW_MASK) >> 8); + raw_imu.gyro_z = ((raw_data[6] & REG_HIGH_MASK) << 8) + + ((raw_data[6] & REG_LOW_MASK) >> 8); + } + + acc_data->x = raw_imu.acc_x / MPU6050_AFS_SEL; + acc_data->y = raw_imu.acc_y / MPU6050_AFS_SEL; + acc_data->z = raw_imu.acc_z / MPU6050_AFS_SEL; + + /* Gyro data in ยบ/s (degrees per second) */ + + gyro_data->x = raw_imu.gyro_x / MPU6050_FS_SEL; + gyro_data->y = raw_imu.gyro_y / MPU6050_FS_SEL; + gyro_data->z = raw_imu.gyro_z / MPU6050_FS_SEL; +} diff --git a/inertial/CMakeLists.txt b/inertial/CMakeLists.txt new file mode 100644 index 00000000000..fb213b9eebc --- /dev/null +++ b/inertial/CMakeLists.txt @@ -0,0 +1,21 @@ +# ############################################################################## +# apps/inertial/CMakeLists.txt +# +# Licensed to the Apache Software Foundation (ASF) under one or more contributor +# license agreements. See the NOTICE file distributed with this work for +# additional information regarding copyright ownership. The ASF licenses this +# file to you under the Apache License, Version 2.0 (the "License"); you may not +# use this file except in compliance with the License. You may obtain a copy of +# the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations under +# the License. +# +# ############################################################################## + +nuttx_generate_kconfig(MENUDESC "Inertial Libraries Support") diff --git a/inertial/Make.defs b/inertial/Make.defs new file mode 100644 index 00000000000..03113ac85de --- /dev/null +++ b/inertial/Make.defs @@ -0,0 +1,21 @@ +############################################################################ +# apps/inertial/Make.defs +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +include $(wildcard $(APPDIR)/inertial/*/Make.defs) diff --git a/inertial/Makefile b/inertial/Makefile new file mode 100644 index 00000000000..e05b10cd28d --- /dev/null +++ b/inertial/Makefile @@ -0,0 +1,23 @@ +############################################################################ +# apps/inertial/Makefile +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +MENUDESC = "Inertial Libraries Support" + +include $(APPDIR)/Directory.mk diff --git a/inertial/madgwick/Kconfig b/inertial/madgwick/Kconfig new file mode 100644 index 00000000000..590f51724d2 --- /dev/null +++ b/inertial/madgwick/Kconfig @@ -0,0 +1,21 @@ +# +# For a description of the syntax of this configuration file, +# see the file kconfig-language.txt in the NuttX tools repository. +# + +config LIB_MADGWICK + bool "Madgwick Fusion Algorithm" + default n + depends on ALLOW_MIT_COMPONENTS + ---help--- + Enable madgwick fusion algorithm. + Madgwick is a quick fusion algorithm use integrate + accelerometer, gyroscope and magnotomer (compass). + +if LIB_MADGWICK + +config LIB_MADGWICK_VER + string "Lib Madgwick version" + default "1.2.1" + +endif # LIB_MADGWICK diff --git a/inertial/madgwick/Make.defs b/inertial/madgwick/Make.defs new file mode 100644 index 00000000000..814fb83af50 --- /dev/null +++ b/inertial/madgwick/Make.defs @@ -0,0 +1,26 @@ +############################################################################ +# apps/inertial/madgwick/Make.defs +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +ifneq ($(CONFIG_LIB_MADGWICK),) +CONFIGURED_APPS += $(APPDIR)/inertial/madgwick + +CFLAGS += ${INCDIR_PREFIX}$(APPDIR)/inertial/madgwick/fusion +CXXFLAGS += ${INCDIR_PREFIX}$(APPDIR)/inertial/madgwick/fusion +endif diff --git a/inertial/madgwick/Makefile b/inertial/madgwick/Makefile new file mode 100644 index 00000000000..936d28219f8 --- /dev/null +++ b/inertial/madgwick/Makefile @@ -0,0 +1,47 @@ +############################################################################ +# apps/inertial/madgwick/Makefile +# +# Licensed to the Apache Software Foundation (ASF) under one or more +# contributor license agreements. See the NOTICE file distributed with +# this work for additional information regarding copyright ownership. The +# ASF licenses this file to you under the Apache License, Version 2.0 (the +# "License"); you may not use this file except in compliance with the +# License. You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT +# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the +# License for the specific language governing permissions and limitations +# under the License. +# +############################################################################ + +include $(APPDIR)/Make.defs + +SRC = fusion/Fusion + +CSRCS += $(SRC)/FusionAhrs.c +CSRCS += $(SRC)/FusionCompass.c +CSRCS += $(SRC)/FusionOffset.c + +CFLAGS += -Wno-shadow -Wno-strict-prototypes -Wno-unknown-pragmas + +MODULE = $(CONFIG_LIB_MADGWICK) + +libmadgwick.tar.gz: + $(Q) curl -L https://github.com/xioTechnologies/Fusion/archive/refs/tags/v$(CONFIG_LIB_MADGWICK_VER).tar.gz -o libmadgwick.tar.gz + $(Q) tar xf libmadgwick.tar.gz + $(Q) mv Fusion-$(CONFIG_LIB_MADGWICK_VER) fusion + +# Download and unpack tarball if no git repo found +ifeq ($(wildcard madgwick/fusion),) +context:: libmadgwick.tar.gz + +distclean:: + $(call DELDIR, fusion) + $(call DELFILE, libmadgwick.tar.gz) +endif + +include $(APPDIR)/Application.mk