<commit_before>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the above copyright notice, 
 * this list of conditions and the following disclaimer.
 * 
 * 2. Redistributions in binary form must reproduce the above copyright notice, 
 * this list of conditions and the following disclaimer in the documentation 
 * and/or other materials provided with the distribution.
 * 
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file imu.c
 * 
 * \author MAV'RIC Team
 * \author <NAME>
 * \author <NAME>
 *   
 * \brief This file implements the IMU data structure
 *
 ******************************************************************************/


#include "imu.h"

#include "delay.h"
#include "time_keeper.h"
#include "print_util.h"
#include "mavlink_stream.h"
#include "tasks.h"
#include "coord_conventions.h"


//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS DECLARATION
//------------------------------------------------------------------------------

/**
 * \brief	Computes the oriented sensors values from the raw sensor values
 *
 * \param	imu		Pointer structure of the imu
 */
static void imu_raw2oriented(imu_t *imu);


/**
 * \brief	Computes the scaled sensors values from the oriented sensor values
 * 
 * \param	imu		Pointer structure of the imu
 */
static void imu_oriented2scale(imu_t *imu);

static void imu_start_calibration(imu_t* imu, mavlink_command_long_t* packet);

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

static void imu_raw2oriented(imu_t *imu)
{	
	uint16_t i;
	
	for (i=0; i<3; i++)
	{
		imu->oriented_gyro.data[i]		= imu->raw_gyro.data[imu->calib_gyro.axis[i]]     * imu->calib_gyro.orientation[i];
		imu->oriented_accelero.data[i]  = imu->raw_accelero.data[imu->calib_accelero.axis[i]] * imu->calib_accelero.orientation[i];
		imu->oriented_compass.data[i]	= imu->raw_compass.data[imu->calib_compass.axis[i]]  * imu->calib_compass.orientation[i];
	}
	
	/*if (imu->calib_gyro.calibration)
	{
		for (i=0; i<3; i++)
		{
			imu->calib_gyro.max_oriented_values[i] = maths_f_max(imu->calib_gyro.max_oriented_values[i],imu->oriented_gyro.data[i]);
			imu->calib_gyro.min_oriented_values[i] = maths_f_min(imu->calib_gyro.min_oriented_values[i],imu->oriented_gyro.data[i]);
		}
	}*/
	
	/*if (imu->calib_accelero.calibration)
	{
		for (i=0; i<3; i++)
		{
			imu->calib_accelero.max_oriented_values[i] = maths_f_max(imu->calib_accelero.max_oriented_values[i],imu->oriented_accelero.data[i]);
			imu->calib_accelero.min_oriented_values[i] = maths_f_min(imu->calib_accelero.min_oriented_values[i],imu->oriented_accelero.data[i]);
		}
	}*/
	
	if (imu->calib_compass.calibration)
	{
		for (i=0; i<3; i++)
		{
			imu->calib_compass.max_oriented_values[i] = maths_f_max(imu->calib_compass.max_oriented_values[i],imu->oriented_compass.data[i]);
			imu->calib_compass.min_oriented_values[i] = maths_f_min(imu->calib_compass.min_oriented_values[i],imu->oriented_compass.data[i]);
		}
	}
}


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

