<commit_before>/** 
 * \page The MAV'RIC license
 *
 * The MAV'RIC Framework
 *
 * Copyright © 2011-2014
 *
 * Laboratory of Intelligent Systems, EPFL
 */


/**
 * \file imu.c
 *
 * This file implements the code to read the IMU data
 */


#include "imu.h"

#include "delay.h"
#include "lsm330dlc_driver.h"
#include "compass_hmc5883l.h"
#include "time_keeper.h"
#include "print_util.h"
#include "mavlink_stream.h"
#include "tasks.h"

int32_t ic;

void imu_raw2oriented(Imu_Data_t *imu1);

/**
 * \brief	Computes the transition from raw values to scaled values
 *
 * \param	attitude	the pointer structure of the attitude
 * \param	rates		the array of angular rates (IMU), accelerations and magnetometer
 */
void imu_oriented2scale(Imu_Data_t *imu1);

void imu_init (Imu_Data_t *imu1)
{
	//itg3200_driver_init_slow();
	//adxl345_driver_init_slow();
	lsm330dlc_driver_init();
	print_util_dbg_print("LSM330 initialised \r");	
	
	compass_hmc58831l_init_slow();
	print_util_dbg_print("HMC5883 initialised \r");
	
	//imu_calibrate_Gyros(imu1);
	
	//init gyro
	imu1->calib_gyro.scale_factor[X] =  1.0f / RAW_GYRO_X_SCALE;
	imu1->calib_gyro.scale_factor[Y] =  1.0f / RAW_GYRO_Y_SCALE;
	imu1->calib_gyro.scale_factor[Z] =  1.0f / RAW_GYRO_Z_SCALE;
	imu1->calib_gyro.bias[X] = 0.0f;
	imu1->calib_gyro.bias[Y] = 0.0f;
	imu1->calib_gyro.bias[Z] = 0.0f;
	imu1->calib_gyro.orientation[X] = GYRO_AXIS_X;
	imu1->calib_gyro.orientation[Y] = GYRO_AXIS_Y;
	imu1->calib_gyro.orientation[Z] = GYRO_AXIS_Z;
	
	//init accelero
	imu1->calib_accelero.scale_factor[X] =  1.0f / RAW_ACC_X_SCALE;
	imu1->calib_accelero.scale_factor[Y] =  1.0f / RAW_ACC_Y_SCALE;
	imu1->calib_accelero.scale_factor[Z] =  1.0f / RAW_ACC_Z_SCALE;
	imu1->calib_accelero.bias[X] = ACC_BIAIS_X;
	imu1->calib_accelero.bias[Y] = ACC_BIAIS_Y;
	imu1->calib_accelero.bias[Z] = ACC_BIAIS_Z;
	imu1->calib_accelero.orientation[X] = ACC_AXIS_X;
	imu1->calib_accelero.orientation[Y] = ACC_AXIS_Y;
	imu1->calib_accelero.orientation[Z] = ACC_AXIS_Z;
	
	//init compass
	imu1->calib_compass.scale_factor[X] =  1.0f / RAW_MAG_X_SCALE;
	imu1->calib_compass.scale_factor[Y] =  1.0f / RAW_MAG_Y_SCALE;
	imu1->calib_compass.scale_factor[Z] =  1.0f / RAW_MAG_Z_SCALE;
	imu1->calib_compass.bias[X] = MAG_BIAIS_X;
	imu1->calib_compass.bias[Y] = MAG_BIAIS_Y;
	imu1->calib_compass.bias[Z] = MAG_BIAIS_Z;
	imu1->calib_compass.orientation[X] = MAG_AXIS_X;
	imu1->calib_compass.orientation[Y] = MAG_AXIS_Y;
	imu1->calib_compass.orientation[Z] = MAG_AXIS_Z;
		
	imu_last_update_init = false;
}

