/**
* @brief Change the Full Scale of LIS3DSH
* @param FS_value: new full scale value.
* This parameter can be one of the following values:
* @arg LIS3DSH_FULLSCALE_2 : +-2g
* @arg LIS3DSH_FULLSCALE_4 : +-4g
* @arg LIS3DSH_FULLSCALE_6 : +-6g
* @arg LIS3DSH_FULLSCALE_8 : +-8g
* @arg LIS3DSH_FULLSCALE_16 : +-16g
* @retval None
*/
void LIS3DSH_FullScaleCmd(uint8_t FS_value)
{
uint8_t tmpreg;
/* Read CTRL_REG1 register */
LIS3DSH_Read(&tmpreg, LIS3DSH_CTRL_REG5, 1);
/* Set new full scale configuration */
tmpreg &= (uint8_t)0xC7;
tmpreg |= FS_value;
/* Write value to MEMS CTRL_REG1 regsister */
LIS3DSH_Write(&tmpreg, LIS3DSH_CTRL_REG5, 1);
}
/**
* @brief Change to lowpower mode for LIS3DSH
* @retval None
*/
void LIS3DSH_LowpowerCmd(void)
{
uint8_t tmpreg;
/* Read CTRL_REG1 register */
LIS3DSH_Read(&tmpreg, LIS3DSH_CTRL_REG4, 1);
/* Set new low power mode configuration */
tmpreg &= (uint8_t)0x0F;
tmpreg |= LIS3DSH_PWRDWN;
/* Write value to MEMS CTRL_REG1 regsister */
LIS3DSH_Write(&tmpreg, LIS3DSH_CTRL_REG4, 1);
}
/**
* @brief Data Rate command
* @param DataRateValue: Data rate value
* This parameter can be one of the following values:
* @arg LIS3DSH_DATARATE_3_125 : 3.125 Hz output data rate
* @arg LIS3DSH_DATARATE_6_25 : 6.25 Hz output data rate
* @arg LIS3DSH_DATARATE_12_5 : 12.5 Hz output data rate
* @arg LIS3DSH_DATARATE_25 : 25 Hz output data rate
* @arg LIS3DSH_DATARATE_50 : 50 Hz output data rate
* @arg LIS3DSH_DATARATE_100 : 100 Hz output data rate
* @arg LIS3DSH_DATARATE_400 : 400 Hz output data rate
* @arg LIS3DSH_DATARATE_800 : 800 Hz output data rate
* @arg LIS3DSH_DATARATE_1600 : 1600 Hz output data rate
* @retval None
*/
void LIS3DSH_DataRateCmd(uint8_t DataRateValue)
{
uint8_t tmpreg;
/* Read CTRL_REG1 register */
LIS3DSH_Read(&tmpreg, LIS3DSH_CTRL_REG4, 1);
/* Set new Data rate configuration */
tmpreg &= (uint8_t)0x0F;
tmpreg |= DataRateValue;
/* Write value to MEMS CTRL_REG1 regsister */
LIS3DSH_Write(&tmpreg, LIS3DSH_CTRL_REG4, 1);
}
/**
* @brief Set LIS3DSH Interrupt configuration
* @param LIS3DSH_InterruptConfig_TypeDef: pointer to a LIS3DSH_InterruptConfig_TypeDef
* structure that contains the configuration setting for the LIS3DSH Interrupt.
* @retval None
*/
void LIS3DSH_DataReadyInterruptConfig(LIS3DSH_DRYInterruptConfigTypeDef *LIS3DSH_IntConfigStruct)
{
uint8_t ctrl = 0x00;
/* Read CLICK_CFG register */
LIS3DSH_Read(&ctrl, LIS3DSH_CTRL_REG3, 1);
/* Configure latch Interrupt request, click interrupts and double click interrupts */
ctrl = (uint8_t)(LIS3DSH_IntConfigStruct->Dataready_Interrupt| \
LIS3DSH_IntConfigStruct->Interrupt_signal | \
LIS3DSH_IntConfigStruct->Interrupt_type);
/* Write value to MEMS CLICK_CFG register */
LIS3DSH_Write(&ctrl, LIS3DSH_CTRL_REG3, 1);
}
static void accelerometer_read_raw(void) {
uint8_t Buffer[6];
LIS3DSH_Read(&Buffer[0], LIS3DSH_OUT_X_L, 1);
LIS3DSH_Read(&Buffer[1], LIS3DSH_OUT_X_H, 1);
LIS3DSH_Read(&Buffer[2], LIS3DSH_OUT_Y_L, 1);
LIS3DSH_Read(&Buffer[3], LIS3DSH_OUT_Y_H, 1);
LIS3DSH_Read(&Buffer[4], LIS3DSH_OUT_Z_L, 1);
LIS3DSH_Read(&Buffer[5], LIS3DSH_OUT_Z_H, 1);
x_raw = Buffer[0] + (int16_t)(Buffer[1] << 8);
y_raw = Buffer[2] + (int16_t)(Buffer[3] << 8);
z_raw = Buffer[4] + (int16_t)(Buffer[5] << 8);
}
void accelerometer_read_raw(void) {
uint8_t Buffer[6];
#ifdef LSM9DS1
for (uint8_t i = 0; i < 6; i++) {
LSM9DS1_Read(Buffer + i, LSM9DS1_OUT_X_ADDR + i, 1);
}
#else
LIS3DSH_Read(&Buffer[0], LIS3DSH_OUT_X_L, 1);
LIS3DSH_Read(&Buffer[1], LIS3DSH_OUT_X_H, 1);
LIS3DSH_Read(&Buffer[2], LIS3DSH_OUT_Y_L, 1);
LIS3DSH_Read(&Buffer[3], LIS3DSH_OUT_Y_H, 1);
LIS3DSH_Read(&Buffer[4], LIS3DSH_OUT_Z_L, 1);
LIS3DSH_Read(&Buffer[5], LIS3DSH_OUT_Z_H, 1);
#endif
x_raw = Buffer[0] + (int16_t)(Buffer[1] << 8);
y_raw = Buffer[2] + (int16_t)(Buffer[3] << 8);
z_raw = Buffer[4] + (int16_t)(Buffer[5] << 8);
}
int16_t* Accelerometer_Read(void)
{
uint8_t Buffer[6];
LIS3DSH_Read(&Buffer[0], LIS3DSH_OUT_X_L, 1);
LIS3DSH_Read(&Buffer[1], LIS3DSH_OUT_X_H, 1);
LIS3DSH_Read(&Buffer[2], LIS3DSH_OUT_Y_L, 1);
LIS3DSH_Read(&Buffer[3], LIS3DSH_OUT_Y_H, 1);
LIS3DSH_Read(&Buffer[4], LIS3DSH_OUT_Z_L, 1);
LIS3DSH_Read(&Buffer[5], LIS3DSH_OUT_Z_H, 1);
x_raw = Buffer[0] + (int16_t)(Buffer[1] << 8);
y_raw = Buffer[2] + (int16_t)(Buffer[3] << 8);
z_raw = Buffer[4] + (int16_t)(Buffer[5] << 8);
values[0] = x_raw;
values[1] = y_raw;
values[2] = z_raw;
return values;
}
/**
* @brief Read LIS3DSH output register, and calculate the acceleration
* ACC[mg]=SENSITIVITY* (out_h*256+out_l)/16 (12 bit rappresentation)
* @param s16 buffer to store data
* @retval None
*/
void LIS3DSH_ReadACC(float* out)
{
uint8_t buffer[6];
uint8_t ctrl, i = 0x00;
uint8_t offsetX, offsetY, offsetZ;
int16_t aggregateResult = 0;
LIS3DSH_Read(&offsetX, LIS3DSH_OFF_X, 1);
LIS3DSH_Read(&offsetY, LIS3DSH_OFF_Y, 1);
LIS3DSH_Read(&offsetZ, LIS3DSH_OFF_Z, 1);
LIS3DSH_Read(&ctrl, LIS3DSH_CTRL_REG5, 1);
LIS3DSH_Read(&buffer[0], LIS3DSH_OUT_X_L, 1);
LIS3DSH_Read(&buffer[1], LIS3DSH_OUT_X_H, 1);
LIS3DSH_Read(&buffer[2], LIS3DSH_OUT_Y_L, 1);
LIS3DSH_Read(&buffer[3], LIS3DSH_OUT_Y_H, 1);
LIS3DSH_Read(&buffer[4], LIS3DSH_OUT_Z_L, 1);
LIS3DSH_Read(&buffer[5], LIS3DSH_OUT_Z_H, 1);
ctrl = (ctrl & 0x38) >> 3;
switch(ctrl)
{
/* FS bits = 000 ==> Sensitivity typical value = 0.061 milligals/digit*/
case 0x00:
for(i=0; i<0x06; i=i+2)
{
aggregateResult = (int32_t)(buffer[i] | buffer[i+1] << 8);
*out =(float)(LIS3DSH_SENSITIVITY_2G * (float)aggregateResult);
out++;
}
break;
/* FS bit = 001 ==> Sensitivity typical value = 0.122 milligals/digit*/
case 0x01:
for(i=0; i<0x06; i=i+2)
{
aggregateResult = (int32_t)(buffer[i] | buffer[i+1] << 8);
*out =(float)(LIS3DSH_SENSITIVITY_4G * (float)aggregateResult);
out++;
}
break;
/* FS bit = 010 ==> Sensitivity typical value = 0.183 milligals/digit*/
case 0x02:
for(i=0; i<0x06; i=i+2)
{
aggregateResult = (int32_t)(buffer[i] | buffer[i+1] << 8);
*out =(float)(LIS3DSH_SENSITIVITY_6G * (float)aggregateResult);
out++;
}
break;
/* FS bit = 011 ==> Sensitivity typical value = 0.244 milligals/digit*/
case 0x03:
for(i=0; i<0x06; i=i+2)
{
aggregateResult = (int32_t)(buffer[i] | buffer[i+1] << 8);
*out =(float)(LIS3DSH_SENSITIVITY_8G * (float)aggregateResult);
out++;
}
break;
/* FS bit = 100 ==> Sensitivity typical value = 0.488 milligals/digit*/
case 0x04:
for(i=0; i<0x06; i=i+2)
{
aggregateResult = (int32_t)(buffer[i] | buffer[i+1] << 8);
*out =(float)(LIS3DSH_SENSITIVITY_16G * (float)aggregateResult);
out++;
}
break;
default:
break;
}
}
void vBalanceTask(void *pvParameters)
{
const portTickType twosecDelay = 2000;
const portTickType xDelay = 3000;
vTaskDelay( twosecDelay );
PWM_Motor1 = PWM_MOTOR_INIT_MAX;
PWM_Motor2 = PWM_MOTOR_INIT_MAX;
PWM_Motor3 = PWM_MOTOR_INIT_MAX;
PWM_Motor4 = PWM_MOTOR_INIT_MAX;
vTaskDelay( xDelay ); //6S
PWM_Motor1 = PWM_MOTOR_INIT_MIN;
PWM_Motor2 = PWM_MOTOR_INIT_MIN;
PWM_Motor3 = PWM_MOTOR_INIT_MIN;
PWM_Motor4 = PWM_MOTOR_INIT_MIN;
/*inital Offset value of Gryo. and Acce.*/
LIS3DSH_Read(Buffer_Hx, LIS3DSH_OUT_X_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Hy, LIS3DSH_OUT_Y_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Lx, LIS3DSH_OUT_X_L_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Ly, LIS3DSH_OUT_Y_L_REG_ADDR, 1);
XOffset = (int16_t)(Buffer_Hx[0] << 8 | Buffer_Lx[0]);
YOffset = (int16_t)(Buffer_Hy[0] << 8 | Buffer_Ly[0]);
/* reset gyro offset */
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
GXOffset = (int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]);
GYOffset = (int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]);
GZOffset = (int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]);
int i;
for(i = 1;i<128;i++) {
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
GXOffset = (int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]);
GYOffset = (int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]);
GZOffset = (int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]);
}
GXOffset = GXOffset / 128;
GYOffset = GYOffset / 128;
GZOffset = GZOffset / 128;
angle_x = 0;
angle_y = 0;
pwm_flag = 0;
argv.PitchP = 5.5f;
argv.PitchD = 1.2f;
argv.RollP = 5.5f;
argv.RollD = 1.2f;
argv.YawD = 4.1f;
Pitch_desire = 0.0f; //Desire angle of Pitch
Roll_desire = 0.0f; //Desire angle of Roll
xTimerStart(xTimerSampleRate, 0);
while(1){
//qprintf(xQueueUARTSend, "P12:%d ,P13:%d ,P14:%d ,P15:%d ,angle_x: %d,angle_y: %d\n\r", PWM_Motor1, PWM_Motor2, PWM_Motor3, PWM_Motor4, (int)angle_x, (int)angle_y);
}
}
/* sample rate and calculate rate (4ms,250HZ) */
void vTimerSample(xTimerHandle pxTimer)
{
LIS3DSH_Read(Buffer_Hx, LIS3DSH_OUT_X_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Hy, LIS3DSH_OUT_Y_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Lx, LIS3DSH_OUT_X_L_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Ly, LIS3DSH_OUT_Y_L_REG_ADDR, 1);
x_acc = (float)((int16_t)(Buffer_Hx[0] << 8 | Buffer_Lx[0]) - XOffset) * Sensitivity_2G / 1000 * 180 / 3.14159f;
y_acc = (float)((int16_t)(Buffer_Hy[0] << 8 | Buffer_Ly[0]) - YOffset) * Sensitivity_2G / 1000 * 180 / 3.14159f;
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
x_gyro = (float)((int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]) - GXOffset) * Sensitivity_250 / 1000;
y_gyro = (float)((int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]) - GYOffset) * Sensitivity_250 / 1000;
z_gyro = (float)((int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]) - GZOffset) * Sensitivity_250 / 1000;
angle_x = (0.99f) * (angle_x + y_gyro * 0.004f) - (0.01f) * (x_acc);
angle_y = (0.99f) * (angle_y + x_gyro * 0.004f) + (0.01f) * (y_acc);
argv.Pitch = angle_y; //pitch degree
argv.Roll = angle_x; //roll degree
argv.Pitch_v = x_gyro; //pitch velocity
argv.Roll_v = y_gyro; //Roll velocity
argv.Pitch_err = Pitch_desire - argv.Pitch;
argv.Roll_err = Roll_desire - argv.Roll;
ROLL = (int)(argv.RollP * argv.Roll_err - argv.RollD * argv.Roll_v);
PITCH = (int)(argv.PitchP * argv.Pitch_err - argv.PitchD * argv.Pitch_v);
YAW = (int)(argv.YawD * z_gyro);
if (PITCH > MAXNUM) {
PITCH = MAXNUM;
}else if (PITCH < MINNUM) {
PITCH = MINNUM;
}else{
PITCH = PITCH;
}
if (ROLL > MAXNUM) {
ROLL = MAXNUM;
}else if (ROLL < MINNUM) {
ROLL = MINNUM;
}else{
ROLL = ROLL;
}
if(argv.Roll > 35 || argv.Roll < -35){
pwm_flag = 0;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
}
if(argv.Pitch > 35 || argv.Pitch < -35){
pwm_flag = 0;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
}
if(pwm_flag == 0){
}else{
Motor1 = PWM_Motor1_tmp + PITCH - ROLL - YAW; //LD4
Motor2 = PWM_Motor2_tmp + PITCH + ROLL + YAW; //LD3
Motor3 = PWM_Motor3_tmp - PITCH + ROLL - YAW; //LD5
Motor4 = PWM_Motor4_tmp - PITCH - ROLL + YAW; //LD6
Motor_Control(Motor1, Motor2, Motor3, Motor4);
}
}