static void imu_start_calibration(imu_t* imu, mavlink_command_long_t* packet)
{
	MAV_RESULT result;
	int16_t i;
	
	print_util_dbg_print("Calibration cmd received");
	
	/* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty|  */
	if  (packet->param1 == 1)
	{
		/*if( (imu->state->mav_state == MAV_STATE_STANDBY)||(imu->state->mav_state == MAV_STATE_CALIBRATING) )
		{
			if (imu->calib_gyro.calibration)
			{
				print_util_dbg_print("Stopping gyro calibration\r\n");
				imu->calib_gyro.calibration = false;
				
				for (i = 0; i < 3; i++)
				{
					imu->.bias[i] = (imu->calib_gyro.max_oriented_values[i] + imu->calib_gyro.min_oriented_values[i])/2.0f;
					imu->calib_gyro.max_oriented_values[i] = -10000.0;
					imu->calib_gyro.min_oriented_values[i] =  10000.0;
				}
				print_util_dbg_print("New biais:");
				print_util_dbg_print_vector(imu->calib_gyro.bias,2);
			}
			else
			{
				print_util_dbg_print("Starting gyro calibration\r\n");
				imu->calib_gyro.calibration = true;
				print_util_dbg_print("Old biais:");
				print_util_dbg_print_vector(imu->calib_gyro.bias,2);
			}
			result = MAV_RESULT_ACCEPTED;
		}
		else
		{
			result = MAV_RESULT_TEMPORARILY_REJECTED;
		}*/
		result = MAV_RESULT_UNSUPPORTED;
	}
	
	if (packet->param2 == 1)
	{
		if ( (imu->state->mav_state == MAV_STATE_STANDBY)||(imu->state->mav_state == MAV_STATE_CALIBRATING) )
		{
			if (imu->calib_compass.calibration)
			{
				print_util_dbg_print("Stopping compass calibration\r\n");
				imu->calib_compass.calibration = false;
				
				for (i = 0; i < 3; i++)
				{
					imu->calib_compass.bias[i] = (imu->calib_compass.max_oriented_values[i] + imu->calib_compass.min_oriented_values[i])/2.0f;
					imu->calib_compass.max_oriented_values[i] = -10000.0;
					imu->calib_compass.min_oriented_values[i] =  10000.0;
				}
				print_util_dbg_print("New biais:");
				print_util_dbg_print_vector(imu->calib_compass.bias,2);
			}
			else
			{
				print_util_dbg_print("Starting magnetometers calibration\r\n");
				imu->calib_compass.calibration = true;
				print_util_dbg_print("Old biais:");
				print_util_dbg_print_vector(imu->calib_compass.bias,2);
			}
			result = MAV_RESULT_ACCEPTED;
		}
		else
		{
			result = MAV_RESULT_TEMPORARILY_REJECTED;
		}
	}
	
	if (packet->param3 == 1)
	{
		print_util_dbg_print("Starting ground pressure calibration\r\n");
		
		result = MAV_RESULT_UNSUPPORTED;
	}
	
	if (packet->param4 == 1)
	{
		print_util_dbg_print("Starting radio calibration\r\n");
		
		result = MAV_RESULT_UNSUPPORTED;
	}
	
	if (packet->param5 == 1)
	{
		/*if( (imu->state->mav_state == MAV_STATE_STANDBY)||(imu->state->mav_state == MAV_STATE_CALIBRATING) )
		{
			if (imu->calib_accelero.calibration)
			{
				print_util_dbg_print("Stopping accelerometer calibration\r\n");
				imu->calib_accelero.calibration = false;
				
				for (i = 0; i < 3; i++)
				{
					imu->calib_accelero.bias[i] = (imu->calib_accelero.max_oriented_values[i] + imu->calib_accelero.min_oriented_values[i])/2.0f;
					imu->calib_accelero.max_oriented_values[i] = -10000.0;
					imu->calib_accelero.min_oriented_values[i] =  10000.0;
				}
				print_util_dbg_print("New biais:");
				print_util_dbg_print_vector(imu->calib_accelero.bias,2);
			}
			else
			{
				print_util_dbg_print("Starting accelerometers calibration\r\n");
				imu->calib_accelero.calibration = true;
				print_util_dbg_print("Old biais:");
				print_util_dbg_print_vector(imu->calib_accelero.bias,2);
			}
			result = MAV_RESULT_ACCEPTED;
		}
		else
		{
			result = MAV_RESULT_TEMPORARILY_REJECTED;
		}*/
		result = MAV_RESULT_UNSUPPORTED;
	}
	
	mavlink_message_t msg;
	mavlink_msg_command_ack_pack( 	imu->mavlink_stream->sysid,
									imu->mavlink_stream->compid,
									&msg,
									MAV_CMD_PREFLIGHT_CALIBRATION,
									result);
	
	mavlink_stream_send(imu->mavlink_stream, &msg);
	
}

