/** * @brief Main program * @param None * @retval None */ int main(void) { int16_t accData[3]; #ifdef WITH_USART char msg[3] = {'0','\r','\n'}; #endif HAL_Init(); HW_Init(); BlueNRG_Init(); while (1) { HCI_Process(); if (set_connectable) { set_bluenrg_connectable(); set_connectable = 0; } /* Blink the orange LED */ if (HAL_GetTick() % 500 == 0) BSP_LED_Toggle(LED3); if (HAL_GetTick() % 100 == 0) { BSP_ACCELERO_GetXYZ(accData); Acc_Update(accData); } #ifdef WITH_USART msg[0] = (msg[0] == '9')? '0' : msg[0]+1; if (HAL_USART_Transmit(&UsartHandle, (uint8_t *)msg, 3, 5000) != HAL_OK) ColorfulRingOfDeath(); #endif } }
static void ACCELERO_ReadAcc(void) { int16_t buffer[3] = {0}; int16_t xval, yval = 0x00; /* Read Acceleration*/ BSP_ACCELERO_GetXYZ(buffer); /* Update autoreload and capture compare registers value*/ xval = buffer[0]; yval = buffer[1]; if(xval > yval) { if(xval > ThresholdHigh) { /* LEFT */ BSP_LCD_SetTextColor(LCD_COLOR_BLUE); BSP_LCD_FillCircle(CIRCLE_LEFT_X_POS, CIRCLE_LEFT_Y_POS, CIRCLE_RADIUS); HAL_Delay(10); } else if(xval < ThresholdLow) { HAL_Delay(10); } else { /* UP */ BSP_LCD_SetTextColor(LCD_COLOR_YELLOW); BSP_LCD_FillCircle(CIRCLE_UP_X_POS, CIRCLE_UP_Y_POS, CIRCLE_RADIUS); HAL_Delay(10); } } else { if(yval < ThresholdLow) { HAL_Delay(10); } else if(yval > ThresholdHigh) { /* RIGHT */ BSP_LCD_SetTextColor(LCD_COLOR_GREEN); BSP_LCD_FillCircle(CIRCLE_RIGHT_X_POS, CIRCLE_RIGHT_Y_POS, CIRCLE_RADIUS); HAL_Delay(10); } else { /* DOWN */ BSP_LCD_SetTextColor(LCD_COLOR_RED); BSP_LCD_FillCircle(CIRCLE_DOWN_X_POS, CIRCLE_DOWN_Y_POS, CIRCLE_RADIUS); HAL_Delay(10); } } BSP_LED_Off(LED_ORANGE); BSP_LED_Off(LED_GREEN); BSP_LED_Off(LED_RED); BSP_LED_Off(LED_BLUE); }
/** * @brief Test ACCELERATOR MEMS Hardware. * The main objective of this test is to check acceleration on 2 axis X and Y * @param None * @retval None */ void ACCELERO_MEMS_Test(void) { int16_t buffer[3] = {0}; int16_t xval, yval = 0x00; /* Read Acceleration*/ BSP_ACCELERO_GetXYZ(buffer); /* Update autoreload and capture compare registers value*/ xval = buffer[0]; yval = buffer[1]; if((ABS(xval))>(ABS(yval))) { if(xval > ThresholdHigh) { /* LED10 On */ BSP_LED_On(LED10); HAL_Delay(10); } else if(xval < ThresholdLow) { /* LED3 On */ BSP_LED_On(LED3); HAL_Delay(10); } else { HAL_Delay(10); } } else { if(yval < ThresholdLow) { /* LED7 On */ BSP_LED_On(LED7); HAL_Delay(10); } else if(yval > ThresholdHigh) { /* LED6 On */ BSP_LED_On(LED6); HAL_Delay(10); } else { HAL_Delay(10); } } BSP_LED_Off(LED3); BSP_LED_Off(LED6); BSP_LED_Off(LED7); BSP_LED_Off(LED4); BSP_LED_Off(LED10); BSP_LED_Off(LED8); BSP_LED_Off(LED9); BSP_LED_Off(LED5); }
/* mm/s^2, without offset */ void acc_data_obtain(float *out) { int16_t acc[3] = {0}; BSP_ACCELERO_GetXYZ(acc); acc_data_normalize(acc, out); for (int i = 0; i < 3; i++) out[i] -= acc_offset[i]; }
/** * @brief Read Acceleration data. * @param None * @retval None */ static void ACCELERO_ReadAcc(void) { int16_t buffer[3] = {0}; int16_t xval, yval = 0x00; /* Read Acceleration */ BSP_ACCELERO_GetXYZ(buffer); xval = buffer[0]; yval = buffer[1]; if((ABS(xval))>(ABS(yval))) { if(xval > ThresholdHigh) { /* LED5 On */ BSP_LED_On(LED5); HAL_Delay(10); } else if(xval < ThresholdLow) { /* LED4 On */ BSP_LED_On(LED4); HAL_Delay(10); } else { HAL_Delay(10); } } else { if(yval < ThresholdLow) { /* LED6 On */ BSP_LED_On(LED6); HAL_Delay(10); } else if(yval > ThresholdHigh) { /* LED3 On */ BSP_LED_On(LED3); HAL_Delay(10); } else { HAL_Delay(10); } } BSP_LED_Off(LED3); BSP_LED_Off(LED4); BSP_LED_Off(LED5); BSP_LED_Off(LED6); }
void acc_calculate_offset(void) { int16_t temp[3] = {0}; float eps = 0.2; for (int i=0; i<1000; i++){ int16_t acc[3] = {0}; BSP_ACCELERO_GetXYZ(acc); for (int j = 0; j < 3; j++) temp[j] = (1 - eps) * temp[j] + eps*acc[j]; } acc_data_normalize(temp, acc_offset); acc_offset[2] -= g*1000; }
void CmdAccel(int mode) { int16_t xyz[3]; if(mode != CMD_INTERACTIVE) { return; } BSP_ACCELERO_GetXYZ(xyz); printf("Accelerometer returns:\n" " X: %d\n" " Y: %d\n" " Z: %d\n", xyz[0],xyz[1],xyz[2]); }
/** * @brief SYSTICK callback. * @param None * @retval None */ void HAL_SYSTICK_Callback(void) { uint8_t *buf; uint16_t Temp_X, Temp_Y = 0x00; uint16_t NewARR_X, NewARR_Y = 0x00; if (DemoEnterCondition != 0x00) { buf = USBD_HID_GetPos(); if((buf[1] != 0) ||(buf[2] != 0)) { USBD_HID_SendReport (&hUSBDDevice, buf, 4); } Counter ++; if (Counter == 10) { /* Reset Buffer used to get accelerometer values */ Buffer[0] = 0; Buffer[1] = 0; /* Disable All TIM4 Capture Compare Channels */ HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_1); HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_2); HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_3); HAL_TIM_PWM_Stop(&htim4, TIM_CHANNEL_4); /* Read Acceleration*/ BSP_ACCELERO_GetXYZ(Buffer); /* Set X and Y positions */ X_Offset = Buffer[0]; Y_Offset = Buffer[1]; /* Update New autoreload value in case of X or Y acceleration*/ /* Basic acceleration X_Offset and Y_Offset are divide by 40 to fir with ARR range */ NewARR_X = TIM_ARR - ABS(X_Offset/40); NewARR_Y = TIM_ARR - ABS(Y_Offset/40); /* Calculation of Max acceleration detected on X or Y axis */ Temp_X = ABS(X_Offset/40); Temp_Y = ABS(Y_Offset/40); MaxAcceleration = MAX_AB(Temp_X, Temp_Y); if(MaxAcceleration != 0) { /* Reset CNT to a lowest value (equal to min CCRx of all Channels) */ __HAL_TIM_SET_COUNTER(&htim4,(TIM_ARR-MaxAcceleration)/2); if (X_Offset < ThreadholdAcceleroLow) { /* Sets the TIM4 Capture Compare for Channel1 Register value */ /* Equal to NewARR_X/2 to have duty cycle equal to 50% */ __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, NewARR_X/2); /* Time base configuration */ __HAL_TIM_SET_AUTORELOAD(&htim4, NewARR_X); /* Enable TIM4 Capture Compare Channel1 */ HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1); } else if (X_Offset > ThreadholdAcceleroHigh) { /* Sets the TIM4 Capture Compare for Channel3 Register value */ /* Equal to NewARR_X/2 to have duty cycle equal to 50% */ __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_3, NewARR_X/2); /* Time base configuration */ __HAL_TIM_SET_AUTORELOAD(&htim4, NewARR_X); /* Enable TIM4 Capture Compare Channel3 */ HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3); } if (Y_Offset > ThreadholdAcceleroHigh) { /* Sets the TIM4 Capture Compare for Channel2 Register value */ /* Equal to NewARR_Y/2 to have duty cycle equal to 50% */ __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_2,NewARR_Y/2); /* Time base configuration */ __HAL_TIM_SET_AUTORELOAD(&htim4, NewARR_Y); /* Enable TIM4 Capture Compare Channel2 */ HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2); } else if (Y_Offset < ThreadholdAcceleroLow) { /* Sets the TIM4 Capture Compare for Channel4 Register value */ /* Equal to NewARR_Y/2 to have duty cycle equal to 50% */ __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, NewARR_Y/2); /* Time base configuration */ __HAL_TIM_SET_AUTORELOAD(&htim4, NewARR_Y); /* Enable TIM4 Capture Compare Channel4 */ HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4); } } Counter = 0x00; } } }
/** * \brief Module executive. Call once for each iteration of the * main processing loop. Drives the module state machine. */ void sensorHub_exec(void) { int16_t data_XYZ[3]; GPIO_PinState chamberBit; // Read ADC snd.adcL1Val += adcGetValue(ADC_RSVR_HIGH); snd.adcL2Val += adcGetValue(ADC_RSVR_LOW); // Average the ADC values for the Level sensors snd.adcLevelAvgCounter++; if (snd.adcLevelAvgCounter >= SYS_TICKS_PER_LEVEL_UPDATE) { snd.adcLevelAvgCounter = 0; bool L1_is_high = false; bool L2_is_high = false; snd.adcL1AvgVal = snd.adcL1Val/SYS_TICKS_PER_LEVEL_UPDATE; snd.adcL2AvgVal = snd.adcL2Val/SYS_TICKS_PER_LEVEL_UPDATE; if (snd.adcL1AvgVal > snd.adcL1Thresh) { L1_is_high = true; } if (snd.adcL2AvgVal > snd.adcL2Thresh) { L2_is_high = true; } // Level Logic // L1 ADC reading Low+L2 ADC High= butter full // L1 ADC reading high+L2 ADC high= butter half full // L1 ADC reading high +L2 ADC low= butter empty if (!L1_is_high && L2_is_high) { snd.LevelStatus = SH_LEVEL_STATUS_FULL; } else if (L1_is_high && L2_is_high) { snd.LevelStatus = SH_LEVEL_STATUS_HALF_FULL; } else if (L1_is_high && !L2_is_high) { snd.LevelStatus = SH_LEVEL_STATUS_EMPTY; } Log_User_SetLev(snd.LevelStatus); Log_User_SetL1Adc(snd.adcL1AvgVal); Log_User_SetL2Adc(snd.adcL2AvgVal); // printf("\r\n%03d: L1 ADC VAL: 0x%03x, L2 ADC VAL: 0x%03x\r\n", snd.adcLevelMeasCount, snd.adcL1AvgVal, snd.adcL2AvgVal); snd.adcL1Val = 0; snd.adcL2Val = 0; snd.adcLevelMeasCount++; } // Read Tilt Status BSP_ACCELERO_GetXYZ(data_XYZ); // If the angle is too high, then set tilt snd.TiltStatus = ComputeTilt(data_XYZ); #ifdef USE_STM32F4_SPRAYER // Read Chamber Status chamberBit = BSP_GpioRead(GP_CHAMB); if (chamberBit == GPIO_PIN_SET) { snd.TiltStatus = SH_CHAMBER_STATUS_CLOSED; } else { snd.ChamberStatus = SH_CHAMBER_STATUS_OPEN; } #endif }
void acc_get(int16_t *value) { BSP_ACCELERO_GetXYZ(value); }