void imu_calibrate_gyros(Imu_Data_t *imu1)
{
	int32_t i,j;
	//imu_get_raw_data(imu1);
	imu_raw2oriented(imu1);
	
	for (j = 0; j < 3; j++)
	{
		imu1->calib_gyro.bias[j] = (float)imu1->oriented_gyro.data[j];
	}
	
	for (i = 0; i < 100; i++)
	{
		//imu_get_raw_data(imu1);

		//imu1->imu1->calib_sensor.bias[0 + ACC_OFFSET] = (0.9f * imu1->imu1->calib_accelero.bias[0] + 0.1f * (float)imu1->oriented_accelero.data[0]);
		//imu1->imu1->calib_sensor.bias[1 + ACC_OFFSET] = (0.9f * imu1->imu1->calib_accelero.bias[1] + 0.1f * (float)imu1->oriented_accelero.data[1]);
		//imu1->imu1->calib_sensor.bias[2 + ACC_OFFSET] = (0.9f * imu1->imu1->calib_accelero.bias[2] + 0.1f * ((float)imu1->oriented_accelero.data[2] - imu1->calib_accelero.scale_factor[2]));
		for (j = 0; j < 3; j++)
		{
			imu1->calib_gyro.bias[j] = (0.9f * imu1->calib_gyro.bias[j] + 0.1f * (float)imu1->oriented_gyro.data[j]);
			//imu1->attitude.raw_mag_mean[j] = (1.0 - fMAG_LPF) * imu1->attitude.raw_mag_mean[j] + MAG_LPF * ((float)imu1->oriented_compass.data[j]);
		}
	
		delay_ms(4);
	}
}

void imu_update(Imu_Data_t *imu1)
{
	uint32_t t = time_keeper_get_time_ticks();
	
	if (!imu_last_update_init)
	{
		imu1->last_update = t;
		imu_last_update_init = true;
	}
	else
	{
		imu1->dt = time_keeper_ticks_to_seconds(t - imu1->last_update);
		imu1->last_update = t;
		imu_raw2oriented(imu1);
		imu_oriented2scale(imu1);
	}
}

void imu_raw2oriented(Imu_Data_t *imu1)
{
	for (uint16_t i=0; i<3; i++)
	{
		imu1->oriented_gyro.data[i]		= imu1->raw_gyro.data[i] * imu1->calib_gyro.orientation[i];
		imu1->oriented_accelero.data[i] = imu1->raw_accelero.data[i] * imu1->calib_accelero.orientation[i];
		imu1->oriented_compass.data[i]	= imu1->raw_compass.data[i] * imu1->calib_compass.orientation[i];
	}
}

void imu_oriented2scale(Imu_Data_t *imu1)
{
	for (int16_t i = 0; i < 3; i++)
	{
		imu1->scaled_gyro.data[i]  = (1.0f - GYRO_LPF) * imu1->scaled_gyro.data[i] + GYRO_LPF * (((float)imu1->oriented_gyro.data[i] - imu1->calib_gyro.bias[i]) * imu1->calib_gyro.scale_factor[i]);
		imu1->scaled_accelero.data[i]   = (1.0f - ACC_LPF) * imu1->scaled_accelero.data[i] + ACC_LPF * (((float)imu1->oriented_accelero.data[i] - imu1->calib_accelero.bias[i]) * imu1->calib_accelero.scale_factor[i]);
		imu1->scaled_compass.data[i] = (1.0f - MAG_LPF) * imu1->scaled_compass.data[i] + MAG_LPF * (((float)imu1->oriented_compass.data[i] - imu1->calib_compass.bias[i]) * imu1->calib_compass.scale_factor[i]);
	}
}

void imu_relevel(Imu_Data_t *imu1)
{
	uint32_t i,j;
	float raw_mag_mean[3];			///< The raw magnetometer values to compute the initial heading of the platform

	for (j = 0;j < 3;j++)
	{
		raw_mag_mean[j] = (float)imu1->oriented_compass.data[j];
	}

	for (i = 1000; i > 0; i--)
	{
		tasks_run_imu_update(0);
		
		for (j = 0;j < 3;j++)
		{
			raw_mag_mean[j] = (1.0f - MAG_LPF) * raw_mag_mean[j] + MAG_LPF * ((float)imu1->oriented_compass.data[j]);
		}

		delay_ms(5);
	}
	
	for(i = 0; i < 3; i++)
	{
		imu1->scaled_compass.data[i] = (raw_mag_mean[i] - imu1->calib_compass.bias[i]) * imu1->calib_compass.scale_factor[i];
	}
}<commit_msg>move raw_mag_mean to relevel imu<commit_after>/** 
 * \page The MAV'RIC license
 *
 * The MAV'RIC Framework
 *
 * Copyright © 2011-2014
 *
 * Laboratory of Intelligent Systems, EPFL
 */


/**
 * \file imu.c
 *
 * This file implements the code to read the IMU data
 */


#include "imu.h"

#include "delay.h"
#include "lsm330dlc_driver.h"
#include "compass_hmc5883l.h"
#include "time_keeper.h"
#include "print_util.h"
#include "mavlink_stream.h"
#include "tasks.h"

