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