1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126
|
#include "ma782_driver.h"
MA782_Driver* new_MA782_Driver(uint8_t dev_id, SPI_HandleTypeDef *hspi, GPIO_TypeDef *cs_port, uint16_t cs_pin) { MA782_Driver* hdev = NULL; hdev = (MA782_Driver*)malloc(sizeof(MA782_Driver)); if (hdev == NULL) { return NULL; }
hdev->dev_id = dev_id; hdev->hspi = hspi; hdev->cs_port = cs_port; hdev->cs_pin = cs_pin;
return hdev; }
void delete_MA782_Driver(MA782_Driver* hdev) { free(hdev->hspi); free(hdev->cs_port); free(hdev->data); free(hdev); }
HAL_StatusTypeDef MA782_Driver_Init(MA782_Driver *hdev, q15_t* data) { if(hdev->data != NULL) free(hdev->data); if(data == NULL) hdev->data = (q15_t*)malloc(sizeof(q15_t)); else hdev->data = data; HAL_GPIO_WritePin(hdev->cs_port, hdev->cs_pin, GPIO_PIN_SET); return HAL_OK; }
HAL_StatusTypeDef MA782_Driver_SetCommand(MA782_Driver *hdev, uint16_t pTxData, uint16_t* pRxData) { hdev->status = HAL_OK; HAL_GPIO_WritePin(hdev->cs_port, hdev->cs_pin, GPIO_PIN_RESET); hdev->tx_buf[0] = pTxData; hdev->tx_buf[1] = 0; hdev->status = HAL_SPI_TransmitReceive(hdev->hspi, (uint8_t*)hdev->tx_buf, (uint8_t*)hdev->rx_buf, 2, HAL_MAX_DELAY); if((hdev->rx_buf[0] & 0xF0) != (hdev->rx_buf[1] & 0xF0)) hdev->status = HAL_ERROR; else *pRxData = hdev->rx_buf[1] & 0x0F; HAL_GPIO_WritePin(hdev->cs_port, hdev->cs_pin, GPIO_PIN_SET); return hdev->status; }
HAL_StatusTypeDef MA782_Driver_ReadAngle(MA782_Driver *hdev) { HAL_GPIO_WritePin(hdev->cs_port, hdev->cs_pin, GPIO_PIN_RESET); hdev->tx_buf[0] = 0; hdev->tx_buf[1] = 0; hdev->status = HAL_SPI_TransmitReceive(hdev->hspi, (uint8_t*)hdev->tx_buf, (uint8_t*)hdev->rx_buf, 1, HAL_MAX_DELAY); if (hdev->status == HAL_OK) *hdev->data = (q15_t)(hdev->rx_buf[0] << 1) - hdev->origin; HAL_GPIO_WritePin(hdev->cs_port, hdev->cs_pin, GPIO_PIN_SET); return hdev->status; }
bool MA782_Driver_CheckMagField(MA782_Driver *hdev) { uint16_t cmd = MA782_CMD_READ_REGISTER | MA782_REG_MAGNET_FLAGS; uint8_t* rxdata; MA782_Driver_SetCommand(hdev, cmd, (uint16_t*)rxdata); if(*rxdata & 0xC0) return false; else return true; }
HAL_StatusTypeDef MA782_Driver_SetOrigin(MA782_Driver *hdev, q15_t origin) { hdev->origin = origin; uint16_t cmd = MA782_CMD_WRITE_REGISTER | MA782_REG_ZERO_HIGH | (hdev->origin >> 8); uint8_t* rxdata; hdev->status = MA782_Driver_SetCommand(hdev, cmd, (uint16_t*)rxdata); if(*rxdata != (hdev->origin >> 8)) hdev->status = HAL_ERROR; cmd = MA782_CMD_WRITE_REGISTER | MA782_REG_ZERO_LOW | (hdev->origin & 0x00FF); hdev->status = MA782_Driver_SetCommand(hdev, cmd, (uint16_t*)rxdata); if(*rxdata != (hdev->origin & 0x00FF)) hdev->status = HAL_ERROR; return hdev->status; }
HAL_StatusTypeDef MA782_Driver_SetRotDirection(MA782_Driver *hdev, bool dir) { uint16_t cmd = MA782_CMD_WRITE_REGISTER | MA782_REG_ROTATION_DIRECTION | (dir << 7); uint8_t* rxdata; hdev->status = MA782_Driver_SetCommand(hdev, cmd, (uint16_t*)rxdata); if(*rxdata != (dir << 7)) hdev->status = HAL_ERROR; return hdev->status; }
float32_t MA782_Driver_Q15_to_angle(MA782_Driver *hdev) { return ((*hdev->data) >> 15)*360*gear_ratio; }
|