Makefile - library download and compilation ok

Working read from mpu6050 - fixed makefile issues

Succesfull yaw pitch roll reads

Linter fixes - still erroring on mixed case

Added inertial lib

Fixed makefile to use inertial lib

Kconfig fix
This commit is contained in:
Filipe Cavalcanti 2023-10-07 15:29:01 -03:00 committed by Alan Carvalho de Assis
parent ce2ee9bb3f
commit 36a9ccc3a8
11 changed files with 414 additions and 0 deletions

View File

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

View File

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

View File

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

View File

@ -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 <nuttx/config.h>
#include <stdio.h>
#include <fcntl.h>
#include <unistd.h>
#include <nuttx/sensors/sensor.h>
#include "Fusion/Fusion.h"
/****************************************************************************
* 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;
FusionVector accelerometer;
FusionVector gyroscope;
FusionEuler euler;
FusionAhrs ahrs;
FusionAhrsInitialise(&ahrs);
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,
gyroscope,
accelerometer,
acq_period);
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;
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
inertial/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/Kconfig

21
inertial/CMakeLists.txt Normal file
View File

@ -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
inertial/Make.defs Normal file
View File

@ -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
inertial/Makefile Normal file
View File

@ -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
inertial/madgwick/Kconfig Normal file
View File

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

View File

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

View File

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