198 lines
5.4 KiB
C
198 lines
5.4 KiB
C
/*
|
||
* 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;
|
||
}
|