DMP-ICM20948/src/Icm20948Augmented.c

/*
* ________________________________________________________________________________________________________
* Copyright © 2014-2015 InvenSense Inc. Portions Copyright © 2014-2015 Movea. All rights reserved.
* This software, related documentation and any modifications thereto (collectively “Software”) is subject
* to InvenSense and its licensors' intellectual property rights under U.S. and international copyright and
* other intellectual property rights laws.
* InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
* and any use, reproduction, disclosure or distribution of the Software without an express license
* agreement from InvenSense is strictly prohibited.
* ________________________________________________________________________________________________________
*/

#include "Icm20948.h"
#include "Icm20948Augmented.h"
#include "Icm20948DataConverter.h"

#include "Icm20948DataBaseControl.h"

// Determine the fastest ODR for all gravity-based sensors
#define AUGMENTED_SENSOR_GET_6QUAT_MIN_ODR(s, newOdr) \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_GRAVITY)) \
		newOdr = MIN(s->sGravityOdrMs,newOdr); \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_GAME_ROTATION_VECTOR)) \
		newOdr = MIN(s->sGrvOdrMs,newOdr);  \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_LINEAR_ACCELERATION)) \
		newOdr = MIN(s->sLinAccOdrMs,newOdr);
#define AUGMENTED_SENSOR_GET_6QUATWU_MIN_ODR(s, newOdr) \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_WAKEUP_GRAVITY)) \
		newOdr = MIN(s->sGravityWuOdrMs,newOdr); \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR)) \
		newOdr = MIN(s->sGrvWuOdrMs,newOdr);  \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION)) \
		newOdr = MIN(s->sLinAccWuOdrMs,newOdr);

// Determine the fastest ODR for all rotation vector-based sensors
#define AUGMENTED_SENSOR_GET_9QUAT_MIN_ODR(s, newOdr) \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_ORIENTATION)) \
		newOdr = MIN(s->sOriOdrMs,newOdr); \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_ROTATION_VECTOR)) \
		newOdr = MIN(s->sRvOdrMs,newOdr);
#define AUGMENTED_SENSOR_GET_9QUATWU_MIN_ODR(s, newOdr) \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_WAKEUP_ORIENTATION)) \
		newOdr = MIN(s->sOriWuOdrMs,newOdr); \
	if	(inv_icm20948_ctrl_androidSensor_enabled	(s, ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR)) \
		newOdr = MIN(s->sRvWuOdrMs,newOdr);

int inv_icm20948_augmented_init(struct inv_icm20948 * s)
{
	// ODR expected for gravity-based sensors
	s->sGravityOdrMs = 0xFFFF;
	s->sGrvOdrMs = 0xFFFF;
	s->sLinAccOdrMs = 0xFFFF;
	s->sGravityWuOdrMs = 0xFFFF;
	s->sGrvWuOdrMs = 0xFFFF;
	s->sLinAccWuOdrMs = 0xFFFF;
	// ODR expected for rotation vector-based sensors
	s->sRvOdrMs = 0xFFFF;
	s->sOriOdrMs = 0xFFFF;
	s->sRvWuOdrMs = 0xFFFF;
	s->sOriWuOdrMs = 0xFFFF;
	
	return 0;
}

int inv_icm20948_augmented_sensors_get_gravity(struct inv_icm20948 * s, long gravity[3], const long quat6axis_3e[3])
{
	long quat6axis_4e[4];
	long quat6axis_4e_body_to_world[4];

	if(!gravity) return -1;
	if(!quat6axis_3e) return -1;

	// compute w element
	inv_icm20948_convert_compute_scalar_part_fxp(quat6axis_3e, quat6axis_4e);
	// apply mounting matrix
	inv_icm20948_q_mult_q_qi(quat6axis_4e, s->s_quat_chip_to_body, quat6axis_4e_body_to_world);

	gravity[0] = ( 2 * inv_icm20948_convert_mult_qfix_fxp(quat6axis_4e_body_to_world[1], quat6axis_4e_body_to_world[3], 30) - 
	               2 * inv_icm20948_convert_mult_qfix_fxp(quat6axis_4e_body_to_world[0], quat6axis_4e_body_to_world[2], 30) ) >> (30 - 16);
	gravity[1] = ( 2 * inv_icm20948_convert_mult_qfix_fxp(quat6axis_4e_body_to_world[2], quat6axis_4e_body_to_world[3], 30) + 
	               2 * inv_icm20948_convert_mult_qfix_fxp(quat6axis_4e_body_to_world[0], quat6axis_4e_body_to_world[1], 30) ) >> (30 - 16);
	gravity[2] = ( (1 << 30) - 2 * inv_icm20948_convert_mult_qfix_fxp(quat6axis_4e_body_to_world[1], quat6axis_4e_body_to_world[1], 30) - 
	                2 * inv_icm20948_convert_mult_qfix_fxp(quat6axis_4e_body_to_world[2], quat6axis_4e_body_to_world[2], 30) ) >> (30 - 16);

	return MPU_SUCCESS;
}

int inv_icm20948_augmented_sensors_get_linearacceleration(long linacc[3], const long gravity[3], const long accel[3])
{
    if(!linacc) return -1;
    if(!gravity) return -1;
    if(!accel) return -1;
    
    linacc[0] = accel[0] - gravity[0];
    linacc[1] = accel[1] - gravity[1];
    linacc[2] = accel[2] - gravity[2];
                    
    return MPU_SUCCESS;
}


