/* USER CODE BEGIN Header */ /** ****************************************************************************** * File Name : App/p2p_server_app.c * Description : P2P Server Application ****************************************************************************** * @attention * * Copyright (c) 2019-2021 STMicroelectronics. * All rights reserved. * * This software is licensed under terms that can be found in the LICENSE file * in the root directory of this software component. * If no LICENSE file comes with this software, it is provided AS-IS. * ****************************************************************************** */ /* USER CODE END Header */ /* Includes ------------------------------------------------------------------*/ #include "main.h" #include "app_common.h" #include "dbg_trace.h" #include "ble.h" #include "p2p_server_app.h" #include "stm32_seq.h" /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ #include "TJ_MPU6050.h" #include "vl53l1_api.h" #include "hw_conf.h" #include "stm32wbxx_hal_rtc_ex.h" #include "math.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ /* USER CODE BEGIN PTD */ typedef struct{ uint8_t Device_Led_Selection; uint8_t Led1; }P2P_LedCharValue_t; typedef struct{ uint8_t Device_Button_Selection; uint8_t ButtonStatus; }P2P_ButtonCharValue_t; typedef struct{ int16_t SensorHeightValue; float MPU6050AccelValueX; float MPU6050AccelValueY; float MPU6050AccelValueZ; float RollAngle; //float PitchAngle; //int16_t RawDataX; //int16_t RawDataY; //int16_t RawDataZ; }P2P_SensorCharValue_t; typedef struct { uint8_t Notification_Status; /* used to check if P2P Server is enabled to Notify */ P2P_LedCharValue_t LedControl; P2P_ButtonCharValue_t ButtonControl; P2P_SensorCharValue_t SensorControl; uint16_t ConnectionHandle; } P2P_Server_App_Context_t; /* USER CODE END PTD */ /* Private defines ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ #define VL53L1X_I2C_ADDRESS 0x52 #define PI 3.141592f /* USER CODE END PD */ /* Private macros -------------------------------------------------------------*/ /* USER CODE BEGIN PM */ /* USER CODE END PM */ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ // Variables for MPU6050 sensor MPU_ConfigTypeDef MpuConfig; ScaledData_Def CalibAccelData; RawData_Def RawAccelData, RawGyroData; // Variables for TOF laser sensor VL53L1_RangingMeasurementData_t RangingData; VL53L1_Dev_t dev; // center module VL53L1_DEV Dev = &dev; // Dev is a pointer to VL53L1_Dev_t structure ? // Timer variables static uint8_t ActivateMPUSensor_timer_Id; //static uint8_t ActivateVL53L1XSensor_timer_Id; static bool isMpuTimerActive = false; static bool isBtCommandReceived = true; /** * START of Section BLE_APP_CONTEXT */ PLACE_IN_SECTION("BLE_APP_CONTEXT") static P2P_Server_App_Context_t P2P_Server_App_Context; /** * END of Section BLE_APP_CONTEXT */ /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ /* USER CODE BEGIN PFP */ //static void P2PS_Send_Notification(void); static void P2PS_APP_LED_BUTTON_context_Init(void); /* USER CODE END PFP */ /* Functions Definition ------------------------------------------------------*/ void P2PS_STM_App_Notification(P2PS_STM_App_Notification_evt_t *pNotification) { /* USER CODE BEGIN P2PS_STM_App_Notification_1 */ /* USER CODE END P2PS_STM_App_Notification_1 */ switch(pNotification->P2P_Evt_Opcode) { /* USER CODE BEGIN P2PS_STM_App_Notification_P2P_Evt_Opcode */ #if(BLE_CFG_OTA_REBOOT_CHAR != 0) case P2PS_STM_BOOT_REQUEST_EVT: APP_DBG_MSG("-- P2P APPLICATION SERVER : BOOT REQUESTED\n"); APP_DBG_MSG(" \n\r"); *(uint32_t*)SRAM1_BASE = *(uint32_t*)pNotification->DataTransfered.pPayload; NVIC_SystemReset(); break; #endif /* USER CODE END P2PS_STM_App_Notification_P2P_Evt_Opcode */ case P2PS_STM__NOTIFY_ENABLED_EVT: /* USER CODE BEGIN P2PS_STM__NOTIFY_ENABLED_EVT */ P2P_Server_App_Context.Notification_Status = 1; APP_DBG_MSG("-- P2P APPLICATION SERVER : NOTIFICATION ENABLED\n"); APP_DBG_MSG(" \n\r"); /* USER CODE END P2PS_STM__NOTIFY_ENABLED_EVT */ break; case P2PS_STM_NOTIFY_DISABLED_EVT: /* USER CODE BEGIN P2PS_STM_NOTIFY_DISABLED_EVT */ P2P_Server_App_Context.Notification_Status = 0; APP_DBG_MSG("-- P2P APPLICATION SERVER : NOTIFICATION DISABLED\n"); APP_DBG_MSG(" \n\r"); /* USER CODE END P2PS_STM_NOTIFY_DISABLED_EVT */ break; case P2PS_STM_WRITE_EVT: /* USER CODE BEGIN P2PS_STM_WRITE_EVT */ if(pNotification->DataTransfered.pPayload[0] == 0x01){ //APP_DBG_MSG("-- P2P APPLICATION SERVER 1 : LED1 ON\n"); //APP_DBG_MSG(" \n\r"); //BSP_LED_On(LED_BLUE); //P2P_Server_App_Context.LedControl.Led1=0x01; /* LED1 ON */ //MPU6050_Receive_Sensor_Data_Action(); if(!isMpuTimerActive && isBtCommandReceived){ // We need to prevent calling this timer recursively while it is active HW_TS_Start(ActivateMPUSensor_timer_Id, THIRTY_MS); isBtCommandReceived = false; } //APP_DBG_MSG("-- The device is parallel to the ground\n"); //APP_DBG_MSG(" \n\r"); //VL53L1X_Receive_Sensor_Data_Action(); //HW_TS_Start(ActivateVL53L1XSensor_timer_Id, ONE_S); } if(pNotification->DataTransfered.pPayload[0] == 0x00){ //BSP_LED_Off(LED_BLUE); //APP_DBG_MSG("-- P2P APPLICATION SERVER 1 : LED1 OFF\n"); //APP_DBG_MSG(" \n\r"); //P2P_Server_App_Context.LedControl.Led1=0x00; /* LED1 OFF */ //HW_TS_Stop(ActivateMPUSensor_timer_Id); } if(pNotification->DataTransfered.pPayload[0] == 0x02){ enter_shutdown_mode(); } /* USER CODE END P2PS_STM_WRITE_EVT */ break; default: /* USER CODE BEGIN P2PS_STM_App_Notification_default */ /* USER CODE END P2PS_STM_App_Notification_default */ break; } /* USER CODE BEGIN P2PS_STM_App_Notification_2 */ /* USER CODE END P2PS_STM_App_Notification_2 */ return; } void P2PS_APP_Notification(P2PS_APP_ConnHandle_Not_evt_t *pNotification) { /* USER CODE BEGIN P2PS_APP_Notification_1 */ /* USER CODE END P2PS_APP_Notification_1 */ switch(pNotification->P2P_Evt_Opcode) { /* USER CODE BEGIN P2PS_APP_Notification_P2P_Evt_Opcode */ /* USER CODE END P2PS_APP_Notification_P2P_Evt_Opcode */ case PEER_CONN_HANDLE_EVT : /* USER CODE BEGIN PEER_CONN_HANDLE_EVT */ /* USER CODE END PEER_CONN_HANDLE_EVT */ break; case PEER_DISCON_HANDLE_EVT : /* USER CODE BEGIN PEER_DISCON_HANDLE_EVT */ P2PS_APP_LED_BUTTON_context_Init(); /* USER CODE END PEER_DISCON_HANDLE_EVT */ break; default: /* USER CODE BEGIN P2PS_APP_Notification_default */ /* USER CODE END P2PS_APP_Notification_default */ break; } /* USER CODE BEGIN P2PS_APP_Notification_2 */ /* USER CODE END P2PS_APP_Notification_2 */ return; } void P2PS_APP_Init(void) { /* USER CODE BEGIN P2PS_APP_Init */ //UTIL_SEQ_RegTask( 1<< CFG_TASK_SW1_BUTTON_PUSHED_ID, UTIL_SEQ_RFU, P2PS_Send_Notification ); UTIL_SEQ_RegTask(1 << CFG_TASK_RECEIVE_MPU6050_DATA_ID, UTIL_SEQ_RFU, MPU6050_Receive_Sensor_Data); UTIL_SEQ_RegTask(1 << CFG_TASK_RECEIVE_VL53L1X_DATA_ID, UTIL_SEQ_RFU, VL53L1X_Receive_Sensor_Data); /** * Create timer to activate MPU6050 sensor measurement */ HW_TS_Create(CFG_TIM_PROC_ID_MPU6050, &ActivateMPUSensor_timer_Id, hw_ts_Repeated, MPU6050_Receive_Sensor_Data_Action); /** * Create timer to activate VL53L1X sensor measurement */ //HW_TS_Create(CFG_TIM_PROC_ID_VL53L1X, &ActivateVL53L1XSensor_timer_Id, hw_ts_SingleShot, VL53L1X_Receive_Sensor_Data_Action); /** * Initialize LedButton Service */ P2P_Server_App_Context.Notification_Status=0; P2PS_APP_LED_BUTTON_context_Init(); /* USER CODE END P2PS_APP_Init */ return; } void Mpu6050_Init_Cfg(void){ //2. Configure Accelerometer and Gyroscope paramaters MpuConfig.Accel_Full_Scale = AFS_SEL_2g; MpuConfig.CONFIG_DLPF = DLPF_260A_256G_Hz; MpuConfig.ClockSource = Ext_32_768KHz; MpuConfig.Gyro_Full_Scale = FS_SEL_2000; MpuConfig.Sleep_Mode_Bit = 0; //if set to 1, the device will go in sleep mode. MPU6050_Config(&MpuConfig); return; } void MPU6050_Receive_Sensor_Data(void){ //MPU6050_Get_Accel_RawData(&RawAccelData); MPU6050_Get_Accel_Cali(&CalibAccelData); //P2P_Server_App_Context.SensorControl.RawDataX = RawAccelData.x; //P2P_Server_App_Context.SensorControl.RawDataY = RawAccelData.y; //P2P_Server_App_Context.SensorControl.RawDataZ = RawAccelData.z; P2P_Server_App_Context.SensorControl.MPU6050AccelValueX = CalibAccelData.x; P2P_Server_App_Context.SensorControl.MPU6050AccelValueY = CalibAccelData.y; P2P_Server_App_Context.SensorControl.MPU6050AccelValueZ = CalibAccelData.z; P2P_Server_App_Context.SensorControl.RollAngle = atanf(CalibAccelData.y / sqrtf(CalibAccelData.x * CalibAccelData.x + CalibAccelData.z * CalibAccelData.z)) * 1 / (PI / 180); //P2P_Server_App_Context.SensorControl.PitchAngle = -atanf(CalibAccelData.x / //sqrtf(CalibAccelData.y * CalibAccelData.y + //CalibAccelData.z * CalibAccelData.z)) * 1 / (PI / 180); P2PS_STM_App_Update_Char(P2P_READ1_CHAR_UUID, (uint8_t *)&P2P_Server_App_Context.SensorControl.MPU6050AccelValueZ); if((P2P_Server_App_Context.SensorControl.MPU6050AccelValueZ <= -975.0) && (P2P_Server_App_Context.SensorControl.MPU6050AccelValueZ >= -995.0) && (P2P_Server_App_Context.SensorControl.RollAngle <= 1.5) &&(P2P_Server_App_Context.SensorControl.RollAngle >= -1.5)){ HW_TS_Stop(ActivateMPUSensor_timer_Id); UTIL_SEQ_PauseTask(1 << CFG_TASK_RECEIVE_MPU6050_DATA_ID); if(UTIL_SEQ_IsPauseTask(1 << CFG_TASK_RECEIVE_MPU6050_DATA_ID)){ VL53L1X_Receive_Sensor_Data_Action(); isMpuTimerActive = true; } } return; } void MPU6050_Receive_Sensor_Data_Action(void){ UTIL_SEQ_SetTask(1 << CFG_TASK_RECEIVE_MPU6050_DATA_ID, CFG_SCH_PRIO_0); return; } void Vl53l1x_Init(I2C_HandleTypeDef * I2cHandle){ // Initialize vl53l1x communication parameters Dev->I2cHandle = I2cHandle; Dev->I2cDevAddr = VL53L1X_I2C_ADDRESS; // Mandatory function calls VL53L1_WaitDeviceBooted( Dev ); VL53L1_DataInit( Dev ); VL53L1_StaticInit( Dev ); // Set the distance mode VL53L1_SetDistanceMode( Dev, VL53L1_DISTANCEMODE_MEDIUM ); // Timing budget is defined as the programmed time needed by the sensor // to perform and report ranging measurement data. // Timing budget can be adjusted to improve ranging accuracy. VL53L1_SetMeasurementTimingBudgetMicroSeconds( Dev, 50000 ); // Inter-measurement is defined as the programmed time between two consecutive measurements. // Min inter-measurement must be longer than the timing budget + 4 ms. // Set the IM to a half second - a crazy long time and // You will get your range data and have nearly forever to issue the stop command. // Then, we set UINT32_MAX VL53L1_SetInterMeasurementPeriodMilliSeconds( Dev, UINT32_MAX ); // VL53L1_StartMeasurement( Dev ); // We give start in Vl53l1x_Get_Measurement function and stop when we succesfully receive a measurement. return; } int16_t Vl53l1x_Get_Measurement(void){ //uint8_t buff[50]; VL53L1_StartMeasurement( Dev ); int16_t rangingResult = 0; // polls on the device interrupt status until ranging data are ready. VL53L1_WaitMeasurementDataReady( Dev ); VL53L1_GetRangingMeasurementData( Dev, &RangingData ); rangingResult = RangingData.RangeMilliMeter; //sprintf( (char*)buff, "%d, %.2f, %.2f, %.2f\n\r", RangingData.RangeStatus, RangingData.RangeMilliMeter / 10.0, //( RangingData.SignalRateRtnMegaCps / 65536.0 ), RangingData.AmbientRateRtnMegaCps / 65336.0 ); //HAL_UART_Transmit( &huart1, buff, strlen( (char*)buff ), 0xFFFF ); VL53L1_ClearInterruptAndStartMeasurement( Dev ); return rangingResult; } void VL53L1X_Receive_Sensor_Data(void){ P2P_Server_App_Context.SensorControl.SensorHeightValue = Vl53l1x_Get_Measurement(); P2PS_STM_App_Update_Char(P2P_READ_CHAR_UUID, (uint8_t *)&P2P_Server_App_Context.SensorControl.SensorHeightValue); VL53L1_StopMeasurement( Dev ); UTIL_SEQ_PauseTask(1 << CFG_TASK_RECEIVE_VL53L1X_DATA_ID); if(UTIL_SEQ_IsPauseTask(1 << CFG_TASK_RECEIVE_VL53L1X_DATA_ID)){ UTIL_SEQ_Idle(); isMpuTimerActive = false; isBtCommandReceived = true; } return; } void VL53L1X_Receive_Sensor_Data_Action(void){ UTIL_SEQ_SetTask(1 << CFG_TASK_RECEIVE_VL53L1X_DATA_ID, CFG_SCH_PRIO_0); return; } /* void P2PS_APP_SW1_Button_Action(void) { UTIL_SEQ_SetTask( 1<