Created May 15, 2021

Dynamics Tracking Device for Outdoor Activities

Track your detailed dynamics when trailing, hiking, cycling or exercising.

Things used in this project

Hardware components

Nordic Semiconductor nRF5340 Development Kit

SparkFun IMU Breakout - MPU-9250

Nordic Semiconductor Power Profiler Kit II

Case G1389G (Gainta Industries Ltd. )

Outdoor chest pouch (Generic)

Software apps and online services

Nordic Semiconductor nRF Connect SDK

SEGGER Embedded Studio

Hand tools and fabrication machines

Soldering iron (generic)

Story

Introduction

In many practical cases where precise and highly responsive motion tracking is required smartphones do not provide good solution. The main limitations of the smartphone-based solutions are high battery drain, significant delays in gathering and providing data, insufficient precision and the absence of motion details related to the actual user performance along the route in the produced data.

In this project we will build the wearable dynamics tracking device which will use inertial navigation methods. The timed trace of physical intensity of the motion on the 'micro-level', i.e. harmonic parameters of the the motion. Assuming that device will be attached firmly to the user body near the centre of mass this data will provide good foundation for the estimation of the actual intensity of the training activity.

Solution Description

The solution is based on the nRF5340-DK development kit and Sparkfun's MPU-9250 breakout board. The 9-DoF Invensense navigation module MPU9250 consists of an 6-DoF MPU6505 IMU including an accelerometer and a gyroscope and the Asahi-Kosei AK8963 matnetometer module connected to the I2C bus together with the MPU6050.

From a programming point of view the switch from nRF52840-DK to the nRF5340-DK was associated with the move to the RTOS Zephyr which resulted in additional efforts and time but it paid off completely. Overall I got the impression that Zephyr is a way to go for embedded programming and I plan to use this system in my projects whenever possible.

The transition to Zephyr led to the need to modify the existing drivers for the MPU6050 and MPU9150 for the MPU 9250. For the needs of the project it was necessary also to expand the sensor API of Zephyr which turned out to be easy task.

The configuration of the system hardware based on DeviceTree turned out to be very convenient and understandable job. For the solution it required to form a small overlay file which is also attached.

The formation of a set of configuration parameters templates for the added devices turned out to be as simple and intuitive also.

During the design phase the two options for polling sensors were considered - the pull and the push mode. The push mode was chosen in which the sensors periodically publish the digitized measurement results in their registers and the microcontroller periodically polls these registers. In this case when the unread values are overwritten or when the data is not ready the IMU signals the microprocessor with special flags. These flags are displayed in the debug output and can be used when setting the delay for polling sensors by the microcontroller.

The mode of operation based on data readiness and interrupts (the push mode) was also implemented in the MPU9250 drivers but has not been tested.

The solution includes an optional code for calibrating the magnetometer. The calibration of the accelerometer and gyroscope is less relevant and has been postponed.

In the application part of the solution Chris Wiener's implementation of the Sebastian Madgwick's filter made for Arduino was ported.

For field testing and debugging of the system, a case and a bag for attaching the device to a belt or chest were selected and modified appropriately:

Power Profiling with PPK II

During the tests, the average power consumption was measured in the active mode and in the stop polling mode:

Power consumption during active polling

Without polling the IMU continues measurements and is the main power consumer:

Power consumption with polling stopped

Using PPK II built-in logic analyzer it is possible to check e.g. if some peripheral device lines are active when unnecessary:

PPK II sample mixed logic + power trace

Sample Output

The sample output is presented in the picture below.

Sample output

Experimenting with the device under development

Future Development

Below is the main additions and extensions I would like to implement in future the following features:

Calibration for gyroscope and accelerometer.
Interrupt-based polling of sensors.
Geolocation on the base of combination of inertial system and GNSS data.
Complex data analysis, activity recognition and high-level data output.
Add real-time physical user interface based on smart watch or any type of tactile communications for dynamic in-training feedback.
Smartphone integration.
Cloud integration (assumes migration to nRF9160 SiP).

Implementation Guide

Install nRF Connect with SEGGER Embedded Studio.
Attache sensor modulte to nRF5340-DK board according to the attached photos.
Deploy all files in their respective folders including application and the related files in your preferred application folder and driver modules into the relevant system folders (see the comments).
Configure your experiment in the code of the main.c file and enjoy!

Conclusion

The proposed development platform for embedded applications is unique, both from the point of view of the proposed SoC, and from the point of view of development and debugging tools:

The most advanced short-range solutions.
Two modern high-performance Cortex-M33 cores.
Ultra-low power consumption.
Open RTOS Zephyr.
Support of the leading debugging tool-set (SEGGER) and integrated IDE.
Very affordable kit for power profiling with built-in 8-channel logic analyzer.

The initial time investment in Zephyr RTOS is well worth it, and the experience with Nordic Semiconductor tools and documentation, as well as the support in the developer community, has been a purely positive experience.

Nordic Power Profiler Kit II is a product achievement itself: the unique combination of power fine tuning solution and integrated logic analyzer gives completely different level of understanding of the internal life of the tested system than anything available for the comparable budget.

Unfortunately, it was not possible to complete the project according to the initial scope, but the work on the project was a great pleasure.

P.S. About Conservation. The last but not least: both devices came in recyclable packs (compared to e.g. nRF52840-DK which I received last year in the package without the recycling icon).

nRF5340-DK and Power Profiler Kit II recyclable packages

Schematics

Code

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 * Adapted from drivers for MPU6050 and 
 * MPU 9150 by Intel Corporation (c) 2016
 */

#ifndef ZEPHYR_DRIVERS_SENSOR_MPU9250_MPU9250_H_
#define ZEPHYR_DRIVERS_SENSOR_MPU9250_MPU9250_H_

#include <device.h>
#include <drivers/gpio.h>

struct mpu9250_raw_data{
		int16_t A_x;
		int16_t A_y;
		int16_t A_z;
		int16_t T;
		int16_t G_x;
		int16_t G_y;
		int16_t G_z;
		int16_t A_x_off;
		int16_t A_y_off;
		int16_t A_z_off;
		int16_t A_fs;
		int16_t G_x_off;
		int16_t G_y_off;
		int16_t G_z_off;
		int16_t G_fs;
};
typedef struct mpu9250_raw_data mpu9250_raw_data;
//#define MPU9250_ACC_OFFSET_SCALE 9.7656E-4f

struct mpu9250_data {
	const struct device *i2c;
	const struct device *ak8963;
	struct mpu9250_raw_data raw_data;
#ifdef CONFIG_MPU9250_TRIGGER
	const struct device *dev;
	const struct device *gpio;
	struct gpio_callback gpio_cb;
	struct sensor_trigger data_ready_trigger;
	sensor_trigger_handler_t data_ready_handler;
	struct k_work work;
#endif
};

struct mpu9250_config {
	const char *i2c_label;
	uint16_t i2c_addr;
#ifdef CONFIG_MPU9250_TRIGGER
	uint8_t int_pin;
	uint8_t int_flags;
	const char *int_label;
#endif
};

#ifdef CONFIG_MPU9250_TRIGGER
int mpu9250_trigger_set(const struct device *dev,
			const struct sensor_trigger *trig,
			sensor_trigger_handler_t handler);
int mpu9250_init_interrupt(const struct device *dev);
#endif

#endif // ZEPHYR_DRIVERS_SENSOR_MPU9250_MPU9250_H_

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef MPU9250_EXT_H
#define MPU9250_EXT_H

#include <drivers/sensor.h>
#include <sys/util.h>

// Sensor specific channels and attributes
enum mpu9250_channel {
    MPU9250_CHAN_ACCEL = SENSOR_CHAN_ACCEL_XYZ,
    MPU9250_CHAN_GYRO = SENSOR_CHAN_GYRO_XYZ,
    MPU9250_CHAN_TEMP = SENSOR_CHAN_DIE_TEMP,
    MPU9250_CHAN_CONTROL = SENSOR_CHAN_PRIV_START,
    MPU9250_CHAN_END
};
enum mpu9250_attribute {
    MPU9250_ATTR_RANGE = SENSOR_ATTR_FULL_SCALE,
	MPU9250_ATTR_SLEEP = SENSOR_ATTR_PRIV_START,
    MPU9250_ATTR_WAKEUP,
    MPU9250_ATTR_CLOCK_SOURCE,
    MPU9250_ATTR_CLOCK_DIVIDER,
    MPU9250_ATTR_BANDWIDTH,
    MPU9250_ATTR_RESOLUTION,
    MPU9250_ATTR_MODE,
    MPU9250_ATTR_I2C_MODE,
    MPU9250_ATTR_END
};

// System Clock
enum mpu9250_mode { 
    MPU9250_WAKEUP,
    MPU9250_SLEEP 
};
typedef enum mpu9250_mode mpu9250_mode;

// System Clock
enum mpu9250_clock_source { 
    MPU9250_INTERNAL, 
    MPU9250_AUTO 
};
typedef enum mpu9250_clock_source mpu9250_clock_source;

// System Clock Divider
enum mpu9250_clock_divider { 
    MPU9250_1000,
    MPU9250_500,
    MPU9250_333,
    MPU9250_250,
    MPU9250_200,
    MPU9250_167,
    MPU9250_143,
    MPU9250_125,
    MPU9250_111,
    MPU9250_100 
};
typedef enum mpu9250_clock_divider mpu9250_clock_divider;

// MPU9250 I2C mode
enum mpu9250_i2C_mode { 
    MPU9250_BYPASS = 2,
};
typedef enum mpu9250_i2C_mode mpu9250_i2c_mode;

// Gyroscope range
enum mpu9250_gyro_range { MPU9250_G250DPS,
                          MPU9250_G500DPS,
                          MPU9250_G1000DPS,
                          MPU9250_G2000DPS };
