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:
parent
ce2ee9bb3f
commit
36a9ccc3a8
38
examples/sensor_fusion/Kconfig
Normal file
38
examples/sensor_fusion/Kconfig
Normal 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
|
24
examples/sensor_fusion/Make.defs
Normal file
24
examples/sensor_fusion/Make.defs
Normal 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
|
30
examples/sensor_fusion/Makefile
Normal file
30
examples/sensor_fusion/Makefile
Normal 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
|
162
examples/sensor_fusion/sensor_fusion_main.c
Normal file
162
examples/sensor_fusion/sensor_fusion_main.c
Normal 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
1
inertial/.gitignore
vendored
Normal file
@ -0,0 +1 @@
|
||||
/Kconfig
|
21
inertial/CMakeLists.txt
Normal file
21
inertial/CMakeLists.txt
Normal 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
21
inertial/Make.defs
Normal 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
23
inertial/Makefile
Normal 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
21
inertial/madgwick/Kconfig
Normal 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
|
26
inertial/madgwick/Make.defs
Normal file
26
inertial/madgwick/Make.defs
Normal 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
|
47
inertial/madgwick/Makefile
Normal file
47
inertial/madgwick/Makefile
Normal 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
|
Loading…
Reference in New Issue
Block a user