/** * @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); } }