typedef enum mpu9250_gyro_range mpu9250_gyro_range;
#define MPU9250_GYRO_RESOLUTION (float[]){7.6294E-3f, \
                                          1.5259E-2f, \
                                          3.0518E-2f, \
                                          6.1036E-2f}

// Gyroscope bandwidth:                     BW    D     FS BW_T D_T
enum mpu9250_gyro_fchoice { MPU9250_FC11 = 0U,   // see dlpf
                            MPU9250_FCX0 = 1U,   // 8800  0.064 32 4000 0.04
                            MPU9250_FC01 = 2U }; // 3600  0.11  32 4000 0.04
typedef enum mpu9250_gyro_fchoice mpu9250_gyro_fchoice;

// Gyroscope low-pass filter            //  BW   D    FS BW_T D_T
enum mpu9250_gyro_dlpf { MPU9250_GLPF_250HZ,    //  250  0.97 8  4000  0.04
                         MPU9250_GLPF_184HZ,    //  184  2.9  1   188  1.9
                         MPU9250_GLPF_92HZ,     //   92  3.9  1    98  2.8
                         MPU9250_GLPF_41HZ,     //   41  5.9  1    42  4.8
                         MPU9250_GLPF_20HZ,     //   20  9.9  1    20  8.3
                         MPU9250_GLPF_10HZ,     //   10 17.85 1    10 13.4
                         MPU9250_GLPF_5HZ,      //    5 33.48 1     5 18.6
                         MPU9250_GLPF_3600HZ }; // 3600  0.17 8  4000  0.04
typedef enum mpu9250_gyro_dlpf mpu9250_gyro_dlpf;

// Accelerometer range
enum mpu9250_accel_range { MPU9250_A2G,
                           MPU9250_A4G,
                           MPU9250_A8G,
                           MPU9250_A16G };
typedef enum mpu9250_accel_range mpu9250_accel_range;                          
#define MPU9250_ACCEL_RESOLUTION (float[]){ 6.1035E-5f, \
                                            1.2207E-4f, \
                                            2.4414E-4f, \
                                            4.8828E-4f }

// Accelerometer bandwidth:  FC         DLPF BW    D     FS
enum mpu9250_accel_fchoice { MPU9250_FC0,   //  x    1130  0.75  4
                             MPU9250_FC1 }; //  See below
typedef enum mpu9250_accel_fchoice mpu9250_accel_fchoice;

// Accelerometer LPF DLPF      BW         D      FS
enum mpu9250_accel_dlpf { MPU9250_ALPF_460HZ_0,  //  1.94  1
                          MPU9250_ALPF_184HZ,  //  5.80  1
                          MPU9250_ALPF_92HZ,   //  7.80  1 
                          MPU9250_ALPF_41HZ,   // 11.80  1
                          MPU9250_ALPF_20HZ,   // 19.80  1
                          MPU9250_ALPF_10HZ,   // 35.70  1
                          MPU9250_ALPF_5HZ,    // 66.96  1
                          MPU9250_ALPF_460HZ_1 };//  1.94  1
typedef enum mpu9250_accel_dlpf mpu9250_accel_dlpf;

// Termometer resolution and offset
#define MPU9250_TEMP_SENSITIVITY 333.87f 
#define MPU9250_TEMP_ROOM_OFFSET 0.0f
#define MPU9250_TEMP_OFFSET 21.0f

#endif // MPU9250_EXT_H

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 * Adapted from drivers for MPU6050 and 
 * MPU 9150 by Intel Corporation (c) 2016
 * SPDX-License-Identifier: Apache-2.0
 */

#define DT_DRV_COMPAT invensense_mpu9250

#include <kernel.h>
#include <drivers/sensor.h>
#include <drivers/i2c.h>
#include <sys/byteorder.h>
#include <sys/util.h>
#include <drivers/sensor/mpu9250_ext.h>
#include "mpu9250.h"

#include <logging/log.h>
LOG_MODULE_REGISTER(MPU9250, CONFIG_SENSOR_LOG_LEVEL);

static int pow2small(unsigned pow) {
	int res = 1;
	for (unsigned i = 0; i < pow; i++)
		res *= 2;
	return res;
}

static int mpu9250_sleep(const struct device *mpu){
	const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x6B, BIT(6), BIT(6));
	if (rc != 0) {
		LOG_ERR("Failed to send MPU9250 to sleep.");
		return rc;
	}
	return 0;
}

static int mpu9250_wakeup(const struct device *mpu){
        const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x6B, BIT(6), 0U);
	if (rc != 0) {
		LOG_ERR("Failed to wake up MPU9250.");
		return rc;
	}
	return 0;
}

static int mpu9250_set_mode(const struct device *mpu, 
                            mpu9250_mode m){
    const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x6B, BIT(6), m << 6);
	if (rc != 0) {
		LOG_ERR("Failed to set MPU9250 mode.");
		return rc;
	}
	return 0;
}

static int mpu9250_set_clock_source(const struct device *mpu, 
                             mpu9250_clock_source cs){
    const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x6B, BIT_MASK(3), cs);
	if (rc != 0) {
		LOG_ERR("Failed to set MPU9250 clock source.");
		return rc;
	}
	return 0;
}

static int mpu9250_set_clock_divider(const struct device *mpu, 
                                     mpu9250_clock_divider cd){
    const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x19, BIT_MASK(3), cd);
	if (rc != 0) {
		LOG_ERR("Failed to set MPU9250 clock divider.");
		return rc;
	}
	return 0;
}

static int mpu9250_set_i2c_mode(const struct device *mpu, 
                                mpu9250_i2c_mode m){
    const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x37, BIT(1), m);
	if (rc != 0) {
		LOG_ERR("Failed to set MPU9250 clock source.");
		return rc;
	}
	return 0;
}

static int mpu9250_set_gyro_range(const struct device *mpu, 
                           mpu9250_gyro_range r){
          struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x1A, BIT(3) | BIT(4), r);
	if (rc != 0) {
		LOG_ERR("Failed to set gyroscope range.");
		return rc;
	}
	mpu_data->raw_data.G_fs = r;
	return 0;
}

static int mpu9250_set_gyro_bandwidth(const struct device *mpu, 
                               		  mpu9250_gyro_dlpf dlpf, 
                               		  mpu9250_gyro_fchoice fchoice){
    const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x1A, BIT_MASK(2), fchoice);
	if (rc != 0) {
		LOG_ERR("Failed to set gyroscope bandwidth.");
		return rc;
	}
	rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x1C, BIT_MASK(3), dlpf);
	if (rc != 0) {
		LOG_ERR("Failed to set gyroscope bandwidth.");
		return rc;
	}
	return 0;
}

static int mpu9250_set_accel_range(const struct device *mpu, 
                            	   mpu9250_accel_range r){
          struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x1B, BIT(3) | BIT(4), r);
	if (rc != 0) {
		LOG_ERR("Failed to set accelerometer range.");
		return rc;
	}
	mpu_data->raw_data.A_fs = r;
	return 0;
}

static int mpu9250_set_accel_bandwidth(const struct device *mpu, 
                              		   mpu9250_accel_dlpf dlpf, 
                              		   mpu9250_accel_fchoice fchoice){
    const struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
	int rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
								 0x1C, BIT_MASK(3), dlpf);
	if (rc != 0) {
		LOG_ERR("Failed to set accelerometer bandwidth.");
		return rc;
	}
	rc = i2c_reg_update_byte(mpu_data->i2c, 
	        			         mpu_cfg->i2c_addr, 
						     0x1C, BIT(3), fchoice);
	if (rc != 0) {
		LOG_ERR("Failed to set accelerometer bandwidth.");
		return rc;
	}
        return 0;
}

static int mpu9250_sample_fetch(const struct device *mpu,
								enum sensor_channel chan) {								
	      struct mpu9250_data *mpu_data = mpu->data;
	const struct mpu9250_config *mpu_cfg = mpu->config;
				 mpu9250_raw_data *frame = &mpu_data->raw_data;
	int16_t buf[7];
	int rc = i2c_burst_read(mpu_data->i2c, mpu_cfg->i2c_addr,
			   				0x3B, (uint8_t *)buf, 14);
	if (rc != 0) {
		LOG_ERR("Failed to read data sample.");
		return rc;
	}
	frame->A_x = sys_be16_to_cpu(buf[0]);
	frame->A_y = sys_be16_to_cpu(buf[1]);
	frame->A_z = sys_be16_to_cpu(buf[2]);
	frame->T = sys_be16_to_cpu(buf[3]);
	frame->G_x = sys_be16_to_cpu(buf[4]);
	frame->G_y = sys_be16_to_cpu(buf[5]);
	frame->G_z = sys_be16_to_cpu(buf[6]);
	return rc;
}

