Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Ported some I2C code from AQ32PlusF3 which itself looked ported from the

Browse source 
STM32 examples.
This commit is contained in:
Dominic Clifton 2014-04-28 19:18:57 +01:00
parent 8d9ce86a5a
commit 4febeb3969
12 changed files with 883 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

1
Makefile
View file

@ -180,6 +180,7 @@ STM32F3DISCOVERY_SRC = startup_stm32f30x_md_gcc.S \
drivers/accgyro_mpu3050.c \ drivers/accgyro_mpu3050.c \
drivers/accgyro_mpu6050.c \ drivers/accgyro_mpu6050.c \
drivers/accgyro_l3g4200d.c \ drivers/accgyro_l3g4200d.c \
drivers/accgyro_lsm303dlhc.c \
drivers/adc_common.c \ drivers/adc_common.c \
drivers/bus_i2c_stm32f30x.c \ drivers/bus_i2c_stm32f30x.c \
drivers/bus_spi.c \ drivers/bus_spi.c \

2
src/drivers/accgyro_adxl345.c
View file

@ -77,7 +77,7 @@ static void adxl345Init(void)
i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G); i2cWrite(ADXL345_ADDRESS, ADXL345_DATA_FORMAT, ADXL345_FULL_RANGE | ADXL345_RANGE_8G);
i2cWrite(ADXL345_ADDRESS, ADXL345_BW_RATE, ADXL345_RATE_100); 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; uint8_t acc_samples = 0;

127
src/drivers/accgyro_lsm303dlhc.c Normal file
View file

@ -0,0 +1,127 @@
#include <stdbool.h>
#include <stdint.h>
#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;
}

460
src/drivers/accgyro_lsm303dlhc.h Normal file
View file

@ -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);

280
src/drivers/bus_i2c_stm32f30x.c
View file

@ -12,27 +12,291 @@
#ifndef SOFT_I2C #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) void i2cInit(I2C_TypeDef *I2C)
{ {
// FIXME implement i2cInitPort(I2C1);
} }
uint16_t i2cGetErrorCounter(void) uint16_t i2cGetErrorCounter(void)
{ {
// FIXME implement // 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 I2C_TypeDef* I2Cx = I2C1;
return false;
/* 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 I2C_TypeDef* I2Cx = I2C1;
return false; /* 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 #endif

1
src/sensors_acceleration.c
View file

@ -18,6 +18,7 @@
acc_t acc; // acc access functions acc_t acc; // acc access functions
uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected uint8_t accHardware = ACC_DEFAULT; // which accel chip is used/detected
sensor_align_e accAlign = 0; 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. 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.

6
src/sensors_acceleration.h
View file

@ -7,13 +7,15 @@ typedef enum AccelSensors {
ACC_MPU6050 = 2, ACC_MPU6050 = 2,
ACC_MMA8452 = 3, ACC_MMA8452 = 3,
ACC_BMA280 = 4, ACC_BMA280 = 4,
ACC_FAKE = 5, ACC_LSM303DLHC = 5,
ACC_NONE = 6 ACC_FAKE = 6,
ACC_NONE = 7
} AccelSensors; } AccelSensors;
extern uint8_t accHardware; extern uint8_t accHardware;
extern sensor_align_e accAlign; extern sensor_align_e accAlign;
extern acc_t acc; extern acc_t acc;
extern uint16_t acc_1G;
bool isAccelerationCalibrationComplete(void); bool isAccelerationCalibrationComplete(void);
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired); void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);

1
src/sensors_gyro.c
View file

@ -15,7 +15,6 @@
#include "sensors_gyro.h" #include "sensors_gyro.h"
uint16_t calibratingG = 0; uint16_t calibratingG = 0;
uint16_t acc_1G = 256; // this is the 1G measured acceleration.
static gyroConfig_t *gyroConfig; static gyroConfig_t *gyroConfig;

1
src/sensors_gyro.h
View file

@ -1,6 +1,5 @@
#pragma once #pragma once
extern uint16_t acc_1G;
extern gyro_t gyro; extern gyro_t gyro;
extern sensor_align_e gyroAlign; extern sensor_align_e gyroAlign;

20
src/sensors_initialisation.c
View file

@ -16,6 +16,9 @@
#include "drivers/accgyro_mma845x.h" #include "drivers/accgyro_mma845x.h"
#include "drivers/accgyro_mpu3050.h" #include "drivers/accgyro_mpu3050.h"
#include "drivers/accgyro_mpu6050.h" #include "drivers/accgyro_mpu6050.h"
#ifdef STM32F3DISCOVERY
#include "drivers/accgyro_lsm303dlhc.h"
#endif
#endif #endif
#include "drivers/barometer_common.h" #include "drivers/barometer_common.h"
@ -146,13 +149,20 @@ retry:
} }
#endif #endif
#ifdef STM32F3DISCOVERY #ifdef STM32F3DISCOVERY
default: case ACC_LSM303DLHC:
if (fakeAccDetect(&acc)) { if (lsm303dlhcAccDetect(&acc)) {
accHardware = ACC_BMA280; accHardware = ACC_LSM303DLHC;
accAlign = CW0_DEG; // accAlign = ALIGN_DEFAULT; //
if (accHardwareToUse == ACC_FAKE) if (accHardwareToUse == ACC_LSM303DLHC)
break; break;
} }
// default:
// if (fakeAccDetect(&acc)) {
// accHardware = ACC_FAKE;
// accAlign = CW0_DEG; //
// if (accHardwareToUse == ACC_FAKE)
// break;
// }
#endif #endif
} }

2
src/serial_cli.c
View file

@ -92,7 +92,7 @@ static const char * const sensorNames[] = {
}; };
static const char * const accNames[] = { static const char * const accNames[] = {
"", "ADXL345", "MPU6050", "MMA845x", "BMA280", "FAKE", "None", NULL "", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "FAKE", "None", NULL
}; };
typedef struct { typedef struct {

2
src/serial_msp.c
View file

@ -663,7 +663,7 @@ static void evaluateCommand(void)
case MSP_DEBUG: case MSP_DEBUG:
headSerialReply(8); headSerialReply(8);
// make use of this crap, output some useful QA statistics // 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++) for (i = 0; i < 4; i++)
serialize16(debug[i]); // 4 variables are here for general monitoring purpose serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break; break;