add mes func
This commit is contained in:
65
Core/Inc/gyro_mes.h
Normal file
65
Core/Inc/gyro_mes.h
Normal file
@@ -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 <stdint.h>
|
||||
#include <stddef.h>
|
||||
|
||||
/* 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 */
|
||||
197
Core/Src/gyro_mes.c
Normal file
197
Core/Src/gyro_mes.c
Normal file
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user