static int mpu9250_channel_get(const struct device *mpu,
			       			   enum sensor_channel chan,
			                   struct sensor_value *val) {								   
	struct mpu9250_raw_data *frame = &((struct mpu9250_data *)mpu->data)->raw_data;
	switch (chan) {
		case SENSOR_CHAN_ACCEL_X:
			val->val1 = frame->A_x /*- frame->A_x_off * 16 / pow2small(frame->A_fs)*/;
			val->val2 = 0;
			break;
		case SENSOR_CHAN_ACCEL_Y:
			val->val1 = frame->A_y /*- frame->A_y_off * 16 / pow2small(frame->A_fs)*/;
			val->val2 = 0;
			break;
		case SENSOR_CHAN_ACCEL_Z:
			val->val1 = frame->A_z /*- frame->A_z_off * 16 / pow2small(frame->A_fs)*/;
			val->val2 = 0;
			break;
		case SENSOR_CHAN_GYRO_X:
			val->val1 = frame->G_x /*- frame->G_x_off * 4 / pow2small(frame->G_fs)*/;
			val->val2 = 0;
			//val->val2 = -frame->G_x_off * 4 % pow2small(frame->G_fs) * 1000000;
			break;
		case SENSOR_CHAN_GYRO_Y:
			val->val1 = frame->G_y /*- frame->G_y_off * 4 / pow2small(frame->G_fs)*/;
			val->val2 = 0;
			//val->val2 = -frame->G_y_off * 4 % pow2small(frame->G_fs) * 1000000;
			break;
		case SENSOR_CHAN_GYRO_Z:
			val->val1 = frame->G_z /*- frame->G_z_off * 4 / pow2small(frame->G_fs)*/;
			val->val2 = 0;
			//val->val2 = -frame->G_z_off * 4 % pow2small(frame->G_fs) * 1000000;
			break;
		case SENSOR_CHAN_DIE_TEMP:
			val->val1 = frame->T;
			val->val2 = 0;
			break;
		default: // Any other channel - unsupported
			LOG_WRN("Unsuported channel.");
			val->val1 = 0;
			val->val2 = 0;
	}
	return 0;	
}

static int mpu9250_attr_set(const struct device *mpu,
				 			enum sensor_channel schan,
				 			enum sensor_attribute sattr,
				 			const struct sensor_value *val){
	int rc;
	enum mpu9250_channel chan = (enum mpu9250_channel)schan;
	enum mpu9250_attribute attr = (enum mpu9250_attribute)sattr;
	switch (chan) {
		case MPU9250_CHAN_CONTROL:
			if (attr == MPU9250_ATTR_SLEEP) {
				rc = mpu9250_sleep(mpu);
			}
			else {
				if (attr == MPU9250_ATTR_WAKEUP) {
					rc = mpu9250_wakeup(mpu);
				}
				else {
					if (attr == MPU9250_ATTR_CLOCK_SOURCE) {
						rc = mpu9250_set_clock_source(mpu, val->val1);
					}
					else {
						if(attr == MPU9250_ATTR_CLOCK_DIVIDER) {
							rc = mpu9250_set_clock_divider(mpu, val->val1);
						}
						else {
							if (attr == MPU9250_ATTR_MODE) {
								rc = mpu9250_set_mode(mpu, val->val1);
							}
							else {
								if(attr == MPU9250_ATTR_I2C_MODE) {
									rc = mpu9250_set_i2c_mode(mpu, val->val1);
								}
								else {
									LOG_WRN("Unexpected device attribute");
									rc = EINVAL;
								}
							}
						}	
					}
				}	
			}
			break;
		case MPU9250_CHAN_GYRO:
			if (attr == MPU9250_ATTR_RANGE) {
				rc = mpu9250_set_gyro_range(mpu, val->val1);
			}
			else {
				if (attr == MPU9250_ATTR_BANDWIDTH){
					rc = mpu9250_set_gyro_bandwidth(mpu, val->val1, val->val2);
				}
				else {
					LOG_WRN("Unexpected device attribute");
					rc = EINVAL;
				}
			}
			break;
		case MPU9250_CHAN_ACCEL:
			if (attr == MPU9250_ATTR_RANGE) {
				rc = mpu9250_set_accel_range(mpu, val->val1);
			}
			else {
				if (attr == MPU9250_ATTR_BANDWIDTH){
					rc = mpu9250_set_accel_bandwidth(mpu, val->val1, val->val2);
				}
				else {
					LOG_WRN("Unexpected device attribute");
					rc = EINVAL;
				}
			}
			break;	
	/*	case MPU9250_CHAN_TEMP:
			if (attr == MPU9250_ATTR_RESOLUTION) {
				rc = mpu9250_set_temp_resolution_offset(mpu, val->val1, val->val2);
			}
			else {
				LOG_WRN("Unexpected device attribute");
				rc = EINVAL;
			}
			break;	*/
		default: // Any other channel - unsupported
			LOG_WRN("Unsuported channel.");
			rc = EINVAL;
	}
	return rc;	
}

static const struct sensor_driver_api mpu9250_driver_api = {
#if CONFIG_MPU9250_TRIGGER
	.trigger_set = mpu9250_trigger_set,
#endif
	.sample_fetch = mpu9250_sample_fetch,
	.channel_get = mpu9250_channel_get,
	.attr_set = mpu9250_attr_set,
};

int mpu9250_init(const struct device *dev)
{
	      struct mpu9250_data *drv_data = dev->data;
	const struct mpu9250_config *cfg = dev->config;
				 mpu9250_raw_data *frame = &drv_data->raw_data;
	drv_data->i2c = device_get_binding(cfg->i2c_label);
	if (drv_data->i2c == NULL) {
		LOG_ERR("Failed to get pointer to %s device", cfg->i2c_label);
		return -EINVAL;
	}
	int rc;
	// Wakeup chip
	rc = i2c_reg_update_byte(drv_data->i2c, cfg->i2c_addr, 0x6B, BIT(6), 0);
	if (rc != 0) {
		LOG_ERR("Failed to awake MPU9250.");
		return rc;
	} 
	// Check chip ID 
	uint8_t id;
	rc = i2c_reg_read_byte(drv_data->i2c, cfg->i2c_addr, 0x75, &id);  
	if (rc != 0) {
		LOG_ERR("Failed to read chip ID.");
		return rc;
	}
	if (id != 0x71) {
		LOG_ERR("Invalid chip ID: %X.", id);
		return -EINVAL;
	}
	// Wait for initialization of the registers
	k_sleep(K_MSEC(300));
	// Read factory gyroscope offset data
	int16_t buf[3];
	rc = i2c_burst_read(drv_data->i2c, cfg->i2c_addr,
			   				0x13, (uint8_t *)buf, 6);
	if (rc != 0) {
		LOG_ERR("Failed to read gyroscope calibration data.");
		return rc;
	}
	frame->G_x_off = sys_be16_to_cpu(buf[0]);
	frame->G_y_off = sys_be16_to_cpu(buf[1]);
	frame->G_z_off = sys_be16_to_cpu(buf[2]);
	// Read factory accelerometer offset data
	rc = i2c_burst_read(drv_data->i2c, cfg->i2c_addr,
			   				0x77, (uint8_t *)buf, 6);
	if (rc != 0) {
		LOG_ERR("Failed to read accelerometer calibration data.");
		return rc;
	}
	frame->A_x_off = sys_be16_to_cpu(buf[0]);
	frame->A_y_off = sys_be16_to_cpu(buf[1]);
	frame->A_z_off = sys_be16_to_cpu(buf[2]);
	// Enable MPU9250 pass-through to have access to ak8963 
	// [TBD] Make it configurable behavior
	rc = i2c_reg_update_byte(drv_data->i2c, cfg->i2c_addr,
				0x37, BIT(1), BIT(1));
	if (rc != 0) {
		LOG_ERR("Failed to enable pass-through mode for MPU9250.");
		return rc;
	}
	// Link AK8963
/*	const char * const label1 = DT_LABEL(DT_INST(0, asahi_kasei_ak8963));
	const struct device *ak8963 = device_get_binding(label1);
	if (!ak8963) {
		LOG_ERR("Failed to find sensor %s", label1);
	}
	drv_data->ak8963 = ak8963; */
	// Send chips to sleep
	rc = i2c_reg_update_byte(drv_data->i2c, cfg->i2c_addr, 0x6B, BIT(6), BIT(6));
	if (rc != 0) {
		LOG_ERR("Failed to send MPU9250 to sleep.");
		return rc;
	} 
	else
		LOG_DBG("MPU9250 is ready."); 
#ifdef CONFIG_MPU9250_TRIGGER
	if (mpu9250_init_interrupt(dev) < 0) {
		LOG_ERR("Failed to initialize interrupts.");
		return -EIO;
	}
	else { 
		LOG_DBG("Interrupt initialized successfully.");
	}
#endif
	return 0;
}

static struct mpu9250_data data;
static const struct mpu9250_config config = {
	.i2c_label = DT_INST_BUS_LABEL(0),
	.i2c_addr = DT_INST_REG_ADDR(0),
#ifdef CONFIG_MPU9250_TRIGGER
	.int_pin = DT_INST_GPIO_PIN(0, int_gpios),
	.int_flags = DT_INST_GPIO_FLAGS(0, int_gpios),
	.int_label = DT_INST_GPIO_LABEL(0, int_gpios),
#endif // CONFIG_MPU9250_TRIGGER 
};

DEVICE_DT_INST_DEFINE(0, mpu9250_init, device_pm_control_nop, &data, &config, 
                      POST_KERNEL, CONFIG_SENSOR_INIT_PRIORITY, &mpu9250_driver_api);

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 * Adapted from drivers for MPU6050 and 
 * MPU 9150 by Intel Corporation (c) 2016
 */

#include <drivers/i2c.h>
#include <sys/util.h>
#include <kernel.h>
#include <drivers/sensor.h>
#include <device.h>
#include <logging/log.h>
#include <drivers/gpio.h>

#include "mpu9250.h"

LOG_MODULE_DECLARE(MPU9250, CONFIG_SENSOR_LOG_LEVEL);

// Method of MPU9250 driver API: sensor_trigger_set(...)
// Invoked from the user application
int mpu9250_trigger_set(const struct device *dev,
			const struct sensor_trigger *trig,
			sensor_trigger_handler_t handler) {
	struct mpu9250_data *drv_data = dev->data;
	const struct mpu9250_config *cfg = dev->config;
	if (trig->type != SENSOR_TRIG_DATA_READY) {
		return -ENOTSUP;
	}
	gpio_pin_interrupt_configure(drv_data->gpio, cfg->int_pin,
				     GPIO_INT_DISABLE);
	drv_data->data_ready_handler = handler;
	if (handler == NULL) {
		return 0;
	}
	drv_data->data_ready_trigger = *trig;
	gpio_pin_interrupt_configure(drv_data->gpio, cfg->int_pin,
				     GPIO_INT_EDGE_TO_ACTIVE);
	return 0;
}