int inv_icm20948_augmented_sensors_get_orientation(long orientation[3], const long quat9axis_3e[4])
{
    long lQuat9axis4e[4];
	long lMatrixQ30[9];       
	long lMatrixQ30Square; 
	long lRad2degQ16 = 0x394BB8; // (float)(180.0 / 3.14159265358979) in Q16
    
    if(!orientation) return -1;
    if(!quat9axis_3e) return -1;
    
    // compute w element
	inv_icm20948_convert_compute_scalar_part_fxp(quat9axis_3e, lQuat9axis4e);
    
	// quaternion to a rotation matrix, q30 to q30
	inv_icm20948_convert_quat_to_col_major_matrix_fxp((const long *)lQuat9axis4e, (long *)lMatrixQ30);

	// compute orientation in q16
	// orientationFlt[0] = atan2f(-matrixFlt[1][0], matrixFlt[0][0]) * rad2deg;
	orientation[0] = inv_icm20948_math_atan2_q15_fxp(-lMatrixQ30[3] >> 15, lMatrixQ30[0] >> 15) << 1;
	orientation[0] = inv_icm20948_convert_mult_qfix_fxp(orientation[0], lRad2degQ16, 16);

	// orientationFlt[1] = atan2f(-matrixFlt[2][1], matrixFlt[2][2]) * rad2deg;
	orientation[1] = inv_icm20948_math_atan2_q15_fxp(-lMatrixQ30[7] >> 15, lMatrixQ30[8] >> 15) << 1;
	orientation[1] = inv_icm20948_convert_mult_qfix_fxp(orientation[1], lRad2degQ16, 16);

	// orientationFlt[2] = asinf ( matrixFlt[2][0]) * rad2deg;
	// asin(x) = atan (x/sqrt(1-x²))
	// atan2(y,x) = atan(y/x)
	// asin(x) = atan2(x, sqrt(1-x²))
	lMatrixQ30Square = inv_icm20948_convert_mult_qfix_fxp(lMatrixQ30[6], lMatrixQ30[6], 30); // x²
	lMatrixQ30Square = (1UL << 30) - lMatrixQ30Square; // 1-x²
	lMatrixQ30Square = inv_icm20948_convert_fast_sqrt_fxp(lMatrixQ30Square); // sqrt(1-x²)
	orientation[2] = inv_icm20948_math_atan2_q15_fxp(lMatrixQ30[6] >> 15,  lMatrixQ30Square >> 15) << 1; // atan2(x, sqrt(1-x²))
	orientation[2] = inv_icm20948_convert_mult_qfix_fxp(orientation[2], lRad2degQ16, 16); // * rad2deg

	if (orientation[0] < 0)
		orientation[0] += 360UL << 16;

    return MPU_SUCCESS;
}

unsigned short inv_icm20948_augmented_sensors_set_odr(struct inv_icm20948 * s, unsigned char androidSensor, unsigned short delayInMs)
{
	switch(androidSensor)
	{
		case ANDROID_SENSOR_GRAVITY:
			s->sGravityOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_6QUAT_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_GAME_ROTATION_VECTOR:
			s->sGrvOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_6QUAT_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_LINEAR_ACCELERATION:
			s->sLinAccOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_6QUAT_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_ORIENTATION:
			s->sOriOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_9QUAT_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_ROTATION_VECTOR:
			s->sRvOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_9QUAT_MIN_ODR(s, delayInMs);
			break;
		case ANDROID_SENSOR_WAKEUP_GRAVITY:
			s->sGravityWuOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_6QUATWU_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR:
			s->sGrvWuOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_6QUATWU_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION:
			s->sLinAccWuOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_6QUATWU_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_WAKEUP_ORIENTATION:
			s->sOriWuOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_9QUATWU_MIN_ODR(s, delayInMs);
			break;
        case ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR:
			s->sRvWuOdrMs = delayInMs;
			AUGMENTED_SENSOR_GET_9QUATWU_MIN_ODR(s, delayInMs);
			break;
		default :
			break;
	}

	return delayInMs;
}


void inv_icm20948_augmented_sensors_update_odr(struct inv_icm20948 * s, unsigned char androidSensor, unsigned short * updatedDelayPtr)
{
	unsigned short lDelayInMs = 0xFFFF; // max value of uint16_t, so that we can get min value of all enabled sensors
	switch(androidSensor)
	{
		case ANDROID_SENSOR_GRAVITY:
        case ANDROID_SENSOR_GAME_ROTATION_VECTOR:
        case ANDROID_SENSOR_LINEAR_ACCELERATION:
			AUGMENTED_SENSOR_GET_6QUAT_MIN_ODR(s, lDelayInMs);
			*updatedDelayPtr = lDelayInMs;
			break;
		case ANDROID_SENSOR_WAKEUP_GRAVITY:
        case ANDROID_SENSOR_WAKEUP_GAME_ROTATION_VECTOR:
        case ANDROID_SENSOR_WAKEUP_LINEAR_ACCELERATION:
			AUGMENTED_SENSOR_GET_6QUATWU_MIN_ODR(s, lDelayInMs);
			*updatedDelayPtr = lDelayInMs;
			break;
		case ANDROID_SENSOR_ORIENTATION:
        case ANDROID_SENSOR_ROTATION_VECTOR:
			AUGMENTED_SENSOR_GET_9QUAT_MIN_ODR(s, lDelayInMs);
			*updatedDelayPtr = lDelayInMs;
			break;
		case ANDROID_SENSOR_WAKEUP_ORIENTATION:
        case ANDROID_SENSOR_WAKEUP_ROTATION_VECTOR:
			AUGMENTED_SENSOR_GET_9QUATWU_MIN_ODR(s, lDelayInMs);
			*updatedDelayPtr = lDelayInMs;
			break;
		default :
			break;
	}

}