/*
 * 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[1] << 8) | pMes[2];
	temp_val = ((uint16_t)pMes[3] << 8) | pMes[4];


	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;
}