// Callback initialization. Invoked on sensor initialization
// during system start from sensor initialization procedure.
static void mpu9250_gpio_callback(const struct device *dev,
				  struct gpio_callback *cb, uint32_t pins)
{
	struct mpu9250_data *drv_data =
		CONTAINER_OF(cb, struct mpu9250_data, gpio_cb);
	const struct mpu9250_config *cfg = drv_data->dev->config;
	ARG_UNUSED(pins);
	gpio_pin_interrupt_configure(drv_data->gpio, cfg->int_pin,
				     GPIO_INT_DISABLE);
	k_work_submit(&drv_data->work);
}

// Handler invoked from handler proxy. Invokes the actual
// handler for the actual trigger set by user application.
static void mpu9250_thread_cb(const struct device *dev)
{
	struct mpu9250_data *drv_data = dev->data;
	const struct mpu9250_config *cfg = dev->config;
	if (drv_data->data_ready_handler != NULL) {
		drv_data->data_ready_handler(dev,
					     &drv_data->data_ready_trigger);
	}
	gpio_pin_interrupt_configure(drv_data->gpio, cfg->int_pin,
				     GPIO_INT_EDGE_TO_ACTIVE);
}

// Handler proxy (?). Invoked by system queue processor
// on work execution start. Set on interrupt setup.
static void mpu9250_work_cb(struct k_work *work){
	struct mpu9250_data *drv_data =
		CONTAINER_OF(work, struct mpu9250_data, work);
	mpu9250_thread_cb(drv_data->dev);
}

// Interrupt setup procedure. Invoked by sensor driver
// setup procedure on system start.
int mpu9250_init_interrupt(const struct device *mpu)
{
	struct mpu9250_data *drv_data = mpu->data;
	const struct mpu9250_config *cfg = mpu->config;

	// setup data ready gpio interrupt 
	drv_data->gpio = device_get_binding(cfg->int_label);
	if (drv_data->gpio == NULL) {
		LOG_ERR("Failed to get pointer to %s device",
			    cfg->int_label);
		return -EINVAL;
	}
	
	drv_data->dev = mpu;

	gpio_pin_configure(drv_data->gpio, cfg->int_pin,
			   GPIO_INPUT | cfg->int_flags);
	gpio_init_callback(&drv_data->gpio_cb,
			   mpu9250_gpio_callback,
			   BIT(cfg->int_pin));
	if (gpio_add_callback(drv_data->gpio, &drv_data->gpio_cb) < 0) {
		LOG_ERR("Failed to set gpio callback");
		return -EIO;
	}
	
	// Enable data ready interrupt
	if (i2c_reg_write_byte(drv_data->i2c, cfg->i2c_addr,
			       0x38, BIT(0)) < 0) {
		LOG_ERR("Failed to enable data ready interrupt.");
		return -EIO;
	}

	drv_data->work.handler = mpu9250_work_cb;

	gpio_pin_interrupt_configure(drv_data->gpio, cfg->int_pin,
				     GPIO_INT_EDGE_TO_ACTIVE);

	return 0;
}

# MPU9250 Nine-Axis Motion Tracking device configuration options
# This file describes options for MPU9050 and for MPU6050 inside;
# See the configuration for built-in AK8963
# Copyright (c) 2021 Anton Bondarenko
# SPDX-License-Identifier: Apache-2.0

menuconfig MPU9250
	bool "MPU9250 Nine-Axis Motion Tracking Device"
	depends on I2C
	help
	  Enable driver for MPU9250 I2C-based nine-axis motion tracking device.

if MPU9250

config MPU9250_TRIGGER
	bool "MPU9250 Interrupt"
	depends on GPIO
	help
		Enable interrupt-based pull mode
endif # MPU9250

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 * Based on driver for AK8975 by 
 * Copyright (c) 2016 Intel Corporation
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef ZEPHYR_DRIVERS_SENSOR_AK8963_AK8963_H_
#define ZEPHYR_DRIVERS_SENSOR_AK8963_AK8963_H_

#include <device.h>

struct ak8963_raw_data{
		int16_t M_x;
		int16_t M_y;
		int16_t M_z;
		int8_t M_x_off;
		int8_t M_y_off;
		int8_t M_z_off;
		bool valid;
};
typedef struct ak8963_raw_data ak8963_raw_data;

struct ak8963_data {
	const struct device *i2c;
	struct ak8963_raw_data raw_data;
};

struct ak8963_config {
	const char *i2c_label;
	uint16_t i2c_addr;
};

#endif // ZEPHYR_DRIVERS_SENSOR_AK8963_AK8963_H_

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef AK8963_EXT_H
#define AK8963_EXT_H

#include <drivers/sensor.h>
#include <drivers/sensor/mpu9250_ext.h>

// Sensor specific channels
enum ak8963_channel {
    AK8963_CHAN_CONTROL = SENSOR_CHAN_PRIV_START,
    AK8963_CHAN_MAGN = SENSOR_CHAN_MAGN_XYZ,
    AK8963_CHAN_MAGN_RAW_X = MPU9250_CHAN_END,
    AK8963_CHAN_MAGN_RAW_Y,
    AK8963_CHAN_MAGN_RAW_Z,
};

// Sensor specific attributes
enum ak8963_attribute {
    AK8963_ATTR_RESOLUTION = MPU9250_ATTR_RESOLUTION,
    AK8963_ATTR_MODE = MPU9250_ATTR_MODE,
};

// Magnetometer resolution:
enum ak8963_mag_resolution { AK8963_14B,
                             AK8963_16B };
typedef enum ak8963_mag_resolution ak8963_mag_resolution;                       
#define AK8963_MAG_RESOLUTION (float[]){ 5.8594E-1f, \
                                          1.4648E-1f }

// Magnetometer mode of operations:
enum ak8963_mode { AK8963_MODE_DOWN   = 0U,  // 0000 Power-down mode
                   AK8963_MODE_SNGL   = 1U,  // 0001 Single measurement mode
                   AK8963_MODE_8HZ    = 2U,  // 0010 Continuous measurement mode 1 (8Hz)
                   AK8963_MODE_100HZ  = 6U,  // 0110 Continuous measurement mode 2 (100Hz)
                   AK8963_MODE_TRIG   = 4U,  // 0100 External trigger measurement mode
                   AK8963_MODE_TEST   = 8U,  // 1000 Self-test mode
                   AK8963_MODE_FUSE   = 15U };  // 1111 Fuse ROM access mode
typedef enum ak8963_mode ak8963_mode;

#define AK8963_MAX_READING_ATTEMPTS 10000

#endif // AK89863_EXT_H

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 * Based on driver for AK8975 by
 * Copyright (c) 2016 Intel Corporation
 * SPDX-License-Identifier: Apache-2.0
 */

#define DT_DRV_COMPAT asahi_kasei_ak8963

#include <kernel.h>
#include <drivers/sensor.h>
#include <drivers/i2c.h>
#include <sys/util.h>
#include <drivers/sensor/ak8963_ext.h>
#include "ak8963.h"

#include <logging/log.h>
LOG_MODULE_REGISTER(AK8963, CONFIG_SENSOR_LOG_LEVEL);

static int ak8963_set_mag_resolution(const struct device *ak, 
                               		 ak8963_mag_resolution res){
    const struct ak8963_data *ak_data = ak->data;
	const struct ak8963_config *ak_cfg = ak->config;
	int rc = i2c_reg_update_byte(ak_data->i2c, 
	        			         ak_cfg->i2c_addr, 
								 0x0A, BIT(4), res << 4);
	if (rc != 0) {
		LOG_ERR("Failed to set magnetometer resolution.");
		return rc;
	}
	return 0;
}

static int ak8963_set_mag_mode(const struct device *ak, 
						 	   ak8963_mode mode){
    const struct ak8963_data *ak_data = ak->data;
	const struct ak8963_config *ak_cfg = ak->config;
	int rc = i2c_reg_update_byte(ak_data->i2c, 
	        			         ak_cfg->i2c_addr, 
								 0x0A, BIT_MASK(4), mode);
	if (rc != 0) {
		LOG_ERR("Failed to set magnetometer mode.");
		return rc;
	}
	return 0;
}

static int ak8963_sample_fetch(const struct device *ak,
							   enum sensor_channel chan) {
	      struct ak8963_data *ak_data = ak->data;
	const struct ak8963_config *ak_cfg = ak->config;
				 ak8963_raw_data *frame = &ak_data->raw_data;
	int16_t buf[3];
	uint8_t reg;
	int rc;
	bool dnrdy = true;
	bool ovrlfl = false;
	uint32_t count = AK8963_MAX_READING_ATTEMPTS;
	while (dnrdy && count > 0) {
		rc = i2c_reg_read_byte(ak_data->i2c, ak_cfg->i2c_addr, 0x02, &reg);
		if (rc != 0) {
			LOG_ERR("Failed to read magnetometer status.");
			return rc;
		}					   
		dnrdy = !(reg & BIT(0));
		ovrlfl = (reg & BIT(1));
		if(dnrdy) {
			LOG_DBG("Magnetometer data not ready, attemp # %d",
			AK8963_MAX_READING_ATTEMPTS - count + 1);
			k_sleep(K_MSEC(1));	
			rc = i2c_reg_read_byte(ak_data->i2c, ak_cfg->i2c_addr,
			   				   0x09, &reg);
			if (rc != 0) {
				LOG_ERR("Failed to clean magnetometer status.");
				return rc;
			}		
		}
		if(ovrlfl) { // We don't clean status as it will be cleaned on reading
			LOG_WRN("Magnetometer overrun, attemp # %d",
			AK8963_MAX_READING_ATTEMPTS - count + 1);
		}
		count--;
	}
	if(!count) { 
		LOG_WRN("Magnetometer excided the limit of reading attempts.");
		return EBUSY;
	}
	uint8_t dreg = 0x03;
	for(unsigned i = 0; i < 6; i++) {
		rc = i2c_reg_read_byte(ak_data->i2c, ak_cfg->i2c_addr,
			   				   dreg++, &(((uint8_t *)buf)[i]));
		if (rc != 0) {
			LOG_ERR("Failed to read data sample.");
			return rc;
		}
		rc = i2c_reg_read_byte(ak_data->i2c, ak_cfg->i2c_addr,
			   				   0x09, &reg);
		if (rc != 0) {
			LOG_ERR("Failed to read magnetometer status.");
			return rc;
		}
		rc = 0;
		
		if(reg & BIT(3)) { 
			LOG_WRN("Magnetometer overflow.");
		}
		if(/* ovrlfl || */ reg & BIT(3)) {
			rc = EINVAL;
			frame->valid = false;
		}
		else {
			rc = 0;
			frame->valid = true;
		}
	
	}
	frame->M_x = buf[0];
	frame->M_y = buf[1];
	frame->M_z = buf[2];
	return rc;
}