int32_t ic;

void imu_raw2oriented(Imu_Data_t *imu1);

/**
 * \brief	Computes the transition from raw values to scaled values
 *
 * \param	attitude	the pointer structure of the attitude
 * \param	rates		the array of angular rates (IMU), accelerations and magnetometer
 */
void imu_oriented2scale(Imu_Data_t *imu1);

void imu_init (Imu_Data_t *imu1)
{
	//itg3200_driver_init_slow();
	//adxl345_driver_init_slow();
	lsm330dlc_driver_init();
	print_util_dbg_print("LSM330 initialised \r");	
	
	compass_hmc58831l_init_slow();
	print_util_dbg_print("HMC5883 initialised \r");
	
	//imu_calibrate_Gyros(imu1);
	
	//init gyro
	imu1->calib_gyro.scale_factor[X] =  1.0f / RAW_GYRO_X_SCALE;
	imu1->calib_gyro.scale_factor[Y] =  1.0f / RAW_GYRO_Y_SCALE;
	imu1->calib_gyro.scale_factor[Z] =  1.0f / RAW_GYRO_Z_SCALE;
	imu1->calib_gyro.bias[X] = 0.0f;
	imu1->calib_gyro.bias[Y] = 0.0f;
	imu1->calib_gyro.bias[Z] = 0.0f;
	imu1->calib_gyro.orientation[X] = GYRO_AXIS_X;
	imu1->calib_gyro.orientation[Y] = GYRO_AXIS_Y;
	imu1->calib_gyro.orientation[Z] = GYRO_AXIS_Z;
	
	//init accelero
	imu1->calib_accelero.scale_factor[X] =  1.0f / RAW_ACC_X_SCALE;
	imu1->calib_accelero.scale_factor[Y] =  1.0f / RAW_ACC_Y_SCALE;
	imu1->calib_accelero.scale_factor[Z] =  1.0f / RAW_ACC_Z_SCALE;
	imu1->calib_accelero.bias[X] = ACC_BIAIS_X;
	imu1->calib_accelero.bias[Y] = ACC_BIAIS_Y;
	imu1->calib_accelero.bias[Z] = ACC_BIAIS_Z;
	imu1->calib_accelero.orientation[X] = ACC_AXIS_X;
	imu1->calib_accelero.orientation[Y] = ACC_AXIS_Y;
	imu1->calib_accelero.orientation[Z] = ACC_AXIS_Z;
	
	//init compass
	imu1->calib_compass.scale_factor[X] =  1.0f / RAW_MAG_X_SCALE;
	imu1->calib_compass.scale_factor[Y] =  1.0f / RAW_MAG_Y_SCALE;
	imu1->calib_compass.scale_factor[Z] =  1.0f / RAW_MAG_Z_SCALE;
	imu1->calib_compass.bias[X] = MAG_BIAIS_X;
	imu1->calib_compass.bias[Y] = MAG_BIAIS_Y;
	imu1->calib_compass.bias[Z] = MAG_BIAIS_Z;
	imu1->calib_compass.orientation[X] = MAG_AXIS_X;
	imu1->calib_compass.orientation[Y] = MAG_AXIS_Y;
	imu1->calib_compass.orientation[Z] = MAG_AXIS_Z;
		
	imu_last_update_init = false;
}

void imu_calibrate_gyros(Imu_Data_t *imu1)
{
	int32_t i,j;
	//imu_get_raw_data(imu1);
	imu_raw2oriented(imu1);
	
	for (j = 0; j < 3; j++)
	{
		imu1->calib_gyro.bias[j] = (float)imu1->oriented_gyro.data[j];
	}
	
	for (i = 0; i < 100; i++)
	{
		//imu_get_raw_data(imu1);

		//imu1->imu1->calib_sensor.bias[0 + ACC_OFFSET] = (0.9f * imu1->imu1->calib_accelero.bias[0] + 0.1f * (float)imu1->oriented_accelero.data[0]);
		//imu1->imu1->calib_sensor.bias[1 + ACC_OFFSET] = (0.9f * imu1->imu1->calib_accelero.bias[1] + 0.1f * (float)imu1->oriented_accelero.data[1]);
		//imu1->imu1->calib_sensor.bias[2 + ACC_OFFSET] = (0.9f * imu1->imu1->calib_accelero.bias[2] + 0.1f * ((float)imu1->oriented_accelero.data[2] - imu1->calib_accelero.scale_factor[2]));
		for (j = 0; j < 3; j++)
		{
			imu1->calib_gyro.bias[j] = (0.9f * imu1->calib_gyro.bias[j] + 0.1f * (float)imu1->oriented_gyro.data[j]);
			//imu1->attitude.raw_mag_mean[j] = (1.0 - fMAG_LPF) * imu1->attitude.raw_mag_mean[j] + MAG_LPF * ((float)imu1->oriented_compass.data[j]);
		}
	
		delay_ms(4);
	}
}

