Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

examples/sensor_fusion: Support for Madgwick Fusion lib and example #2113

Merged
merged 1 commit into from
Oct 19, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
38 changes: 38 additions & 0 deletions examples/sensor_fusion/Kconfig
Original file line number Diff line number Diff line change
@@ -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
24 changes: 24 additions & 0 deletions examples/sensor_fusion/Make.defs
Original file line number Diff line number Diff line change
@@ -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
30 changes: 30 additions & 0 deletions examples/sensor_fusion/Makefile
Original file line number Diff line number Diff line change
@@ -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
162 changes: 162 additions & 0 deletions examples/sensor_fusion/sensor_fusion_main.c
Original file line number Diff line number Diff line change
@@ -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
****************************************************************************/
fdcavalcanti marked this conversation as resolved.
Show resolved Hide resolved

#include <nuttx/config.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <nuttx/sensors/sensor.h>
#include "Fusion/Fusion.h"

fdcavalcanti marked this conversation as resolved.
Show resolved Hide resolved
/****************************************************************************
* Pre-processor Definitions
****************************************************************************/

#define REG_LOW_MASK 0xFF00
#define REG_HIGH_MASK 0x00FF
#define MPU6050_FS_SEL 32.8f
#define MPU6050_AFS_SEL 4096.0f

/****************************************************************************
* Private 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, 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;
fdcavalcanti marked this conversation as resolved.
Show resolved Hide resolved
FusionVector accelerometer;

Check failure on line 79 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found
FusionVector gyroscope;

Check failure on line 80 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found
FusionEuler euler;

Check failure on line 81 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found
FusionAhrs ahrs;

Check failure on line 82 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found

FusionAhrsInitialise(&ahrs);

Check failure on line 84 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found

printf("Sensor Fusion example\n");
printf("Sample Rate: %.2f Hz\n", 1.0 / acq_period);

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

accelerometer.axis.x = imu_acc_data.x;
accelerometer.axis.y = imu_acc_data.y;
accelerometer.axis.z = imu_acc_data.z;

gyroscope.axis.x = imu_gyro_data.x;
gyroscope.axis.y = imu_gyro_data.y;
gyroscope.axis.z = imu_gyro_data.z;

FusionAhrsUpdateNoMagnetometer(&ahrs,

Check failure on line 108 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found
gyroscope,
accelerometer,
acq_period);
euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&ahrs));

Check failure on line 112 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found

Check failure on line 112 in examples/sensor_fusion/sensor_fusion_main.c

View workflow job for this annotation

GitHub Actions / check

Mixed case identifier found

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;
int16_t raw_data[7];
memset(&raw_imu, 0, sizeof(raw_imu));

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;
}
1 change: 1 addition & 0 deletions inertial/.gitignore
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
/Kconfig
21 changes: 21 additions & 0 deletions inertial/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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")
21 changes: 21 additions & 0 deletions inertial/Make.defs
Original file line number Diff line number Diff line change
@@ -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)
23 changes: 23 additions & 0 deletions inertial/Makefile
Original file line number Diff line number Diff line change
@@ -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
21 changes: 21 additions & 0 deletions inertial/madgwick/Kconfig
Original file line number Diff line number Diff line change
@@ -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
26 changes: 26 additions & 0 deletions inertial/madgwick/Make.defs
Original file line number Diff line number Diff line change
@@ -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
Loading
Loading