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

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

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

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

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 <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;
+}

inertial/.gitignore

@@ -0,0 +1 @@
+/Kconfig

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

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)

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

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

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