static int ak8963_channel_get(const struct device *ak,
			       			   enum sensor_channel chan,
			                   struct sensor_value *val) {								   
	struct ak8963_data *ak_data = ak->data;
	       ak8963_raw_data *frame = &ak_data->raw_data;
	switch (chan) {
		case SENSOR_CHAN_MAGN_XYZ:
			val->val1 = (int32_t)frame->valid;
			val->val2 = 0; 
			break;
		case SENSOR_CHAN_MAGN_X:
			val->val1 = frame->M_x + frame->M_x * (frame->M_x_off - 128) / 256;
			val->val2 = frame->M_x * (frame->M_x_off - 128) * 1000000 % 256 / 256; 
			break;
		case SENSOR_CHAN_MAGN_Y:
			val->val1 = frame->M_y + frame->M_y * (frame->M_y_off - 128) / 256;
			val->val2 = frame->M_y * (frame->M_y_off - 128) * 1000000 % 256 / 256; 
			break;
		case SENSOR_CHAN_MAGN_Z:
			val->val1 = frame->M_z + frame->M_z * (frame->M_z_off - 128) / 256;
			val->val2 = frame->M_z * (frame->M_z_off - 128) * 1000000 % 256 / 256; 
			break;
		case AK8963_CHAN_MAGN_RAW_X:
			val->val1 = frame->M_x;
			val->val2 = 0; 
			break;
		case AK8963_CHAN_MAGN_RAW_Y:
			val->val1 = frame->M_y;
			val->val2 = 0; 
			break;
		case AK8963_CHAN_MAGN_RAW_Z:
			val->val1 = frame->M_z;
			val->val2 = 0; 
			break;	
		default: // Any other channel - unsupported
			LOG_WRN("Unsuported channel.");
	}
	return 0;	
}

static int ak8963_attr_set(const struct device *ak,
				 		   enum sensor_channel schan,
				 		   enum sensor_attribute sattr,
				 		   const struct sensor_value *val){
	int rc = 0;
	enum ak8963_channel chan = (enum ak8963_channel)schan;
	enum ak8963_attribute attr = (enum ak8963_attribute)sattr;
	switch (chan) {
		case AK8963_CHAN_CONTROL:
			if (attr == AK8963_ATTR_MODE) {
				rc = ak8963_set_mag_mode(ak, val->val1);
			}
			else {
				LOG_WRN("Unexpected device attribute");
				rc = EINVAL;
			}
			break;
		case AK8963_CHAN_MAGN:
			if (attr == AK8963_ATTR_RESOLUTION) {
				rc = ak8963_set_mag_resolution(ak, val->val1);
			}
			break;	
		default: // Any other channel - unsupported
			LOG_WRN("Unsuported channel.");
			rc = EINVAL;
	}
	return rc;	
}

static const struct sensor_driver_api ak8963_driver_api = {
	.sample_fetch = ak8963_sample_fetch,
	.channel_get = ak8963_channel_get,
	.attr_set = ak8963_attr_set,
};

int ak8963_init(const struct device *ak)
{
	      struct ak8963_data *ak_data = ak->data;
	const struct ak8963_config *ak_cfg = ak->config;
	ak_data->i2c = device_get_binding(ak_cfg->i2c_label);
	if (ak_data->i2c == NULL) {
		LOG_ERR("Failed to get pointer to %s device!", ak_cfg->i2c_label);
		return -EINVAL;
	}
	int rc;
	// Wakeup chip
	rc = i2c_reg_update_byte(ak_data->i2c, ak_cfg->i2c_addr, 0x0A, BIT_MASK(4), BIT(1) | BIT(2));
	if (rc != 0) {
		LOG_ERR("Failed to wakeup AK8963.");
		return rc;
	} 
	// Check chip ID 
	uint8_t id;
	rc = i2c_reg_read_byte(ak_data->i2c, ak_cfg->i2c_addr, 0x00, &id); 
	if (rc != 0) {
		LOG_ERR("Failed to read chip ID.");
		return rc;
	}
	if (id != 0x48) {
		LOG_ERR("Invalid chip ID.");
		return -EINVAL;
	}
	// Wait for initialization of the registers
	k_sleep(K_MSEC(300));
	// Enter AK8963 fuse mode
	rc = i2c_reg_update_byte(ak_data->i2c, ak_cfg->i2c_addr, 0x0A, BIT_MASK(4), BIT_MASK(4));
	if (rc != 0) {
		LOG_ERR("Failed to enter AK8963 fuse mode.");
		return rc;
	} 
	// Write down AK8963 factory calibration data
	rc = i2c_burst_read(ak_data->i2c, ak_cfg->i2c_addr, 0x10, 
	                    (uint8_t *)&ak_data->raw_data.M_x_off, 6); 
	if (rc != 0) {
		LOG_ERR("Failed to read fuse register.");
		return rc;
	}
	// Sent chip to slip
	rc = i2c_reg_update_byte(ak_data->i2c, ak_cfg->i2c_addr, 0x0A, BIT_MASK(4), 0);
	if (rc != 0) {
		LOG_ERR("Failed to send AK8963 to sleep.");
		return rc;
	} 
	LOG_DBG("AK8963 is ready.");
	return 0;
}

static struct ak8963_data ak8963_data;
static const struct ak8963_config ak8963_config = {
	.i2c_label = DT_INST_BUS_LABEL(0),
	.i2c_addr = DT_INST_REG_ADDR(0),
};

DEVICE_DT_INST_DEFINE(0, ak8963_init, device_pm_control_nop, 
					  &ak8963_data, &ak8963_config, POST_KERNEL, 
					  CONFIG_SENSOR_INIT_PRIORITY, &ak8963_driver_api);

# SPDX-License-Identifier: Apache-2.0

