diff --git a/Core/Inc/gyro_mes.h b/Core/Inc/gyro_mes.h new file mode 100644 index 0000000..35db368 --- /dev/null +++ b/Core/Inc/gyro_mes.h @@ -0,0 +1,65 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : gyro_mes.h + * @brief : Header for gyro_mes.c file. + * This file contains general definitions for gyroscope messages. + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __GYRO_MES_H +#define __GYRO_MES_H + +#ifdef __cplusplus +extern "C" { +#endif +/* Includes ------------------------------------------------------------------*/ + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include +#include + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ +struct sData { + int16_t temp, rate; +}; +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +/* Exported functions prototypes ---------------------------------------------*/ + +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +/* Private defines -----------------------------------------------------------*/ + +/* USER CODE BEGIN Private defines */ +/* USER CODE END Private defines */ + +/* Functions prototypes -----------------------------------------------------------*/ + +/* USER CODE BEGIN Functions prototypes */ +int8_t comand_mes(uint8_t* pOut, size_t nLen); +int8_t status_mes(uint8_t* pMes, size_t nLen, struct sData *pOut); +/* USER CODE END Functions prototypes */ +#ifdef __cplusplus +} +#endif + +#endif /* __GYRO_MES_H */ diff --git a/Core/Src/gyro_mes.c b/Core/Src/gyro_mes.c new file mode 100644 index 0000000..40c412c --- /dev/null +++ b/Core/Src/gyro_mes.c @@ -0,0 +1,197 @@ +/* + * gyro_mes.c + * + * Created on: 30 янв. 2022 г. + * Author: fedos + */ + +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : gyro_mes.c + * @brief : Body of program message to gyro + ****************************************************************************** + * @attention + * + * Copyright (c) 2022 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 "gyro_mes.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ + +/* USER CODE BEGIN PD */ + +/* ------------------------ + * Configuration + * ------------------------ */ +#define COM_MES_MODE_NORMAL 0 +#define COM_MES_MODE_FACTORY 1 + +#ifndef COM_MES_MODE +#define COM_MES_MODE COM_MES_MODE_NORMAL /* Mode selection */ +#endif + +#define COM_MES_CBIT_DISABLED 0 +#define COM_MES_CBIT_ENABLED 1 + +#ifndef COM_MES_CBIT +#define COM_MES_CBIT COM_MES_CBIT_DISABLED /* CBIT selection */ +#endif + +#define COM_MES_RATE_RANGE_INT 0 +#define COM_MES_RATE_RANGE_SPI 1 + +#ifndef COM_MES_RATE_RANGE +#define COM_MES_RATE_RANGE COM_MES_RATE_RANGE_INT /* Rate range selection */ +#endif + +#define COM_MES_RATE_RANGE_900 0u +#define COM_MES_RATE_RANGE_300 1u +#define COM_MES_RATE_RANGE_150 2u +#define COM_MES_RATE_RANGE_75 3u + +#ifndef COM_MES_RATE_RANGE_VAL +#define COM_MES_RATE_RANGE_VAL COM_MES_RATE_RANGE_900 /* Rate range value selection */ +#endif + +#define SCALE_RATE_RANGE_900 8u +#define SCALE_RATE_RANGE_300 24u +#define SCALE_RATE_RANGE_150 48u +#define SCALE_RATE_RANGE_75 96u +/* ------------------------ + * Errors + * ------------------------ */ + +#define ERROR_SIZE_SMALL 1 +#define ERROR_CRC 2 +#define ERROR_FAIL 3 +#define ERROR_IDENTIFIER 4 +#define ERROR_INVALID 5 +#define ERROR_CBIT 6 +#define ERROR_NORM 7 +#define ERROR_RATE 8 +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ + +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ + +/* USER CODE BEGIN PFP */ +uint8_t gencrc(uint8_t *pData, size_t nLen); +/* USER CODE END PFP */ + +/* Private function code\-----------------------------------------------*/ + +/* USER CODE BEGIN PFC */ +uint8_t gencrc(uint8_t *pData, size_t nLen) +{ + uint8_t crc = 0x0; + size_t i; + for (i = 0; i < nLen; i++) crc += pData[i]; + crc = 0xFF - crc; + return crc; +} + +/* USER CODE END PFC */ + +/** + * @brief Packing command messages for gyroscope + * @retval None + */ +int8_t comand_mes(uint8_t* pOut, size_t nLen) +{ + uint8_t mes = 0; + + if (nLen < 6u) return (-ERROR_SIZE_SMALL); + + mes = COM_MES_MODE << 7 + | COM_MES_CBIT << 6 + | COM_MES_RATE_RANGE << 5 + | ((COM_MES_RATE_RANGE_VAL >> 1) & 1) << 4 + | ((COM_MES_RATE_RANGE_VAL >> 0) & 1) << 3 + | 0 << 2 + | 0 << 1 + | 0 << 0; + pOut[0] = mes; + int i; + for(i = 1; i < 6; i++) pOut[i] = 0; + pOut[5] = gencrc(pOut, 5); + return 0 ; +} + +/** + * @brief Unpacking status messages from gyroscope + * @retval None + */ +int8_t status_mes(uint8_t* pMes, size_t nLen, struct sData *pOut) +{ + uint8_t rate_range_val = 0; + uint8_t scale_rate = 0; + uint16_t rate_val = 0; + uint16_t temp_val = 0; + float scale_temp = 2.75; + + if (nLen < 6u) return (-ERROR_SIZE_SMALL); + + if(pMes[5] != gencrc(pMes, 5)) return (-ERROR_CRC); + + if(((pMes[0] >> 7) & 1) != 0) return (-ERROR_FAIL); + + if(((pMes[0] >> 6) & 1) != 0 && + ((pMes[0] >> 5) & 1) != 0) return (-ERROR_IDENTIFIER); + + if(((pMes[0] >> 4) & 1) != 0) return (-ERROR_INVALID); + + if(((pMes[0] >> 3) & 1) != COM_MES_CBIT) return (-ERROR_CBIT); + + if(((pMes[0] >> 2) & 1) != 0) return (-ERROR_NORM); + + + if(COM_MES_RATE_RANGE == COM_MES_RATE_RANGE_INT) rate_range_val = COM_MES_RATE_RANGE_75; + else rate_range_val = COM_MES_RATE_RANGE_VAL; + + if(((pMes[0] >> 1) & 1) != ((rate_range_val >> 1) & 1) && + ((pMes[0] >> 0) & 1) != ((rate_range_val >> 1) & 1)) return (-ERROR_RATE); + + rate_val = ((uint16_t)pMes[2] << 8) | pMes[1]; + temp_val = ((uint16_t)pMes[4] << 8) | pMes[3]; + + + if(COM_MES_RATE_RANGE_VAL == COM_MES_RATE_RANGE_75) scale_rate = SCALE_RATE_RANGE_75; + else if(COM_MES_RATE_RANGE_VAL == COM_MES_RATE_RANGE_150) scale_rate = SCALE_RATE_RANGE_150; + else if(COM_MES_RATE_RANGE_VAL == COM_MES_RATE_RANGE_300) scale_rate = SCALE_RATE_RANGE_300; + else if(COM_MES_RATE_RANGE_VAL == COM_MES_RATE_RANGE_900) scale_rate = SCALE_RATE_RANGE_900; + + pOut->temp = (temp_val - 531) / scale_temp; + pOut->rate = (int16_t)rate_val / scale_rate; + + return 0; +}