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