diff --git a/Makefile b/Makefile index 506e4db4d9..e90d6b176b 100644 --- a/Makefile +++ b/Makefile @@ -180,6 +180,7 @@ STM32F3DISCOVERY_SRC = startup_stm32f30x_md_gcc.S \ drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu6050.c \ drivers/accgyro_l3g4200d.c \ + drivers/accgyro_lsm303dlhc.c \ drivers/adc_common.c \ drivers/bus_i2c_stm32f30x.c \ drivers/bus_spi.c \ diff --git a/src/drivers/accgyro_adxl345.c b/src/drivers/accgyro_adxl345.c index 0c8b4a01c2..deb426f125 100755 --- a/src/drivers/accgyro_adxl345.c +++ b/src/drivers/accgyro_adxl345.c @@ -77,7 +77,7 @@ static void adxl345Init(void) i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); } - acc_1G = 265; // 3.3V operation + acc_1G = 265; // 3.3V operation // FIXME verify this is supposed to be 265, not 256. Typo? } uint8_t acc_samples = 0; diff --git a/src/drivers/accgyro_lsm303dlhc.c b/src/drivers/accgyro_lsm303dlhc.c new file mode 100644 index 0000000000..9b337bc9a8 --- /dev/null +++ b/src/drivers/accgyro_lsm303dlhc.c @@ -0,0 +1,127 @@ +#include +#include + +#include "platform.h" + +#include "common/maths.h" +#include "common/axis.h" + +#include "system_common.h" +#include "gpio_common.h" +#include "bus_i2c.h" + +#include "accgyro_common.h" +#include "accgyro_lsm303dlhc.h" + +extern int16_t debug[4]; + +// Address + +#define LSM303DLHC_ACCEL_ADDRESS 0x32 +#define LSM303DLHC_MAG_ADDRESS 0x3C + +// Registers + +#define CTRL_REG1_A 0x20 +#define CTRL_REG4_A 0x23 +#define CTRL_REG5_A 0x24 +#define OUT_X_L_A 0x28 +#define CRA_REG_M 0x00 +#define CRB_REG_M 0x01 +#define MR_REG_M 0x02 +#define OUT_X_H_M 0x03 + +/////////////////////////////////////// + +#define ODR_1344_HZ 0x90 +#define AXES_ENABLE 0x07 + +#define FULLSCALE_2G 0x00 +#define FULLSCALE_4G 0x10 +#define FULLSCALE_8G 0x20 +#define FULLSCALE_16G 0x30 + +#define BOOT 0x80 + +/////////////////////////////////////// + +#define ODR_75_HZ 0x18 +#define ODR_15_HZ 0x10 + +#define FS_1P3_GA 0x20 +#define FS_1P9_GA 0x40 +#define FS_2P5_GA 0x60 +#define FS_4P0_GA 0x80 +#define FS_4P7_GA 0xA0 +#define FS_5P6_GA 0xC0 +#define FS_8P1_GA 0xE0 + +#define CONTINUOUS_CONVERSION 0x00 + + +uint8_t accelCalibrating = false; + +float accelOneG = 9.8065; + +int32_t accelSum100Hz[3] = { 0, 0, 0 }; + +int32_t accelSum500Hz[3] = { 0, 0, 0 }; + +int32_t accelSummedSamples100Hz[3]; + +int32_t accelSummedSamples500Hz[3]; + +void lsm303dlhcAccInit(void) +{ + i2cWrite(LSM303DLHC_ACCEL_ADDRESS, CTRL_REG5_A, BOOT); + + delay(100); + + i2cWrite(LSM303DLHC_ACCEL_ADDRESS, CTRL_REG1_A, ODR_1344_HZ | AXES_ENABLE); + + delay(10); + + i2cWrite(LSM303DLHC_ACCEL_ADDRESS, CTRL_REG4_A, FULLSCALE_4G); + + delay(100); + + acc_1G = 512 * 8; +} + +// Read 3 gyro values into user-provided buffer. No overrun checking is done. +static void lsm303dlhcAccRead(int16_t *gyroData) +{ + uint8_t buf[6]; + + bool ok = i2cRead(LSM303DLHC_ACCEL_ADDRESS, OUT_X_L_A, 6, buf); + + if (!ok) + return; + + // the values range from -8192 to +8191 + gyroData[X] = (int16_t)((buf[1] << 8) | buf[0]); + gyroData[Y] = (int16_t)((buf[3] << 8) | buf[2]); + gyroData[Z] = (int16_t)((buf[5] << 8) | buf[4]); + + debug[0] = (int16_t)((buf[1] << 8) | buf[0]); + debug[1] = (int16_t)((buf[3] << 8) | buf[2]); + debug[2] = (int16_t)((buf[5] << 8) | buf[4]); + +} + +bool lsm303dlhcAccDetect(acc_t *acc) +{ + bool ack; + uint8_t status; + + ack = i2cRead(LSM303DLHC_ACCEL_ADDRESS, LSM303DLHC_STATUS_REG_A, 1, &status); + if (!ack) + return false; + + acc->init = lsm303dlhcAccInit; + acc->read = lsm303dlhcAccRead; + return true; +} + + + diff --git a/src/drivers/accgyro_lsm303dlhc.h b/src/drivers/accgyro_lsm303dlhc.h new file mode 100644 index 0000000000..dbc4aff492 --- /dev/null +++ b/src/drivers/accgyro_lsm303dlhc.h @@ -0,0 +1,460 @@ +#pragma once + +/** + * @brief LSM303DLHC Status + */ + +/* LSM303DLHC ACC struct */ +typedef struct +{ + uint8_t Power_Mode; /* Power-down/Normal Mode */ + uint8_t AccOutput_DataRate; /* OUT data rate */ + uint8_t Axes_Enable; /* Axes enable */ + uint8_t High_Resolution; /* High Resolution enabling/disabling */ + uint8_t BlockData_Update; /* Block Data Update */ + uint8_t Endianness; /* Endian Data selection */ + uint8_t AccFull_Scale; /* Full Scale selection */ +}LSM303DLHCAcc_InitTypeDef; + +/* LSM303DLHC Acc High Pass Filter struct */ +typedef struct +{ + uint8_t HighPassFilter_Mode_Selection; /* Internal filter mode */ + uint8_t HighPassFilter_CutOff_Frequency; /* High pass filter cut-off frequency */ + uint8_t HighPassFilter_AOI1; /* HPF_enabling/disabling for AOI function on interrupt 1 */ + uint8_t HighPassFilter_AOI2; /* HPF_enabling/disabling for AOI function on interrupt 2 */ +}LSM303DLHCAcc_FilterConfigTypeDef; + +/* LSM303DLHC Mag struct */ +typedef struct +{ + uint8_t Temperature_Sensor; /* Temperature sensor enable/disable */ + uint8_t MagOutput_DataRate; /* OUT data rate */ + uint8_t Working_Mode; /* operating mode */ + uint8_t MagFull_Scale; /* Full Scale selection */ +}LSM303DLHCMag_InitTypeDef; +/** + * @} + */ + +/** @defgroup STM32F3_DISCOVERY_LSM303DLHC_Exported_Constants + * @{ + */ +#define LSM303DLHC_OK ((uint32_t) 0) +#define LSM303DLHC_FAIL ((uint32_t) 0) + +/* Uncomment the following line to use the default LSM303DLHC_TIMEOUT_UserCallback() + function implemented in stm32f3_discovery_lgd20.c file. + LSM303DLHC_TIMEOUT_UserCallback() function is called whenever a timeout condition + occure during communication (waiting transmit data register empty flag(TXE) + or waiting receive data register is not empty flag (RXNE)). */ +/* #define USE_DEFAULT_TIMEOUT_CALLBACK */ + +/* Maximum Timeout values for flags waiting loops. These timeouts are not based + on accurate values, they just guarantee that the application will not remain + stuck if the I2C communication is corrupted. + You may modify these timeout values depending on CPU frequency and application + conditions (interrupts routines ...). */ +#define LSM303DLHC_FLAG_TIMEOUT ((uint32_t)0x1000) +#define LSM303DLHC_LONG_TIMEOUT ((uint32_t)(10 * LSM303DLHC_FLAG_TIMEOUT)) +/** + * @brief LSM303DLHC I2C Interface pins + */ +#define LSM303DLHC_I2C I2C1 +#define LSM303DLHC_I2C_CLK RCC_APB1Periph_I2C1 + +#define LSM303DLHC_I2C_SCK_PIN GPIO_Pin_6 /* PB.06 */ +#define LSM303DLHC_I2C_SCK_GPIO_PORT GPIOB /* GPIOB */ +#define LSM303DLHC_I2C_SCK_GPIO_CLK RCC_AHBPeriph_GPIOB +#define LSM303DLHC_I2C_SCK_SOURCE GPIO_PinSource6 +#define LSM303DLHC_I2C_SCK_AF GPIO_AF_4 + +#define LSM303DLHC_I2C_SDA_PIN GPIO_Pin_7 /* PB.7 */ +#define LSM303DLHC_I2C_SDA_GPIO_PORT GPIOB /* GPIOB */ +#define LSM303DLHC_I2C_SDA_GPIO_CLK RCC_AHBPeriph_GPIOB +#define LSM303DLHC_I2C_SDA_SOURCE GPIO_PinSource7 +#define LSM303DLHC_I2C_SDA_AF GPIO_AF_4 + +#define LSM303DLHC_DRDY_PIN GPIO_Pin_2 /* PE.02 */ +#define LSM303DLHC_DRDY_GPIO_PORT GPIOE /* GPIOE */ +#define LSM303DLHC_DRDY_GPIO_CLK RCC_AHBPeriph_GPIOE +#define LSM303DLHC_DRDY_EXTI_LINE EXTI_Line2 +#define LSM303DLHC_DRDY_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE +#define LSM303DLHC_DRDY_EXTI_PIN_SOURCE EXTI_PinSource2 +#define LSM303DLHC_DRDY_EXTI_IRQn EXTI2_TS_IRQn + +#define LSM303DLHC_I2C_INT1_PIN GPIO_Pin_4 /* PE.04 */ +#define LSM303DLHC_I2C_INT1_GPIO_PORT GPIOE /* GPIOE */ +#define LSM303DLHC_I2C_INT1_GPIO_CLK RCC_AHBPeriph_GPIOE +#define LSM303DLHC_I2C_INT1_EXTI_LINE EXTI_Line4 +#define LSM303DLHC_I2C_INT1_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE +#define LSM303DLHC_I2C_INT1_EXTI_PIN_SOURCE EXTI_PinSource4 +#define LSM303DLHC_I2C_INT1_EXTI_IRQn EXTI4_IRQn + +#define LSM303DLHC_I2C_INT2_PIN GPIO_Pin_5 /* PE.05 */ +#define LSM303DLHC_I2C_INT2_GPIO_PORT GPIOE /* GPIOE */ +#define LSM303DLHC_I2C_INT2_GPIO_CLK RCC_AHBPeriph_GPIOE +#define LSM303DLHC_I2C_INT2_EXTI_LINE EXTI_Line5 +#define LSM303DLHC_I2C_INT2_EXTI_PORT_SOURCE EXTI_PortSourceGPIOE +#define LSM303DLHC_I2C_INT2_EXTI_PIN_SOURCE EXTI_PinSource5ss +#define LSM303DLHC_I2C_INT2_EXTI_IRQn EXTI9_5_IRQn + +/******************************************************************************/ +/*************************** START REGISTER MAPPING **************************/ +/******************************************************************************/ +/* Acceleration Registers */ +#define LSM303DLHC_CTRL_REG1_A 0x20 /* Control register 1 acceleration */ +#define LSM303DLHC_CTRL_REG2_A 0x21 /* Control register 2 acceleration */ +#define LSM303DLHC_CTRL_REG3_A 0x22 /* Control register 3 acceleration */ +#define LSM303DLHC_CTRL_REG4_A 0x23 /* Control register 4 acceleration */ +#define LSM303DLHC_CTRL_REG5_A 0x24 /* Control register 5 acceleration */ +#define LSM303DLHC_CTRL_REG6_A 0x25 /* Control register 6 acceleration */ +#define LSM303DLHC_REFERENCE_A 0x26 /* Reference register acceleration */ +#define LSM303DLHC_STATUS_REG_A 0x27 /* Status register acceleration */ +#define LSM303DLHC_OUT_X_L_A 0x28 /* Output Register X acceleration */ +#define LSM303DLHC_OUT_X_H_A 0x29 /* Output Register X acceleration */ +#define LSM303DLHC_OUT_Y_L_A 0x2A /* Output Register Y acceleration */ +#define LSM303DLHC_OUT_Y_H_A 0x2B /* Output Register Y acceleration */ +#define LSM303DLHC_OUT_Z_L_A 0x2C /* Output Register Z acceleration */ +#define LSM303DLHC_OUT_Z_H_A 0x2D /* Output Register Z acceleration */ +#define LSM303DLHC_FIFO_CTRL_REG_A 0x2E /* Fifo control Register acceleration */ +#define LSM303DLHC_FIFO_SRC_REG_A 0x2F /* Fifo src Register acceleration */ + +#define LSM303DLHC_INT1_CFG_A 0x30 /* Interrupt 1 configuration Register acceleration */ +#define LSM303DLHC_INT1_SOURCE_A 0x31 /* Interrupt 1 source Register acceleration */ +#define LSM303DLHC_INT1_THS_A 0x32 /* Interrupt 1 Threshold register acceleration */ +#define LSM303DLHC_INT1_DURATION_A 0x33 /* Interrupt 1 DURATION register acceleration */ + +#define LSM303DLHC_INT2_CFG_A 0x34 /* Interrupt 2 configuration Register acceleration */ +#define LSM303DLHC_INT2_SOURCE_A 0x35 /* Interrupt 2 source Register acceleration */ +#define LSM303DLHC_INT2_THS_A 0x36 /* Interrupt 2 Threshold register acceleration */ +#define LSM303DLHC_INT2_DURATION_A 0x37 /* Interrupt 2 DURATION register acceleration */ + +#define LSM303DLHC_CLICK_CFG_A 0x38 /* Click configuration Register acceleration */ +#define LSM303DLHC_CLICK_SOURCE_A 0x39 /* Click 2 source Register acceleration */ +#define LSM303DLHC_CLICK_THS_A 0x3A /* Click 2 Threshold register acceleration */ + +#define LSM303DLHC_TIME_LIMIT_A 0x3B /* Time Limit Register acceleration */ +#define LSM303DLHC_TIME_LATENCY_A 0x3C /* Time Latency Register acceleration */ +#define LSM303DLHC_TIME_WINDOW_A 0x3D /* Time window register acceleration */ + +/* Magnetic field Registers */ +#define LSM303DLHC_CRA_REG_M 0x00 /* Control register A magnetic field */ +#define LSM303DLHC_CRB_REG_M 0x01 /* Control register B magnetic field */ +#define LSM303DLHC_MR_REG_M 0x02 /* Control register MR magnetic field */ +#define LSM303DLHC_OUT_X_H_M 0x03 /* Output Register X magnetic field */ +#define LSM303DLHC_OUT_X_L_M 0x04 /* Output Register X magnetic field */ +#define LSM303DLHC_OUT_Z_H_M 0x05 /* Output Register Z magnetic field */ +#define LSM303DLHC_OUT_Z_L_M 0x06 /* Output Register Z magnetic field */ +#define LSM303DLHC_OUT_Y_H_M 0x07 /* Output Register Y magnetic field */ +#define LSM303DLHC_OUT_Y_L_M 0x08 /* Output Register Y magnetic field */ + +#define LSM303DLHC_SR_REG_M 0x09 /* Status Register magnetic field */ +#define LSM303DLHC_IRA_REG_M 0x0A /* IRA Register magnetic field */ +#define LSM303DLHC_IRB_REG_M 0x0B /* IRB Register magnetic field */ +#define LSM303DLHC_IRC_REG_M 0x0C /* IRC Register magnetic field */ + +#define LSM303DLHC_TEMP_OUT_H_M 0x31 /* Temperature Register magnetic field */ +#define LSM303DLHC_TEMP_OUT_L_M 0x32 /* Temperature Register magnetic field */ +/******************************************************************************/ +/**************************** END REGISTER MAPPING ***************************/ +/******************************************************************************/ + +#define ACC_I2C_ADDRESS 0x32 +#define MAG_I2C_ADDRESS 0x3C + +/** @defgroup Acc_Power_Mode_selection + * @{ + */ +#define LSM303DLHC_NORMAL_MODE ((uint8_t)0x00) +#define LSM303DLHC_LOWPOWER_MODE ((uint8_t)0x08) +/** + * @} + */ + +/** @defgroup Acc_OutPut_DataRate_Selection + * @{ + */ +#define LSM303DLHC_ODR_1_HZ ((uint8_t)0x10) /*!< Output Data Rate = 1 Hz */ +#define LSM303DLHC_ODR_10_HZ ((uint8_t)0x20) /*!< Output Data Rate = 10 Hz */ +#define LSM303DLHC_ODR_25_HZ ((uint8_t)0x30) /*!< Output Data Rate = 25 Hz */ +#define LSM303DLHC_ODR_50_HZ ((uint8_t)0x40) /*!< Output Data Rate = 50 Hz */ +#define LSM303DLHC_ODR_100_HZ ((uint8_t)0x50) /*!< Output Data Rate = 100 Hz */ +#define LSM303DLHC_ODR_200_HZ ((uint8_t)0x60) /*!< Output Data Rate = 200 Hz */ +#define LSM303DLHC_ODR_400_HZ ((uint8_t)0x70) /*!< Output Data Rate = 400 Hz */ +#define LSM303DLHC_ODR_1620_HZ_LP ((uint8_t)0x80) /*!< Output Data Rate = 1620 Hz only in Low Power Mode */ +#define LSM303DLHC_ODR_1344_HZ ((uint8_t)0x90) /*!< Output Data Rate = 1344 Hz in Normal mode and 5376 Hz in Low Power Mode */ + +/** + * @} + */ + +/** @defgroup Acc_Axes_Selection + * @{ + */ +#define LSM303DLHC_X_ENABLE ((uint8_t)0x01) +#define LSM303DLHC_Y_ENABLE ((uint8_t)0x02) +#define LSM303DLHC_Z_ENABLE ((uint8_t)0x04) +#define LSM303DLHC_AXES_ENABLE ((uint8_t)0x07) +#define LSM303DLHC_AXES_DISABLE ((uint8_t)0x00) +/** + * @} + */ + +/** @defgroup Acc_High_Resolution + * @{ + */ +#define LSM303DLHC_HR_ENABLE ((uint8_t)0x08) +#define LSM303DLHC_HR_DISABLE ((uint8_t)0x00) +/** + * @} + */ + +/** @defgroup Acc_Full_Scale_Selection + * @{ + */ +#define LSM303DLHC_FULLSCALE_2G ((uint8_t)0x00) /*!< ±2 g */ +#define LSM303DLHC_FULLSCALE_4G ((uint8_t)0x10) /*!< ±4 g */ +#define LSM303DLHC_FULLSCALE_8G ((uint8_t)0x20) /*!< ±8 g */ +#define LSM303DLHC_FULLSCALE_16G ((uint8_t)0x30) /*!< ±16 g */ +/** + * @} + */ + +/** @defgroup Acc_Block_Data_Update + * @{ + */ +#define LSM303DLHC_BlockUpdate_Continous ((uint8_t)0x00) /*!< Continuos Update */ +#define LSM303DLHC_BlockUpdate_Single ((uint8_t)0x80) /*!< Single Update: output registers not updated until MSB and LSB reading */ +/** + * @} + */ + +/** @defgroup Acc_Endian_Data_selection + * @{ + */ +#define LSM303DLHC_BLE_LSB ((uint8_t)0x00) /*!< Little Endian: data LSB @ lower address */ +#define LSM303DLHC_BLE_MSB ((uint8_t)0x40) /*!< Big Endian: data MSB @ lower address */ +/** + * @} + */ + +/** @defgroup Acc_Boot_Mode_selection + * @{ + */ +#define LSM303DLHC_BOOT_NORMALMODE ((uint8_t)0x00) +#define LSM303DLHC_BOOT_REBOOTMEMORY ((uint8_t)0x80) +/** + * @} + */ + +/** @defgroup Acc_High_Pass_Filter_Mode + * @{ + */ +#define LSM303DLHC_HPM_NORMAL_MODE_RES ((uint8_t)0x00) +#define LSM303DLHC_HPM_REF_SIGNAL ((uint8_t)0x40) +#define LSM303DLHC_HPM_NORMAL_MODE ((uint8_t)0x80) +#define LSM303DLHC_HPM_AUTORESET_INT ((uint8_t)0xC0) +/** + * @} + */ + +/** @defgroup Acc_High_Pass_CUT OFF_Frequency + * @{ + */ +#define LSM303DLHC_HPFCF_8 ((uint8_t)0x00) +#define LSM303DLHC_HPFCF_16 ((uint8_t)0x10) +#define LSM303DLHC_HPFCF_32 ((uint8_t)0x20) +#define LSM303DLHC_HPFCF_64 ((uint8_t)0x30) +/** + * @} + */ + +/** @defgroup Acc_High_Pass_Filter_status + * @{ + */ +#define LSM303DLHC_HIGHPASSFILTER_DISABLE ((uint8_t)0x00) +#define LSM303DLHC_HIGHPASSFILTER_ENABLE ((uint8_t)0x08) +/** + * @} + */ + +/** @defgroup Acc_High_Pass_Filter_Click_status + * @{ + */ +#define LSM303DLHC_HPF_CLICK_DISABLE ((uint8_t)0x00) +#define LSM303DLHC_HPF_CLICK_ENABLE ((uint8_t)0x04) +/** + * @} + */ + +/** @defgroup Acc_High_Pass_Filter_AOI1_status + * @{ + */ +#define LSM303DLHC_HPF_AOI1_DISABLE ((uint8_t)0x00) +#define LSM303DLHC_HPF_AOI1_ENABLE ((uint8_t)0x01) +/** + * @} + */ + +/** @defgroup Acc_High_Pass_Filter_AOI2_status + * @{ + */ +#define LSM303DLHC_HPF_AOI2_DISABLE ((uint8_t)0x00) +#define LSM303DLHC_HPF_AOI2_ENABLE ((uint8_t)0x02) +/** + * @} + */ + +/** @defgroup Acc_LSM303DLHC_Interrupt1_Configuration_definition + * @{ + */ +#define LSM303DLHC_IT1_CLICK ((uint8_t)0x80) +#define LSM303DLHC_IT1_AOI1 ((uint8_t)0x40) +#define LSM303DLHC_IT1_AOI2 ((uint8_t)0x20) +#define LSM303DLHC_IT1_DRY1 ((uint8_t)0x10) +#define LSM303DLHC_IT1_DRY2 ((uint8_t)0x08) +#define LSM303DLHC_IT1_WTM ((uint8_t)0x04) +#define LSM303DLHC_IT1_OVERRUN ((uint8_t)0x02) +/** + * @} + */ + +/** @defgroup Acc_LSM303DLHC_Interrupt2_Configuration_definition + * @{ + */ +#define LSM303DLHC_IT2_CLICK ((uint8_t)0x80) +#define LSM303DLHC_IT2_INT1 ((uint8_t)0x40) +#define LSM303DLHC_IT2_INT2 ((uint8_t)0x20) +#define LSM303DLHC_IT2_BOOT ((uint8_t)0x10) +#define LSM303DLHC_IT2_ACT ((uint8_t)0x08) +#define LSM303DLHC_IT2_HLACTIVE ((uint8_t)0x02) +/** + * @} + */ + +/** @defgroup Acc_INT_Combination_Status + * @{ + */ +#define LSM303DLHC_OR_COMBINATION ((uint8_t)0x00) /*!< OR combination of enabled IRQs */ +#define LSM303DLHC_AND_COMBINATION ((uint8_t)0x80) /*!< AND combination of enabled IRQs */ +#define LSM303DLHC_MOV_RECOGNITION ((uint8_t)0x40) /*!< 6D movement recognition */ +#define LSM303DLHC_POS_RECOGNITION ((uint8_t)0xC0) /*!< 6D position recognition */ +/** + * @} + */ + +/** @defgroup Acc_INT_Axes + * @{ + */ +#define LSM303DLHC_Z_HIGH ((uint8_t)0x20) /*!< Z High enabled IRQs */ +#define LSM303DLHC_Z_LOW ((uint8_t)0x10) /*!< Z low enabled IRQs */ +#define LSM303DLHC_Y_HIGH ((uint8_t)0x08) /*!< Y High enabled IRQs */ +#define LSM303DLHC_Y_LOW ((uint8_t)0x04) /*!< Y low enabled IRQs */ +#define LSM303DLHC_X_HIGH ((uint8_t)0x02) /*!< X High enabled IRQs */ +#define LSM303DLHC_X_LOW ((uint8_t)0x01) /*!< X low enabled IRQs */ +/** + * @} + */ + +/** @defgroup Acc_INT_Click + * @{ + */ +#define LSM303DLHC_Z_DOUBLE_CLICK ((uint8_t)0x20) /*!< Z double click IRQs */ +#define LSM303DLHC_Z_SINGLE_CLICK ((uint8_t)0x10) /*!< Z single click IRQs */ +#define LSM303DLHC_Y_DOUBLE_CLICK ((uint8_t)0x08) /*!< Y double click IRQs */ +#define LSM303DLHC_Y_SINGLE_CLICK ((uint8_t)0x04) /*!< Y single click IRQs */ +#define LSM303DLHC_X_DOUBLE_CLICK ((uint8_t)0x02) /*!< X double click IRQs */ +#define LSM303DLHC_X_SINGLE_CLICK ((uint8_t)0x01) /*!< X single click IRQs */ +/** + * @} + */ + +/** @defgroup Acc_INT1_Interrupt_status + * @{ + */ +#define LSM303DLHC_INT1INTERRUPT_DISABLE ((uint8_t)0x00) +#define LSM303DLHC_INT1INTERRUPT_ENABLE ((uint8_t)0x80) +/** + * @} + */ + +/** @defgroup Acc_INT1_Interrupt_ActiveEdge + * @{ + */ +#define LSM303DLHC_INT1INTERRUPT_LOW_EDGE ((uint8_t)0x20) +#define LSM303DLHC_INT1INTERRUPT_HIGH_EDGE ((uint8_t)0x00) +/** + * @} + */ + +/** @defgroup Mag_Data_Rate + * @{ + */ +#define LSM303DLHC_ODR_0_75_HZ ((uint8_t) 0x00) /*!< Output Data Rate = 0.75 Hz */ +#define LSM303DLHC_ODR_1_5_HZ ((uint8_t) 0x04) /*!< Output Data Rate = 1.5 Hz */ +#define LSM303DLHC_ODR_3_0_HZ ((uint8_t) 0x08) /*!< Output Data Rate = 3 Hz */ +#define LSM303DLHC_ODR_7_5_HZ ((uint8_t) 0x0C) /*!< Output Data Rate = 7.5 Hz */ +#define LSM303DLHC_ODR_15_HZ ((uint8_t) 0x10) /*!< Output Data Rate = 15 Hz */ +#define LSM303DLHC_ODR_30_HZ ((uint8_t) 0x14) /*!< Output Data Rate = 30 Hz */ +#define LSM303DLHC_ODR_75_HZ ((uint8_t) 0x18) /*!< Output Data Rate = 75 Hz */ +#define LSM303DLHC_ODR_220_HZ ((uint8_t) 0x1C) /*!< Output Data Rate = 220 Hz */ +/** + * @} + */ + +/** @defgroup Mag_Full_Scale + * @{ + */ +#define LSM303DLHC_FS_1_3_GA ((uint8_t) 0x20) /*!< Full scale = ±1.3 Gauss */ +#define LSM303DLHC_FS_1_9_GA ((uint8_t) 0x40) /*!< Full scale = ±1.9 Gauss */ +#define LSM303DLHC_FS_2_5_GA ((uint8_t) 0x60) /*!< Full scale = ±2.5 Gauss */ +#define LSM303DLHC_FS_4_0_GA ((uint8_t) 0x80) /*!< Full scale = ±4.0 Gauss */ +#define LSM303DLHC_FS_4_7_GA ((uint8_t) 0xA0) /*!< Full scale = ±4.7 Gauss */ +#define LSM303DLHC_FS_5_6_GA ((uint8_t) 0xC0) /*!< Full scale = ±5.6 Gauss */ +#define LSM303DLHC_FS_8_1_GA ((uint8_t) 0xE0) /*!< Full scale = ±8.1 Gauss */ +/** + * @} + */ + +/** + * @defgroup Magnetometer_Sensitivity + * @{ + */ +#define LSM303DLHC_M_SENSITIVITY_XY_1_3Ga 1100 /*!< magnetometer X Y axes sensitivity for 1.3 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_XY_1_9Ga 855 /*!< magnetometer X Y axes sensitivity for 1.9 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_XY_2_5Ga 670 /*!< magnetometer X Y axes sensitivity for 2.5 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_XY_4Ga 450 /*!< magnetometer X Y axes sensitivity for 4 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_XY_4_7Ga 400 /*!< magnetometer X Y axes sensitivity for 4.7 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_XY_5_6Ga 330 /*!< magnetometer X Y axes sensitivity for 5.6 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_XY_8_1Ga 230 /*!< magnetometer X Y axes sensitivity for 8.1 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_1_3Ga 980 /*!< magnetometer Z axis sensitivity for 1.3 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_1_9Ga 760 /*!< magnetometer Z axis sensitivity for 1.9 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_2_5Ga 600 /*!< magnetometer Z axis sensitivity for 2.5 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_4Ga 400 /*!< magnetometer Z axis sensitivity for 4 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_4_7Ga 355 /*!< magnetometer Z axis sensitivity for 4.7 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_5_6Ga 295 /*!< magnetometer Z axis sensitivity for 5.6 Ga full scale [LSB/Ga] */ +#define LSM303DLHC_M_SENSITIVITY_Z_8_1Ga 205 /*!< magnetometer Z axis sensitivity for 8.1 Ga full scale [LSB/Ga] */ +/** + * @} + */ + +/** @defgroup Mag_Working_Mode + * @{ + */ +#define LSM303DLHC_CONTINUOS_CONVERSION ((uint8_t) 0x00) /*!< Continuous-Conversion Mode */ +#define LSM303DLHC_SINGLE_CONVERSION ((uint8_t) 0x01) /*!< Single-Conversion Mode */ +#define LSM303DLHC_SLEEP ((uint8_t) 0x02) /*!< Sleep Mode */ +/** + * @} + */ + +/** @defgroup Mag_Temperature_Sensor + * @{ + */ +#define LSM303DLHC_TEMPSENSOR_ENABLE ((uint8_t) 0x80) /*!< Temp sensor Enable */ +#define LSM303DLHC_TEMPSENSOR_DISABLE ((uint8_t) 0x00) /*!< Temp sensor Disable */ + + +bool lsm303dlhcAccDetect(acc_t *acc); + diff --git a/src/drivers/bus_i2c_stm32f30x.c b/src/drivers/bus_i2c_stm32f30x.c index bda9137c44..3469d13ebc 100644 --- a/src/drivers/bus_i2c_stm32f30x.c +++ b/src/drivers/bus_i2c_stm32f30x.c @@ -12,27 +12,291 @@ #ifndef SOFT_I2C +#define I2C_SHORT_TIMEOUT ((uint32_t)0x1000) +#define I2C_LONG_TIMEOUT ((uint32_t)(10 * I2C_SHORT_TIMEOUT)) + + +#define I2C1_SCL_GPIO GPIOB +#define I2C1_SCL_PIN GPIO_Pin_6 +#define I2C1_SCL_PIN_SOURCE GPIO_PinSource6 +#define I2C1_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOB +#define I2C1_SDA_GPIO GPIOB +#define I2C1_SDA_PIN GPIO_Pin_7 +#define I2C1_SDA_PIN_SOURCE GPIO_PinSource7 +#define I2C1_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOB + +#define I2C2_SCL_GPIO GPIOF +#define I2C2_SCL_PIN GPIO_Pin_6 +#define I2C2_SCL_PIN_SOURCE GPIO_PinSource6 +#define I2C2_SCL_CLK_SOURCE RCC_AHBPeriph_GPIOF +#define I2C2_SDA_GPIO GPIOA +#define I2C2_SDA_PIN GPIO_Pin_10 +#define I2C2_SDA_PIN_SOURCE GPIO_PinSource10 +#define I2C2_SDA_CLK_SOURCE RCC_AHBPeriph_GPIOA + +static uint32_t i2cTimeout; + +static volatile uint16_t i2c1ErrorCount = 0; +static volatile uint16_t i2c2ErrorCount = 0; + +/////////////////////////////////////////////////////////////////////////////// +// I2C TimeoutUserCallback +/////////////////////////////////////////////////////////////////////////////// + +uint32_t i2cTimeoutUserCallback(I2C_TypeDef *I2Cx) +{ + if (I2Cx == I2C1) + { + i2c1ErrorCount++; + } + else + { + i2c2ErrorCount++; + } + return false; +} + +void i2cInitPort(I2C_TypeDef *I2Cx) +{ + GPIO_InitTypeDef GPIO_InitStructure; + I2C_InitTypeDef I2C_InitStructure; + + /////////////////////////////////// + + if (I2Cx == I2C1) + { + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + RCC_AHBPeriphClockCmd(I2C1_SCL_CLK_SOURCE | I2C1_SDA_CLK_SOURCE, ENABLE); + RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); + + //i2cUnstick(I2Cx); // Clock out stuff to make sure slaves arent stuck + + GPIO_PinAFConfig(I2C1_SCL_GPIO, I2C1_SCL_PIN_SOURCE, GPIO_AF_4); + GPIO_PinAFConfig(I2C1_SDA_GPIO, I2C1_SDA_PIN_SOURCE, GPIO_AF_4); + + GPIO_StructInit(&GPIO_InitStructure); + I2C_StructInit(&I2C_InitStructure); + + // Init pins + + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + + GPIO_InitStructure.GPIO_Pin = I2C1_SCL_PIN; + GPIO_Init(I2C1_SCL_GPIO, &GPIO_InitStructure); + + GPIO_InitStructure.GPIO_Pin = I2C1_SDA_PIN; + GPIO_Init(I2C1_SDA_GPIO, &GPIO_InitStructure); + + I2C_StructInit(&I2C_InitStructure); + + I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; + I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; + I2C_InitStructure.I2C_DigitalFilter = 0x00; + I2C_InitStructure.I2C_OwnAddress1 = 0x00; + I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; + I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + //I2C_InitStructure.I2C_Timing = 0x8000050B; + + I2C_Init(I2C1, &I2C_InitStructure); + + I2C_Cmd(I2C1, ENABLE); + } + + /////////////////////////////////// + + if (I2Cx == I2C2) + { + RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); + RCC_AHBPeriphClockCmd(I2C2_SCL_CLK_SOURCE |I2C2_SDA_CLK_SOURCE, ENABLE); + RCC_I2CCLKConfig(RCC_I2C2CLK_SYSCLK); + + //i2cUnstick(I2Cx); // Clock out stuff to make sure slaves arent stuck + + GPIO_PinAFConfig(I2C2_SCL_GPIO, I2C2_SCL_PIN_SOURCE, GPIO_AF_4); + GPIO_PinAFConfig(I2C2_SDA_GPIO, I2C2_SDA_PIN_SOURCE, GPIO_AF_4); + + GPIO_StructInit(&GPIO_InitStructure); + I2C_StructInit(&I2C_InitStructure); + + // Init pins + GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; + GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; + GPIO_InitStructure.GPIO_OType = GPIO_OType_OD; + GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; + + GPIO_InitStructure.GPIO_Pin = I2C2_SCL_PIN; + GPIO_Init(I2C2_SCL_GPIO, &GPIO_InitStructure); + + GPIO_InitStructure.GPIO_Pin = I2C2_SDA_PIN; + GPIO_Init(I2C2_SDA_GPIO, &GPIO_InitStructure); + + I2C_StructInit(&I2C_InitStructure); + + I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; + I2C_InitStructure.I2C_AnalogFilter = I2C_AnalogFilter_Enable; + I2C_InitStructure.I2C_DigitalFilter = 0x00; + I2C_InitStructure.I2C_OwnAddress1 = 0x00; + I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; + I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; + I2C_InitStructure.I2C_Timing = 0x00E0257A; // 400 Khz, 72Mhz Clock, Analog Filter Delay ON, Rise 100, Fall 10. + //I2C_InitStructure.I2C_Timing = 0x8000050B; + + I2C_Init(I2C2, &I2C_InitStructure); + + I2C_Cmd(I2C2, ENABLE); + } +} + void i2cInit(I2C_TypeDef *I2C) { - // FIXME implement + i2cInitPort(I2C1); } uint16_t i2cGetErrorCounter(void) { // FIXME implement - return 0; + return i2c1ErrorCount; } -bool i2cWrite(uint8_t addr_, uint8_t reg_, uint8_t data) +bool i2cWrite(uint8_t addr_, uint8_t reg, uint8_t data) { - // FIXME implement - return false; + I2C_TypeDef* I2Cx = I2C1; + + /* Test on BUSY Flag */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, 1, I2C_Reload_Mode, I2C_Generate_Start_Write); + + /* Wait until TXIS flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + /* Send Register address */ + I2C_SendData(I2Cx, (uint8_t) reg); + + /* Wait until TCR flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TCR) == RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, 1, I2C_AutoEnd_Mode, I2C_No_StartStop); + + /* Wait until TXIS flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) + { + if((i2cTimeout--) == 0) return + i2cTimeoutUserCallback(I2Cx); + } + + /* Write data to TXDR */ + I2C_SendData(I2Cx, data); + + /* Wait until STOPF flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) + { + if((i2cTimeout--) == 0) return i2cTimeoutUserCallback(I2Cx); + } + + /* Clear STOPF flag */ + I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); + + return true; } -bool i2cRead(uint8_t addr_, uint8_t reg_, uint8_t len, uint8_t* buf) +bool i2cRead(uint8_t addr_, uint8_t reg, uint8_t len, uint8_t* buf) { - // FIXME implement - return false; + I2C_TypeDef* I2Cx = I2C1; + /* Test on BUSY Flag */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_BUSY) != RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write); + + /* Wait until TXIS flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TXIS) == RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + if(len > 1) + reg |= 0x80; + + /* Send Register address */ + I2C_SendData(I2Cx, (uint8_t)reg); + + /* Wait until TC flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_TC) == RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + /* Configure slave address, nbytes, reload, end mode and start or stop generation */ + I2C_TransferHandling(I2Cx, addr_, len, I2C_AutoEnd_Mode, I2C_Generate_Start_Read); + + /* Wait until all data are received */ + while (len) + { + /* Wait until RXNE flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_RXNE) == RESET) + { + if((i2cTimeout--) == 0) + return i2cTimeoutUserCallback(I2Cx); + } + + /* Read data from RXDR */ + *buf = I2C_ReceiveData(I2Cx); + /* Point to the next location where the byte read will be saved */ + buf++; + + /* Decrement the read bytes counter */ + len--; + } + + /* Wait until STOPF flag is set */ + i2cTimeout = I2C_LONG_TIMEOUT; + while(I2C_GetFlagStatus(I2Cx, I2C_ISR_STOPF) == RESET) + { + if((i2cTimeout--) == 0) return + i2cTimeoutUserCallback(I2Cx); + } + + /* Clear STOPF flag */ + I2C_ClearFlag(I2Cx, I2C_ICR_STOPCF); + + /* If all operations OK */ + return true; } #endif diff --git a/src/sensors_acceleration.c b/src/sensors_acceleration.c index b49f6f6788..1c7b7832e5 100644 --- a/src/sensors_acceleration.c +++ b/src/sensors_acceleration.c @@ -18,6 +18,7 @@ acc_t acc; // acc access functions uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected sensor_align_e accAlign = 0; +uint16_t acc_1G = 256; // this is the 1G measured acceleration. uint16_t calibratingA = 0; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode. diff --git a/src/sensors_acceleration.h b/src/sensors_acceleration.h index d974a21c8c..5a0e50071f 100644 --- a/src/sensors_acceleration.h +++ b/src/sensors_acceleration.h @@ -7,13 +7,15 @@ typedef enum AccelSensors { ACC_MPU6050 = 2, ACC_MMA8452 = 3, ACC_BMA280 = 4, - ACC_FAKE = 5, - ACC_NONE = 6 + ACC_LSM303DLHC = 5, + ACC_FAKE = 6, + ACC_NONE = 7 } AccelSensors; extern uint8_t accHardware; extern sensor_align_e accAlign; extern acc_t acc; +extern uint16_t acc_1G; bool isAccelerationCalibrationComplete(void); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); diff --git a/src/sensors_gyro.c b/src/sensors_gyro.c index de0952313d..94cc7098d2 100644 --- a/src/sensors_gyro.c +++ b/src/sensors_gyro.c @@ -15,7 +15,6 @@ #include "sensors_gyro.h" uint16_t calibratingG = 0; -uint16_t acc_1G = 256; // this is the 1G measured acceleration. static gyroConfig_t *gyroConfig; diff --git a/src/sensors_gyro.h b/src/sensors_gyro.h index 8fbefee77b..d04d21465d 100644 --- a/src/sensors_gyro.h +++ b/src/sensors_gyro.h @@ -1,6 +1,5 @@ #pragma once -extern uint16_t acc_1G; extern gyro_t gyro; extern sensor_align_e gyroAlign; diff --git a/src/sensors_initialisation.c b/src/sensors_initialisation.c index 12fb5f8f77..6a62e40ebd 100755 --- a/src/sensors_initialisation.c +++ b/src/sensors_initialisation.c @@ -16,6 +16,9 @@ #include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu6050.h" +#ifdef STM32F3DISCOVERY +#include "drivers/accgyro_lsm303dlhc.h" +#endif #endif #include "drivers/barometer_common.h" @@ -146,13 +149,20 @@ retry: } #endif #ifdef STM32F3DISCOVERY - default: - if (fakeAccDetect(&acc)) { - accHardware = ACC_BMA280; - accAlign = CW0_DEG; // - if (accHardwareToUse == ACC_FAKE) + case ACC_LSM303DLHC: + if (lsm303dlhcAccDetect(&acc)) { + accHardware = ACC_LSM303DLHC; + accAlign = ALIGN_DEFAULT; // + if (accHardwareToUse == ACC_LSM303DLHC) break; } +// default: +// if (fakeAccDetect(&acc)) { +// accHardware = ACC_FAKE; +// accAlign = CW0_DEG; // +// if (accHardwareToUse == ACC_FAKE) +// break; +// } #endif } diff --git a/src/serial_cli.c b/src/serial_cli.c index 23532018ec..00521af406 100644 --- a/src/serial_cli.c +++ b/src/serial_cli.c @@ -92,7 +92,7 @@ static const char * const sensorNames[] = { }; static const char * const accNames[] = { - "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "FAKE", "None", NULL + "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "FAKE", "None", NULL }; typedef struct { diff --git a/src/serial_msp.c b/src/serial_msp.c index 60e1e2e609..86ff39d9f0 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -663,7 +663,7 @@ static void evaluateCommand(void) case MSP_DEBUG: headSerialReply(8); // make use of this crap, output some useful QA statistics - debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] + //debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock] for (i = 0; i < 4; i++) serialize16(debug[i]); // 4 variables are here for general monitoring purpose break;