add_subdirectory_ifdef(CONFIG_ADT7420		adt7420)
add_subdirectory_ifdef(CONFIG_ADXL345		adxl345)
add_subdirectory_ifdef(CONFIG_ADXL362		adxl362)
add_subdirectory_ifdef(CONFIG_ADXL372		adxl372)
add_subdirectory_ifdef(CONFIG_AK8975		ak8975)
add_subdirectory_ifdef(CONFIG_AK8963		ak8963)
add_subdirectory_ifdef(CONFIG_AMG88XX		amg88xx)
add_subdirectory_ifdef(CONFIG_AMS_IAQ_CORE	ams_iAQcore)
add_subdirectory_ifdef(CONFIG_APDS9960		apds9960)
add_subdirectory_ifdef(CONFIG_BMA280		bma280)
add_subdirectory_ifdef(CONFIG_BMC150_MAGN	bmc150_magn)
add_subdirectory_ifdef(CONFIG_BME280		bme280)
add_subdirectory_ifdef(CONFIG_BME680		bme680)
add_subdirectory_ifdef(CONFIG_BMG160		bmg160)
add_subdirectory_ifdef(CONFIG_BMI160		bmi160)
add_subdirectory_ifdef(CONFIG_BMM150		bmm150)
add_subdirectory_ifdef(CONFIG_BQ274XX		bq274xx)
add_subdirectory_ifdef(CONFIG_CCS811		ccs811)
add_subdirectory_ifdef(CONFIG_DHT			dht)
add_subdirectory_ifdef(CONFIG_DPS310		dps310)
add_subdirectory_ifdef(CONFIG_ENS210		ens210)
add_subdirectory_ifdef(CONFIG_FXAS21002		fxas21002)
add_subdirectory_ifdef(CONFIG_FXOS8700		fxos8700)
add_subdirectory(grove)
add_subdirectory_ifdef(CONFIG_TI_HDC		ti_hdc)
add_subdirectory_ifdef(CONFIG_HMC5883L		hmc5883l)
add_subdirectory_ifdef(CONFIG_HP206C		hp206c)
add_subdirectory_ifdef(CONFIG_HTS221		hts221)
add_subdirectory_ifdef(CONFIG_IIS2DH		iis2dh)
add_subdirectory_ifdef(CONFIG_IIS2DLPC		iis2dlpc)
add_subdirectory_ifdef(CONFIG_IIS2ICLX		iis2iclx)
add_subdirectory_ifdef(CONFIG_IIS2MDC		iis2mdc)
add_subdirectory_ifdef(CONFIG_IIS3DHHC		iis3dhhc)
add_subdirectory_ifdef(CONFIG_ISL29035		isl29035)
add_subdirectory_ifdef(CONFIG_ISM330DHCX	ism330dhcx)
add_subdirectory_ifdef(CONFIG_LIS2DH		lis2dh)
add_subdirectory_ifdef(CONFIG_LIS2DS12		lis2ds12)
add_subdirectory_ifdef(CONFIG_LIS2DW12          lis2dw12)
add_subdirectory_ifdef(CONFIG_LIS2MDL		lis2mdl)
add_subdirectory_ifdef(CONFIG_LIS3MDL		lis3mdl)
add_subdirectory_ifdef(CONFIG_LPS22HB		lps22hb)
add_subdirectory_ifdef(CONFIG_LPS22HH		lps22hh)
add_subdirectory_ifdef(CONFIG_LPS25HB		lps25hb)
add_subdirectory_ifdef(CONFIG_LSM303DLHC_MAGN	lsm303dlhc_magn)
add_subdirectory_ifdef(CONFIG_LSM6DS0		lsm6ds0)
add_subdirectory_ifdef(CONFIG_LSM6DSL		lsm6dsl)
add_subdirectory_ifdef(CONFIG_LSM6DSO		lsm6dso)
add_subdirectory_ifdef(CONFIG_LSM9DS0_GYRO	lsm9ds0_gyro)
add_subdirectory_ifdef(CONFIG_LSM9DS0_MFD	lsm9ds0_mfd)
add_subdirectory_ifdef(CONFIG_MAX17055		max17055)
add_subdirectory_ifdef(CONFIG_MAX30101		max30101)
add_subdirectory_ifdef(CONFIG_MAX44009		max44009)
add_subdirectory_ifdef(CONFIG_MCP9808		mcp9808)
add_subdirectory_ifdef(CONFIG_MPR			mpr)
add_subdirectory_ifdef(CONFIG_MPU6050		mpu6050)
add_subdirectory_ifdef(CONFIG_MPU9250		mpu9250)
add_subdirectory_ifdef(CONFIG_MS5607		ms5607)
add_subdirectory_ifdef(CONFIG_MS5837		ms5837)
add_subdirectory_ifdef(CONFIG_OPT3001		opt3001)
add_subdirectory_ifdef(CONFIG_PMS7003		pms7003)
add_subdirectory_ifdef(CONFIG_QDEC_NRFX		qdec_nrfx)
add_subdirectory_ifdef(CONFIG_TEMP_NRF5		nrf5)
add_subdirectory_ifdef(CONFIG_SHT3XD		sht3xd)
add_subdirectory_ifdef(CONFIG_SI7006		si7006)
add_subdirectory_ifdef(CONFIG_SI7055		si7055)
add_subdirectory_ifdef(CONFIG_SI7060		si7060)
add_subdirectory_ifdef(CONFIG_SM351LT		sm351lt)
add_subdirectory_ifdef(CONFIG_STTS751		stts751)
add_subdirectory_ifdef(CONFIG_SX9500		sx9500)
add_subdirectory_ifdef(CONFIG_TH02		    th02)
add_subdirectory_ifdef(CONFIG_TMP007		tmp007)
add_subdirectory_ifdef(CONFIG_TMP112		tmp112)
add_subdirectory_ifdef(CONFIG_TMP116		tmp116)
add_subdirectory_ifdef(CONFIG_VCNL4040		vcnl4040)
add_subdirectory_ifdef(CONFIG_VL53L0X		vl53l0x)
add_subdirectory_ifdef(CONFIG_TEMP_KINETIS	nxp_kinetis_temp)
add_subdirectory_ifdef(CONFIG_TACH_XEC		mchp_tach_xec)
add_subdirectory_ifdef(CONFIG_ITDS		wsen_itds)
add_subdirectory_ifdef(CONFIG_MCUX_ACMP		mcux_acmp)

zephyr_sources_ifdef(CONFIG_USERSPACE sensor_handlers.c)
zephyr_sources_ifdef(CONFIG_SENSOR_SHELL sensor_shell.c)
zephyr_sources_ifdef(CONFIG_SENSOR_SHELL_BATTERY shell_battery.c)

/*
 * by: Kris Winer (c) 2014
 * Very small adaptation to remove globals 
 * and switch to arrays (hopefully)
 * by Anton Bondarenko 2021
 */

// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!

#include "filter.h"

float sqrtf(float value){
	float sqrt = value / 3;
	if (value <= 0) {
		return 0;
	}
	for (int i = 0; i < 6; i++) {
		sqrt = (sqrt + value / sqrt) / 2;
	}
	return sqrt;
}

        void MadgwickQuaternionUpdate(float* q, float deltat, float beta, 
                                      const float *a, const float *g, const float *m){
            float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
            float ax = a[0]*1E6f, ay = a[1]*1E6f, az = a[2]*1E6f;
            float gx = g[0]*M_PI/180.0f, gy = g[1]*M_PI/180.0f, gz = g[2]*M_PI/180.0f;
            float mx = m[0]*10.0f, my = m[1]*10.0f, mz = m[2]*10.0f;
            float norm;
            float hx, hy, _2bx, _2bz;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1mx;
            float _2q1my;
            float _2q1mz;
            float _2q2mx;
            float _4bx;
            float _4bz;
            float _2q1 = 2.0f * q1;
            float _2q2 = 2.0f * q2;
            float _2q3 = 2.0f * q3;
            float _2q4 = 2.0f * q4;
            float _2q1q3 = 2.0f * q1 * q3;
            float _2q3q4 = 2.0f * q3 * q4;
            float q1q1 = q1 * q1;
            float q1q2 = q1 * q2;
            float q1q3 = q1 * q3;
            float q1q4 = q1 * q4;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q2q4 = q2 * q4;
            float q3q3 = q3 * q3;
            float q3q4 = q3 * q4;
            float q4q4 = q4 * q4;

            // Normalise accelerometer measurement
            norm = sqrtf(ax * ax + ay * ay + az * az);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            ax *= norm;
            ay *= norm;
            az *= norm;

            // Normalise magnetometer measurement
            norm = sqrtf(mx * mx + my * my + mz * mz);
            if (norm == 0.0f) return; // handle NaN
            norm = 1.0f/norm;
            mx *= norm;
            my *= norm;
            mz *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2.0f * q1 * mx;
            _2q1my = 2.0f * q1 * my;
            _2q1mz = 2.0f * q1 * mz;
            _2q2mx = 2.0f * q2 * mx;
            hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx = sqrtf(hx * hx + hy * hy);
            _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx = 2.0f * _2bx;
            _4bz = 2.0f * _2bz;

            // Gradient decent algorithm corrective step
            s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            norm = 1.0f/norm;
            s1 *= norm;
            s2 *= norm;
            s3 *= norm;
            s4 *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

            // Integrate to yield quaternion
            q1 += qDot1 * deltat;
            q2 += qDot2 * deltat;
            q3 += qDot3 * deltat;
            q4 += qDot4 * deltat;
            norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
            norm = 1.0f/norm;
            q[0] = q1 * norm;
            q[1] = q2 * norm;
            q[2] = q3 * norm;
            q[3] = q4 * norm;

            return;
        }

/*
 * Copyright (c) 2021 Anton Bondarenko
 * SPDX-License-Identifier: Apache-2.0
 * Based on demo for MPU9150
 * Copyright (c) 2019 Nordic Semiconductor ASA
 * SPDX-License-Identifier: Apache-2.0
 */

#include <kernel.h>
#include <sys_clock.h>
#include <errno.h>
#include <zephyr.h>
#include <device.h>
#include <drivers/i2c.h>
#include <drivers/sensor.h>
#include <drivers/sensor/mpu9250_ext.h>
#include <drivers/sensor/ak8963_ext.h>
#include <stdio.h>
#include <math.h>
#include <logging/log.h>
LOG_MODULE_REGISTER(main);
#include "filter.h"
#define CONFIG_MPU9250_POLL_TIMEOUT 1 // move to config
#define CONFIG_MPU9250_CALIBRATION_TIMEOUT 10
#define CONFIG_AK8963_REFORM_FLUX false	// move to config
#define CONFIG_AK8963_CALIBRATE_SAMPLES 1500 // move to config

float ak8963_mag_bias[3] = {0.0, 0.0, 0.0};
float ak8963_mag_deform[3] = {1.0, 1.0, 1.0};
struct sensors {
	const struct device *mpu;
	const struct device *ak;
} sensors;

struct resolution {
	float A;
	float G;
	float T;
	float M;
} resolution;

static const char *now_str(void) {
	static char buf[16]; // ...HH:MM:SS.MMM 
	uint32_t now = k_uptime_get_32();
	uint32_t ms = now % MSEC_PER_SEC;
	uint32_t s, min, h;
	now /= MSEC_PER_SEC;
	s = now % 60U;
	now /= 60U;
	min = now % 60U;
	now /= 60U;
	h = now;
	snprintf(buf, sizeof(buf), "%u:%02u:%02u.%03u",
		 h, min, s, ms);
	return buf;
}

static inline float sensor_value_to_float(struct sensor_value *val) {
	return (float)val->val1 + (float)val->val2 / 1000000.0;
}

static inline float get_scaled_T(struct sensor_value *val) {
	return (sensor_value_to_float(val) - MPU9250_TEMP_ROOM_OFFSET) /
			MPU9250_TEMP_SENSITIVITY + MPU9250_TEMP_OFFSET;
}

static float get_scaled_A(struct sensor_value *val) {
	return sensor_value_to_float(val) * resolution.A;
}

static float get_scaled_G(struct sensor_value *val) {
	return sensor_value_to_float(val) * resolution.G;
}

static float get_scaled_M(struct sensor_value *val) {
	return sensor_value_to_float(val) * resolution.M;
}