void imu_update(Imu_Data_t *imu1)
{
	uint32_t t = time_keeper_get_time_ticks();
	
	if (!imu_last_update_init)
	{
		imu1->last_update = t;
		imu_last_update_init = true;
	}
	else
	{
		imu1->dt = time_keeper_ticks_to_seconds(t - imu1->last_update);
		imu1->last_update = t;
		imu_raw2oriented(imu1);
		imu_oriented2scale(imu1);
	}
}

void imu_raw2oriented(Imu_Data_t *imu1)
{
	for (uint16_t i=0; i<3; i++)
	{
		imu1->oriented_gyro.data[i]		= imu1->raw_gyro.data[i] * imu1->calib_gyro.orientation[i];
		imu1->oriented_accelero.data[i] = imu1->raw_accelero.data[i] * imu1->calib_accelero.orientation[i];
		imu1->oriented_compass.data[i]	= imu1->raw_compass.data[i] * imu1->calib_compass.orientation[i];
	}
}

void imu_oriented2scale(Imu_Data_t *imu1)
{
	for (int16_t i = 0; i < 3; i++)
	{
		imu1->scaled_gyro.data[i]  = (1.0f - GYRO_LPF) * imu1->scaled_gyro.data[i] + GYRO_LPF * (((float)imu1->oriented_gyro.data[i] - imu1->calib_gyro.bias[i]) * imu1->calib_gyro.scale_factor[i]);
		imu1->scaled_accelero.data[i]   = (1.0f - ACC_LPF) * imu1->scaled_accelero.data[i] + ACC_LPF * (((float)imu1->oriented_accelero.data[i] - imu1->calib_accelero.bias[i]) * imu1->calib_accelero.scale_factor[i]);
		imu1->scaled_compass.data[i] = (1.0f - MAG_LPF) * imu1->scaled_compass.data[i] + MAG_LPF * (((float)imu1->oriented_compass.data[i] - imu1->calib_compass.bias[i]) * imu1->calib_compass.scale_factor[i]);
	}
}

void imu_relevel(Imu_Data_t *imu1)
{
	float raw_mag_mean[3];			///< The raw magnetometer values to compute the initial heading of the platform

	tasks_run_imu_update(0);
	raw_mag_mean[X] = imu1->oriented_compass.data[X];
	raw_mag_mean[Y] = imu1->oriented_compass.data[Y];
	raw_mag_mean[Z] = imu1->oriented_compass.data[Z];

	for (uint32_t i = 1000; i > 0; i--)
	{
		tasks_run_imu_update(0);
		
		raw_mag_mean[X] = (1.0f - MAG_LPF) * raw_mag_mean[X] + MAG_LPF * imu1->oriented_compass.data[X];
		raw_mag_mean[Y] = (1.0f - MAG_LPF) * raw_mag_mean[Y] + MAG_LPF * imu1->oriented_compass.data[Y];
		raw_mag_mean[Z] = (1.0f - MAG_LPF) * raw_mag_mean[Z] + MAG_LPF * imu1->oriented_compass.data[Z];

		delay_ms(5);
	}
	
	imu1->scaled_compass.data[X] = (raw_mag_mean[X] - imu1->calib_compass.bias[X]) * imu1->calib_compass.scale_factor[X];
	imu1->scaled_compass.data[Y] = (raw_mag_mean[Y] - imu1->calib_compass.bias[Y]) * imu1->calib_compass.scale_factor[Y];
	imu1->scaled_compass.data[Z] = (raw_mag_mean[Z] - imu1->calib_compass.bias[Z]) * imu1->calib_compass.scale_factor[Z];
}
{'fraction_non_alphanumeric': 0.1667381054436348, 'fraction_numerical': 0.04209172738962709, 'mean_word_length': 4.755303404045387, 'pattern_counts': {'":': 0, '<': 18, '<?xml version=': 0, '>': 183, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3918822', 'n_tokens_mistral': 6002, 'n_tokens_neox': 5648, 'n_words': 713}