//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

void imu_init (imu_t *imu, const state_t* state, const mavlink_stream_t* mavlink_stream, mavlink_communication_t* mavlink_communication)
{
	imu->mavlink_stream = mavlink_stream;
	imu->state = state;
	
	//imu_calibrate_Gyros(imu);
	
	//init gyro
	imu->calib_gyro.scale_factor[X] =  1.0f / RAW_GYRO_X_SCALE;
	imu->calib_gyro.scale_factor[Y] =  1.0f / RAW_GYRO_Y_SCALE;
	imu->calib_gyro.scale_factor[Z] =  1.0f / RAW_GYRO_Z_SCALE;
	imu->calib_gyro.bias[X] = GYRO_BIAIS_X;
	imu->calib_gyro.bias[Y] = GYRO_BIAIS_Y;
	imu->calib_gyro.bias[Z] = GYRO_BIAIS_Z;
	imu->calib_gyro.orientation[X] = GYRO_ORIENTATION_X;
	imu->calib_gyro.orientation[Y] = GYRO_ORIENTATION_Y;
	imu->calib_gyro.orientation[Z] = GYRO_ORIENTATION_Z;
	imu->calib_gyro.axis[X] = GYRO_AXIS_X;
	imu->calib_gyro.axis[Y] = GYRO_AXIS_Y;
	imu->calib_gyro.axis[Z] = GYRO_AXIS_Z;
	imu->calib_gyro.max_oriented_values[X] = -10000.0f;
	imu->calib_gyro.max_oriented_values[Y] = -10000.0f;
	imu->calib_gyro.max_oriented_values[Z] = -10000.0f;
	imu->calib_gyro.min_oriented_values[X] =  10000.0f;
	imu->calib_gyro.min_oriented_values[Y] =  10000.0f;
	imu->calib_gyro.min_oriented_values[Z] =  10000.0f;
	imu->calib_gyro.calibration = false;
	
	//init accelero
	imu->calib_accelero.scale_factor[X] =  1.0f / RAW_ACC_X_SCALE;
	imu->calib_accelero.scale_factor[Y] =  1.0f / RAW_ACC_Y_SCALE;
	imu->calib_accelero.scale_factor[Z] =  1.0f / RAW_ACC_Z_SCALE;
	imu->calib_accelero.bias[X] = ACC_BIAIS_X;
	imu->calib_accelero.bias[Y] = ACC_BIAIS_Y;
	imu->calib_accelero.bias[Z] = ACC_BIAIS_Z;
	imu->calib_accelero.orientation[X] = ACC_ORIENTATION_X;
	imu->calib_accelero.orientation[Y] = ACC_ORIENTATION_Y;
	imu->calib_accelero.orientation[Z] = ACC_ORIENTATION_Z;
	imu->calib_accelero.axis[X] = ACC_AXIS_X;
	imu->calib_accelero.axis[Y] = ACC_AXIS_Y;
	imu->calib_accelero.axis[Z] = ACC_AXIS_Z;
	imu->calib_accelero.max_oriented_values[X] = -10000.0f;
	imu->calib_accelero.max_oriented_values[Y] = -10000.0f;
	imu->calib_accelero.max_oriented_values[Z] = -10000.0f;
	imu->calib_accelero.min_oriented_values[X] =  10000.0f;
	imu->calib_accelero.min_oriented_values[Y] =  10000.0f;
	imu->calib_accelero.min_oriented_values[Z] =  10000.0f;
	imu->calib_accelero.calibration = false;
	
	//init compass
	imu->calib_compass.scale_factor[X] =  1.0f / RAW_MAG_X_SCALE;
	imu->calib_compass.scale_factor[Y] =  1.0f / RAW_MAG_Y_SCALE;
	imu->calib_compass.scale_factor[Z] =  1.0f / RAW_MAG_Z_SCALE;
	imu->calib_compass.bias[X] = MAG_BIAIS_X;
	imu->calib_compass.bias[Y] = MAG_BIAIS_Y;
	imu->calib_compass.bias[Z] = MAG_BIAIS_Z;
	imu->calib_compass.orientation[X] = MAG_ORIENTATION_X;
	imu->calib_compass.orientation[Y] = MAG_ORIENTATION_Y;
	imu->calib_compass.orientation[Z] = MAG_ORIENTATION_Z;
	imu->calib_compass.axis[X] = MAG_AXIS_X;
	imu->calib_compass.axis[Y] = MAG_AXIS_Y;
	imu->calib_compass.axis[Z] = MAG_AXIS_Z;
	imu->calib_compass.max_oriented_values[X] = -10000.0f;
	imu->calib_compass.max_oriented_values[Y] = -10000.0f;
	imu->calib_compass.max_oriented_values[Z] = -10000.0f;
	imu->calib_compass.min_oriented_values[X] =  10000.0f;
	imu->calib_compass.min_oriented_values[Y] =  10000.0f;
	imu->calib_compass.min_oriented_values[Z] =  10000.0f;
	imu->calib_compass.calibration = false;
	
	// Add callbacks for waypoint handler commands requests
	mavlink_message_handler_cmd_callback_t callbackcmd;
	
	callbackcmd.command_id = MAV_CMD_PREFLIGHT_CALIBRATION; // 241
	callbackcmd.sysid_filter = MAVLINK_BASE_STATION_ID;
	callbackcmd.compid_filter = MAV_COMP_ID_ALL;
	callbackcmd.compid_target = MAV_COMP_ID_IMU; // 200
	callbackcmd.function = (mavlink_cmd_callback_function_t)	&imu_start_calibration;
	callbackcmd.module_struct =									imu;<commit_msg>Change position of if MAV_STATE, inverted start/stop instructions<commit_after>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC Development Team
 * All rights reserved.
 * 
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions are met:
 * 
 * 1. Redistributions of source code must retain the above copyright notice, 
 * this list of conditions and the following disclaimer.
 * 
 * 2. Redistributions in binary form must reproduce the above copyright notice, 
 * this list of conditions and the following disclaimer in the documentation 
 * and/or other materials provided with the distribution.
 * 
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 * POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/

/*******************************************************************************
 * \file imu.c
 * 
 * \author MAV'RIC Team
 * \author <NAME>
 * \author <NAME>
 *   
 * \brief This file implements the IMU data structure
 *
 ******************************************************************************/


#include "imu.h"

#include "delay.h"
#include "time_keeper.h"
#include "print_util.h"
#include "mavlink_stream.h"
#include "tasks.h"
#include "coord_conventions.h"


//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS DECLARATION
//------------------------------------------------------------------------------

/**
 * \brief	Computes the oriented sensors values from the raw sensor values
 *
 * \param	imu		Pointer structure of the imu
 */
static void imu_raw2oriented(imu_t *imu);


/**
 * \brief	Computes the scaled sensors values from the oriented sensor values
 * 
 * \param	imu		Pointer structure of the imu
 */
static void imu_oriented2scale(imu_t *imu);

static void imu_start_calibration(imu_t* imu, mavlink_command_long_t* packet);

//------------------------------------------------------------------------------
// PRIVATE FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

static void imu_raw2oriented(imu_t *imu)
{	
	uint16_t i;
	
	for (i=0; i<3; i++)
	{
		imu->oriented_gyro.data[i]		= imu->raw_gyro.data[imu->calib_gyro.axis[i]]     * imu->calib_gyro.orientation[i];
		imu->oriented_accelero.data[i]  = imu->raw_accelero.data[imu->calib_accelero.axis[i]] * imu->calib_accelero.orientation[i];
		imu->oriented_compass.data[i]	= imu->raw_compass.data[imu->calib_compass.axis[i]]  * imu->calib_compass.orientation[i];
	}
	
	/*if (imu->calib_gyro.calibration)
	{
		for (i=0; i<3; i++)
		{
			imu->calib_gyro.max_oriented_values[i] = maths_f_max(imu->calib_gyro.max_oriented_values[i],imu->oriented_gyro.data[i]);
			imu->calib_gyro.min_oriented_values[i] = maths_f_min(imu->calib_gyro.min_oriented_values[i],imu->oriented_gyro.data[i]);
		}
	}*/
	
	/*if (imu->calib_accelero.calibration)
	{
		for (i=0; i<3; i++)
		{
			imu->calib_accelero.max_oriented_values[i] = maths_f_max(imu->calib_accelero.max_oriented_values[i],imu->oriented_accelero.data[i]);
			imu->calib_accelero.min_oriented_values[i] = maths_f_min(imu->calib_accelero.min_oriented_values[i],imu->oriented_accelero.data[i]);
		}
	}*/
	
	if (imu->calib_compass.calibration)
	{
		for (i=0; i<3; i++)
		{
			imu->calib_compass.max_oriented_values[i] = maths_f_max(imu->calib_compass.max_oriented_values[i],imu->oriented_compass.data[i]);
			imu->calib_compass.min_oriented_values[i] = maths_f_min(imu->calib_compass.min_oriented_values[i],imu->oriented_compass.data[i]);
		}
	}
}


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

static void imu_start_calibration(imu_t* imu, mavlink_command_long_t* packet)
{
	MAV_RESULT result;
	int16_t i;
	
	print_util_dbg_print("Calibration cmd received");
	
	/* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Compass/Motor interference calibration: 0: no, 1: yes| Empty|  */
	
	if ( (imu->state->mav_state == MAV_STATE_STANDBY)||(imu->state->mav_state == MAV_STATE_CALIBRATING) )
	{
		if  (packet->param1 == 1)
		{
			//if (!imu->calib_gyro.calibration)
			//{
				//print_util_dbg_print("Starting gyro calibration\r\n");
				//imu->calib_gyro.calibration = true;
				//print_util_dbg_print("Old biais:");
				//print_util_dbg_print_vector(imu->calib_gyro.bias,2);
			//}
			//else
			//{
				//print_util_dbg_print("Stopping gyro calibration\r\n");
				//imu->calib_gyro.calibration = false;
				//
				//for (i = 0; i < 3; i++)
				//{
					//imu->.bias[i] = (imu->calib_gyro.max_oriented_values[i] + imu->calib_gyro.min_oriented_values[i])/2.0f;
					//imu->calib_gyro.max_oriented_values[i] = -10000.0;
					//imu->calib_gyro.min_oriented_values[i] =  10000.0;
				//}
				//print_util_dbg_print("New biais:");
				//print_util_dbg_print_vector(imu->calib_gyro.bias,2);
			//}
			//result = MAV_RESULT_ACCEPTED;
			
			result = MAV_RESULT_UNSUPPORTED;
		}
	
		if (packet->param2 == 1)
		{
			if (!imu->calib_compass.calibration)
			{
				print_util_dbg_print("Starting magnetometers calibration\r\n");
				imu->calib_compass.calibration = true;
				print_util_dbg_print("Old biais:");
				print_util_dbg_print_vector(imu->calib_compass.bias,2);
			}
			else
			{
				print_util_dbg_print("Stopping compass calibration\r\n");
				imu->calib_compass.calibration = false;
				
				for (i = 0; i < 3; i++)
				{
					imu->calib_compass.bias[i] = (imu->calib_compass.max_oriented_values[i] + imu->calib_compass.min_oriented_values[i])/2.0f;
					imu->calib_compass.max_oriented_values[i] = -10000.0;
					imu->calib_compass.min_oriented_values[i] =  10000.0;
				}
				print_util_dbg_print("New biais:");
				print_util_dbg_print_vector(imu->calib_compass.bias,2);
			}
			result = MAV_RESULT_ACCEPTED;
		}
	
		if (packet->param3 == 1)
		{
			print_util_dbg_print("Starting ground pressure calibration\r\n");
		
			result = MAV_RESULT_UNSUPPORTED;
		}
	
		if (packet->param4 == 1)
		{
			print_util_dbg_print("Starting radio calibration\r\n");
		
			result = MAV_RESULT_UNSUPPORTED;
		}
	
		if (packet->param5 == 1)
		{
			//if (!imu->calib_accelero.calibration)
			//{
				//print_util_dbg_print("Starting accelerometers calibration\r\n");
				//imu->calib_accelero.calibration = true;
				//print_util_dbg_print("Old biais:");
				//print_util_dbg_print_vector(imu->calib_accelero.bias,2);
			//}
			//else
			//{
				//print_util_dbg_print("Stopping accelerometer calibration\r\n");
				//imu->calib_accelero.calibration = false;
				//
				//for (i = 0; i < 3; i++)
				//{
					//imu->calib_accelero.bias[i] = (imu->calib_accelero.max_oriented_values[i] + imu->calib_accelero.min_oriented_values[i])/2.0f;
					//imu->calib_accelero.max_oriented_values[i] = -10000.0;
					//imu->calib_accelero.min_oriented_values[i] =  10000.0;
				//}
				//print_util_dbg_print("New biais:");
				//print_util_dbg_print_vector(imu->calib_accelero.bias,2);
			//}
			//result = MAV_RESULT_ACCEPTED;
			
			result = MAV_RESULT_UNSUPPORTED;
		}
	}
	else
	{
		result = MAV_RESULT_TEMPORARILY_REJECTED;
	}
	
	mavlink_message_t msg;
	mavlink_msg_command_ack_pack( 	imu->mavlink_stream->sysid,
									imu->mavlink_stream->compid,
									&msg,
									MAV_CMD_PREFLIGHT_CALIBRATION,
									result);
	
	mavlink_stream_send(imu->mavlink_stream, &msg);
	
}

//------------------------------------------------------------------------------
// PUBLIC FUNCTIONS IMPLEMENTATION
//------------------------------------------------------------------------------

void imu_init (imu_t *imu, const state_t* state, const mavlink_stream_t* mavlink_stream, mavlink_communication_t* mavlink_communication)
{
	imu->mavlink_stream = mavlink_stream;
	imu->state = state;
	
	//imu_calibrate_Gyros(imu);
	
	//init gyro
	imu->calib_gyro.scale_factor[X] =  1.0f / RAW_GYRO_X_SCALE;
	imu->calib_gyro.scale_factor[Y] =  1.0f / RAW_GYRO_Y_SCALE;
	imu->calib_gyro.scale_factor[Z] =  1.0f / RAW_GYRO_Z_SCALE;
	imu->calib_gyro.bias[X] = GYRO_BIAIS_X;
	imu->calib_gyro.bias[Y] = GYRO_BIAIS_Y;
	imu->calib_gyro.bias[Z] = GYRO_BIAIS_Z;
	imu->calib_gyro.orientation[X] = GYRO_ORIENTATION_X;
	imu->calib_gyro.orientation[Y] = GYRO_ORIENTATION_Y;
	imu->calib_gyro.orientation[Z] = GYRO_ORIENTATION_Z;
	imu->calib_gyro.axis[X] = GYRO_AXIS_X;
	imu->calib_gyro.axis[Y] = GYRO_AXIS_Y;
	imu->calib_gyro.axis[Z] = GYRO_AXIS_Z;
	imu->calib_gyro.max_oriented_values[X] = -10000.0f;
	imu->calib_gyro.max_oriented_values[Y] = -10000.0f;
	imu->calib_gyro.max_oriented_values[Z] = -10000.0f;
	imu->calib_gyro.min_oriented_values[X] =  10000.0f;
	imu->calib_gyro.min_oriented_values[Y] =  10000.0f;
	imu->calib_gyro.min_oriented_values[Z] =  10000.0f;
	imu->calib_gyro.calibration = false;
	
	//init accelero
	imu->calib_accelero.scale_factor[X] =  1.0f / RAW_ACC_X_SCALE;
	imu->calib_accelero.scale_factor[Y] =  1.0f / RAW_ACC_Y_SCALE;
	imu->calib_accelero.scale_factor[Z] =  1.0f / RAW_ACC_Z_SCALE;
	imu->calib_accelero.bias[X] = ACC_BIAIS_X;
	imu->calib_accelero.bias[Y] = ACC_BIAIS_Y;
	imu->calib_accelero.bias[Z] = ACC_BIAIS_Z;
	imu->calib_accelero.orientation[X] = ACC_ORIENTATION_X;
	imu->calib_accelero.orientation[Y] = ACC_ORIENTATION_Y;
	imu->calib_accelero.orientation[Z] = ACC_ORIENTATION_Z;
	imu->calib_accelero.axis[X] = ACC_AXIS_X;
	imu->calib_accelero.axis[Y] = ACC_AXIS_Y;
	imu->calib_accelero.axis[Z] = ACC_AXIS_Z;
	imu->calib_accelero.max_oriented_values[X] = -10000.0f;
	imu->calib_accelero.max_oriented_values[Y] = -10000.0f;
	imu->calib_accelero.max_oriented_values[Z] = -10000.0f;
	imu->calib_accelero.min_oriented_values[X] =  10000.0f;
	imu->calib_accelero.min_oriented_values[Y] =  10000.0f;
	imu->calib_accelero.min_oriented_values[Z] =  10000.0f;
	imu->calib_accelero.calibration = false;
	
	//init compass
	imu->calib_compass.scale_factor[X] =  1.0f / RAW_MAG_X_SCALE;
	imu->calib_compass.scale_factor[Y] =  1.0f / RAW_MAG_Y_SCALE;
	imu->calib_compass.scale_factor[Z] =  1.0f / RAW_MAG_Z_SCALE;
	imu->calib_compass.bias[X] = MAG_BIAIS_X;
	imu->calib_compass.bias[Y] = MAG_BIAIS_Y;
	imu->calib_compass.bias[Z] = MAG_BIAIS_Z;
	imu->calib_compass.orientation[X] = MAG_ORIENTATION_X;
	imu->calib_compass.orientation[Y] = MAG_ORIENTATION_Y;
	imu->calib_compass.orientation[Z] = MAG_ORIENTATION_Z;
	imu->calib_compass.axis[X] = MAG_AXIS_X;
	imu->calib_compass.axis[Y] = MAG_AXIS_Y;
	imu->calib_compass.axis[Z] = MAG_AXIS_Z;
	imu->calib_compass.max_oriented_values[X] = -10000.0f;
	imu->calib_compass.max_oriented_values[Y] = -10000.0f;
	imu->calib_compass.max_oriented_values[Z] = -10000.0f;
	imu->calib_compass.min_oriented_values[X] =  10000.0f;
	imu->calib_compass.min_oriented_values[Y] =  10000.0f;
	imu->calib_compass.min_oriented_values[Z] =  10000.0f;
	imu->calib_compass.calibration = false;
	
	// Add callbacks for waypoint handler commands requests
	mavlink_message_handler_cmd_callback_t callbackcmd;
	
	callbackcmd.command_id = MAV_CMD_PREFLIGHT_CALIBRATION; // 241
	callbackcmd.sysid_filter = MAVLINK_BASE_STATION_ID;
	callbackcmd.compid_filter = MAV_COMP_ID_ALL;
	callbackcmd.compid_target = MAV_COMP_ID_ALL; // 0
	callbackcmd.function = (mavlink_cmd_callback_function_t)	&imu_start_calibration;
	callbackcmd.module_struct =									imu;
{'fraction_non_alphanumeric': 0.18470028544243577, 'fraction_numerical': 0.01857278782112274, 'mean_word_length': 3.7627333695849194, 'pattern_counts': {'":': 0, '<': 23, '<?xml version=': 0, '>': 317, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1721097', 'n_tokens_mistral': 11440, 'n_tokens_neox': 10147, 'n_words': 1584}