static void mpu9250_print_data_frame(const struct device *mpu) {
	float buf[7];
	struct sensor_value val;
	int i = 0;
	sensor_channel_get(mpu, SENSOR_CHAN_DIE_TEMP, &val);
	buf[i++] = get_scaled_T(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_X, &val);
	buf[i++] = get_scaled_A(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_Y, &val);
	buf[i++] = get_scaled_A(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_Z, &val);
	buf[i++] = get_scaled_A(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_X, &val);
	buf[i++] = get_scaled_G(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_Y, &val);
	buf[i++] = get_scaled_G(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_Z, &val);
	buf[i++] = get_scaled_G(&val);
	printf("[%s]:%g Cel\n"
		   "  accel %f %f %f G\n"
		   "  gyro  %f %f %f deg/s\n",
		   now_str(), buf[0], 
		   buf[1], buf[2], buf[3], 
		   buf[4], buf[5], buf[6]);
}

static void mpu9250_print_raw_data_frame(const struct device *mpu) {
	int32_t buf[6];
	struct sensor_value val;
	int i = 0;
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_X, &val);
	buf[i++] = val.val1;
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_Y, &val);
	buf[i++] = val.val1;
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_Z, &val);
	buf[i++] = val.val1;
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_X, &val);
	buf[i++] = val.val1;
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_Y, &val);
	buf[i++] = val.val1;
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_Z, &val);
	buf[i++] = val.val1;
	printf(",accel,%d,%d,%d,,\n"
		   ",gyro,%d,%d,%d,,\n",
		   buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
}

static void ak8963_print_data_frame(const struct device *ak) {
	float buf[3];
	struct sensor_value val;
	int i = 0;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_X, &val);
	buf[i++] = (get_scaled_M(&val) - ak8963_mag_bias[0])/* * ak8963_mag_deform[0]*/;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_Y, &val);
	buf[i++] = (get_scaled_M(&val) - ak8963_mag_bias[1])/* * ak8963_mag_deform[1]*/;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_Z, &val);
    buf[i++] = (get_scaled_M(&val) - ak8963_mag_bias[2])/* * ak8963_mag_deform[2]*/;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_XYZ, &val);
	if(val.val1) {
		printf("  magn  %f %f %f mkT\n",buf[0], buf[1], buf[2]);
	}
	else{
		printf("  magn  %f %f %f mkT (INVALID?)\n",buf[0], buf[1], buf[2]);
	}
}

static float ak8963_get_M(const struct device *ak, enum sensor_channel chan) {
	float buf;
	struct sensor_value val;
	sensor_channel_get(ak, chan, &val);
	buf = (get_scaled_M(&val) - ak8963_mag_bias[0]);
	if(CONFIG_AK8963_REFORM_FLUX) {
		buf = buf * ak8963_mag_deform[chan - SENSOR_CHAN_MAGN_X]; 
	}
	return buf;
}

static void ak8963_print_raw_data_frame(const struct device *ak) {
	int32_t buf[3];
	struct sensor_value val;
	int i = 0;
	sensor_channel_get(ak, AK8963_CHAN_MAGN_RAW_X, &val);
	buf[i++] = val.val1;
	sensor_channel_get(ak, AK8963_CHAN_MAGN_RAW_Y, &val);
	buf[i++] = val.val1;
	sensor_channel_get(ak, AK8963_CHAN_MAGN_RAW_Z, &val);
    buf[i++] = val.val1;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_XYZ, &val);
	if(val.val1) {
		printf(",magn,%d,%d,%d,true\n", buf[0], buf[1], buf[2]);
	}
	else{
		printf(",magn,%d,%d,%d,true\n", buf[0], buf[1], buf[2]);
	}
}

static int process_mpu9250(const struct device *mpu) {
	int rc = sensor_sample_fetch(mpu);
	if( rc != 0){
		LOG_ERR("Failed to read raw data from MPU9250: %d", rc);
		return rc;
	}
	// do magic a lot
	// mpu9250_print_data_frame(mpu);
	// burst data to smartphone or to flash
	return rc;
}

static int process_ak8963(const struct device *ak) {
	int rc = sensor_sample_fetch(ak);
	if( rc == 0){
		// do magic a lot
		// ak8963_print_data_frame(ak);
		// burst data to smartphone or to flash

	}
	else {
		if(rc == EINVAL) {
			//LOG_DBG("Skipping damaged data frame.");
			//ak8963_print_data_frame(ak);
			rc = 0;
		}
		else {	
			LOG_ERR("Failed to read raw data from AK8963: %d", rc);
		}
	}
	return rc;
}

static void get_measured_data(const struct device *mpu, const struct device *ak, 
                              float *a, float *g, float *m) {
    struct sensor_value val;
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_X, &val);
	a[0] = get_scaled_A(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_Y, &val);
	a[1] = get_scaled_A(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_ACCEL_Z, &val);
	a[2] = get_scaled_A(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_X, &val);
	g[0] = get_scaled_G(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_Y, &val);
	g[1] = get_scaled_G(&val);
	sensor_channel_get(mpu, SENSOR_CHAN_GYRO_Z, &val);
	g[2] = get_scaled_G(&val); 
    sensor_channel_get(ak, SENSOR_CHAN_MAGN_X, &val);
	m[0] = (get_scaled_M(&val) - ak8963_mag_bias[0])/* * ak8963_mag_deform[0]*/;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_Y, &val);
	m[1] = (get_scaled_M(&val) - ak8963_mag_bias[1])/* * ak8963_mag_deform[1]*/;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_Z, &val);
    m[2] = (get_scaled_M(&val) - ak8963_mag_bias[2])/* * ak8963_mag_deform[2]*/;
	sensor_channel_get(ak, SENSOR_CHAN_MAGN_XYZ, &val);
    return;
}

static void log_data(bool ok, 
					 const float *a, const float *g, const float *m, const float *q, 
					 float yaw, float pitch, float roll){
	if(ok) {
		printf("\n,,");
	}
	else {
		printf("\n,Invalid?,");
	}
	printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
	       a[0],a[1],a[2],g[0],g[1],g[2],m[0],m[1],m[2],q[0],q[1],q[2],q[3],yaw,pitch,roll);
}

static int calibrate_ak8963(const struct device *ak) {
	int rc = 0;
	int i, j;
	float buf;
	float bias_min[3];
	float bias_max[3];
	struct sensor_value val;
rep:k_sleep(K_MSEC(CONFIG_MPU9250_CALIBRATION_TIMEOUT));
	rc = sensor_sample_fetch(ak);
	if( rc == 0) {
		for(j = 0; j < 3; j++) {
			sensor_channel_get(ak, SENSOR_CHAN_MAGN_X + j, &val);
			buf = get_scaled_M(&val);
			bias_max[j] = buf;
			bias_min[j] = buf;
		}
	}
	else{
		if(rc == EINVAL){
			rc = 0;
			goto rep;
		}
		else {
			LOG_ERR("Calibration of AK8963 failed");
			return rc;
		}
	}
	for(i = 0; i < 3; i++) {
		ak8963_mag_bias[i] = 0.0;
		ak8963_mag_deform[i] = 1.0;
	}
	LOG_DBG("Calibration started.");
	for(i = 0; i < CONFIG_AK8963_CALIBRATE_SAMPLES; i++) {
		k_sleep(K_MSEC(CONFIG_MPU9250_CALIBRATION_TIMEOUT));
		rc = sensor_sample_fetch(ak);
		if( rc == 0) {
			for(j = 0; j < 3; j++) {
				sensor_channel_get(ak, SENSOR_CHAN_MAGN_X + j, &val);
				buf = get_scaled_M(&val);
				if(buf > bias_max[j]) {
					bias_max[j] = buf;
				}
				if(buf < bias_min[j]) {
					bias_min[j] = buf;
				}
			}
		}
		else{
			if(rc == EINVAL){
				rc = 0;
				continue;
			}
			else {
				return rc;
			}
		}
	}
	for(j = 0; j < 3; j++) {
		ak8963_mag_bias[j] = (bias_min[j] + bias_max[j]) / 2;
		ak8963_mag_deform[j] = (bias_max[j] - bias_min[j]) / 2;
	}
	buf = (ak8963_mag_deform[0] + ak8963_mag_deform[1] + ak8963_mag_deform[2]) / 3.0;
	for(j = 0; j < 3; j++) {
		ak8963_mag_deform[j] = buf / ak8963_mag_deform[j];
	}
	k_sleep(K_MSEC(100));
	LOG_DBG("Calibration completed.");
	return rc;
}

#ifdef CONFIG_MPU9250_TRIGGER
static struct sensor_trigger trigger;

static void handle_mpu9250_drdy(const struct device *dev,
				struct sensor_trigger *trig){
	int rc = process_mpu9250(sensors.mpu);
	if (rc != 0) {
		LOG_ERR("Cancelling trigger due to MPU9250 failure: %d\n", rc);
		(void)sensor_trigger_set(dev, trig, NULL);
	}
	int rc = process_ak8963(sensors.ak);
	if (rc != 0) {
		LOG_ERR("Cancelling trigger due to AK8963 failure: %d\n", rc);
		(void)sensor_trigger_set(dev, trig, NULL);
	}
}
#endif // CONFIG_MPU9250_TRIGGER 

void main(void) {
	int rc = 0;
	k_sleep(K_MSEC(400));
	// Setup MPU9250 device
	const char *const label0 = DT_LABEL(DT_INST(0, invensense_mpu9250));
	const struct device *mpu9250 = device_get_binding(label0);
	if (!mpu9250) {
		LOG_ERR("Failed to find sensor %s", label0);
		return;
	}
	sensors.mpu = mpu9250;
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_CONTROL, MPU9250_ATTR_CLOCK_SOURCE, 
				  &(struct sensor_value){ MPU9250_AUTO, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set MPU9250 clock source.");
		return;
	}
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_CONTROL, MPU9250_ATTR_CLOCK_DIVIDER, 
				  &(struct sensor_value){ MPU9250_1000, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set MPU9250 clock divider.");
		return;
	}
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_CONTROL, MPU9250_ATTR_WAKEUP, 
				  &(struct sensor_value){ 0, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to wakeup MPU9250.");
		return;
	}
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_CONTROL, MPU9250_ATTR_I2C_MODE,
				  &(struct sensor_value){ MPU9250_BYPASS, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set I2C bypass mode for AK8963.");
		return;
	}
	//int mpu_gyro_range = MPU9250_G250DPS;
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_GYRO, MPU9250_ATTR_RANGE,
				  &(struct sensor_value){ MPU9250_G2000DPS, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set gyrospope full-scale range.");
		return;
	}
	resolution.G = MPU9250_GYRO_RESOLUTION[MPU9250_G250DPS];
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_GYRO, MPU9250_ATTR_BANDWIDTH,
				  &(struct sensor_value){ MPU9250_GLPF_20HZ, MPU9250_FC11 });
	if(rc != 0) {
		LOG_ERR("Failed to set gyrospope bandwidth.");
		return;
	}
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_ACCEL, MPU9250_ATTR_RANGE,
				  &(struct sensor_value){ MPU9250_A2G, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set accelerometer full-scale range.");
		return;
	}
	resolution.A = MPU9250_ACCEL_RESOLUTION[MPU9250_A2G];
	//int mpu_accel_range = MPU9250_A2G;
	rc = sensor_attr_set(mpu9250, MPU9250_CHAN_ACCEL, MPU9250_ATTR_BANDWIDTH,
				  &(struct sensor_value){ MPU9250_ALPF_20HZ, MPU9250_FC1 });
	if(rc != 0) {
		LOG_ERR("Failed to set accelerometer bandwidth.");
		return;
	}
	/* sensor_attr_set(mpu9250, MPU9250_CHAN_TEMP, MPU9250_ATTR_RESOLUTION,
			 &(struct sensor_value){ MPU9250_TEMP_RESOLUTION, MPU9250_TEMP_OFFSET }); */
			 
	// Setup AK8963 device
	const char * const label1 = DT_LABEL(DT_INST(0, asahi_kasei_ak8963));
	const struct device *ak8963 = device_get_binding(label1);
	if (!ak8963) {
		LOG_ERR("Failed to find sensor %s", label1);
		return;
	}
	sensors.ak = ak8963;
	rc = sensor_attr_set(ak8963, AK8963_CHAN_MAGN, AK8963_ATTR_RESOLUTION,
				  &(struct sensor_value){ AK8963_16B, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set magnetometer resolution.");
		return;
	}
	resolution.M = AK8963_MAG_RESOLUTION[AK8963_16B];
	rc = sensor_attr_set(ak8963, AK8963_CHAN_CONTROL, AK8963_ATTR_MODE,
				  &(struct sensor_value){ AK8963_MODE_100HZ, 0 });
	if(rc != 0) {
		LOG_ERR("Failed to set magnetometer mode/bandwidth.");
		return;
	}
	// Setup NEO-6 device
	// LOG_DBG("Sensor drivers loaded and configured successfully");
	// Establish BLE to smartphone
	// LOG_DBG("Smartphone connected successfully");
#ifdef CONFIG_MPU9250_TRIGGER
	// Arm trigger for data ready signal of MPU6050
	trigger = (struct sensor_trigger) {
		.type = SENSOR_TRIG_DATA_READY,
		.chan = SENSOR_CHAN_ALL,
	};
	if (sensor_trigger_set(mpu9250, &trigger,
			       handle_mpu9250_drdy) < 0) {
		LOG_ERR("Failed to configure trigger.");
		//exit(EINVAL);
		//return;
	};
	LOG_DBG("Configured for triggered sampling.");
#endif
	LOG_DBG("SYSTEM IS UP");
	//k_sleep(K_MSEC(5000));
	//rc = calibrate_ak8963(ak8963);
	if(rc != 0) {
		LOG_ERR("AK8963 calibration failed: %d", rc);
		return;
	}

	// by Kris Wiener:
	// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
	float GyroMeasError = M_PI * (40.0f / 180.0f);   // gyroscope measurement error in rads/s (start at 40 deg/s)
	float GyroMeasDrift = M_PI * (0.0f  / 180.0f);   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
	float beta = sqrtf(3.0f / 4.0f) * GyroMeasError;   // compute beta
//	float zeta = sqrtf(3.0f / 4.0f) * GyroMeasDrift;   // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
//	#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
//	#define Ki 0.0f
	float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};    // vector to hold quaternion
	float deltat = 0.0f;
// float ax, ay, az, gx, gy, gz, mx, my, mz;
	float a[3];
	float g[3];
	float m[3];
	float pitch, yaw, roll;
	uint32_t lastUpdate = 0, now = 0;   // used to calculate integration interval
	
	//printf("\n\n\n\n[DATA OUTPUT START]");
	//printf("\n - Validity - Accelerometer (3) - Gyroscope (3) - Magnetometer (3) - Yaw - Pitch - Roll\n\n");
	
	LOG_DBG("MAIN LOOP START");
	for(int i = 0; i < 1000; i++) {
	//while (!IS_ENABLED(CONFIG_MPU9250_TRIGGER)) {
		rc = process_mpu9250(mpu9250);
		if (rc != 0) {
			LOG_DBG("ERROR READING MPU9250 DATA");
			break;
		}
		rc = process_ak8963(ak8963);
		if (rc != 0) {
			LOG_DBG("ERROR READING AK8963 DATA");
			break;
		}

		get_measured_data(mpu9250, ak8963, a, g, m);

		now = k_uptime_get_32();

  		deltat = ((now - lastUpdate)/1000.0f); // set integration time by time elapsed since last filter update
  		lastUpdate = now;

		MadgwickQuaternionUpdate(q, deltat, beta, a, g, m);

		// by: Kris Winer (c) 
		// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
		// In this coordinate system, the positive z-axis is down toward Earth. 
		// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
		// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
		// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
		// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
		// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
		// applied in the correct order which for this configuration is yaw, pitch, and then roll.
		// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
    	yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
    	pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
    	roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
    	pitch *= 180.0f / M_PI;
    	yaw   *= 180.0f / M_PI; 
    	// yaw   -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
		yaw -= 11.4; // Declination in Moscow, Russia is 11.382 on 2021-05-15.
    	roll  *= 180.0f / M_PI;
/*
		struct sensor_value val;
		sensor_channel_get(ak8963, SENSOR_CHAN_MAGN_XYZ, &val);
		bool ok = (val.val1 != 0);
		log_data(ok, a, g, m, q, yaw, pitch, roll);
*/		
		k_sleep(K_MSEC(CONFIG_MPU9250_POLL_TIMEOUT));
	}
	LOG_DBG("MAIN LOOP END");

	//printf("\n[DATA OUTPUT END]\n\n");
}

/*
 * Copyright (c) 2019 Nordic Semiconductor ASA
 *
 * SPDX-License-Identifier: Apache-2.0
 */

&i2c1 {
	compatible = "nordic,nrf-twim";
	clock-frequency = <I2C_BITRATE_FAST>;
	status = "okay";
	mpu9250@68 {
		compatible = "invensense,mpu9250";
		reg = <0x68>;
		status = "okay";
		label = "MPU9250";
		int-gpios = <&gpio0 36 GPIO_ACTIVE_HIGH>; 
	};
	ak8963@0C {
		compatible = "asahi-kasei,ak8963";
		reg = <0x0C>;
		status = "okay";
		label = "AK8963"; 
	};
};

CONFIG_I2C=y
CONFIG_SENSOR=y
CONFIG_NRFX_TWIM=y
CONFIG_NRFX_TWIM1=y
CONFIG_I2C_NRFX=y
CONFIG_SENSOR_LOG_LEVEL_ERR=y
CONFIG_SENSOR_LOG_LEVEL_WRN=y
CONFIG_SENSOR_LOG_LEVEL_DBG=y
# CONFIG_SENSOR_LOG_LEVEL=2
# CONFIG_SENSOR_INIT_PRIORITY=90

CONFIG_MPU9250=y
CONFIG_AK8963=y
# CONFIG_MPU9250_TRIGGER=y

CONFIG_LOG=y

# ++
CONFIG_UART_CONSOLE=n
CONFIG_USE_SEGGER_RTT=n
CONFIG_RTT_CONSOLE=n
CONFIG_LOG_DEFAULT_LEVEL=4

# --
# CONFIG_LOG_BACKEND_UART=y

CONFIG_CBPRINTF_FP_SUPPORT=y
CONFIG_CBPRINTF_LIBC_SUBSTS=y

CONFIG_NEWLIB_LIBC=y
CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y
CONFIG_CMSIS_DSP_BASICMATH=y
CONFIG_CMSIS_DSP_COMPLEXMATH=y
CONFIG_CMSIS_DSP_FASTMATH=y

Credits

Anton Bondarenko

3 projects • 7 followers

Embed the widget on your own site

Dynamics Tracking Device for Outdoor Activities

Dynamics Tracking Device for Outdoor Activities

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Introduction

Solution Description

Power Profiling with PPK II

Sample Output

Future Development

Implementation Guide

Conclusion

Custom parts and enclosures

Case Gainta g1389 Drawing

Schematics

PPK II connection

MPU-9250 Connection

nRF5340 connection

Code

mpu9250.h

mpu9250_ext.h

mpu9250.c

mpu9250_trigger.c

Kconfig

CMakeLists.txt

ak8963.h

ak8963_ext.h

ak8963.c

Kconfig

CMakeLists.txt

CMakeLists.txt

Madgwick filter for AHRS (Attitude and heading reference system)

Madgwick filter for AHRS (Attitude and heading reference system)

main.c

nrf5340dk_nrf5340_cpuappns.overlay

prj.conf

CMakeLists.txt for main program

Credits

Anton Bondarenko

Comments