mirror of
https://github.com/goldenfishs/MRobot.git
synced 2025-09-14 21:04:32 +08:00
90 lines
3.3 KiB
C
90 lines
3.3 KiB
C
/* Includes ----------------------------------------------------------------- */
|
||
#include <stdio.h>
|
||
#include <string.h>
|
||
#include "device/vofa.h"
|
||
#include "bsp/uart.h"
|
||
/* Private define ----------------------------------------------------------- */
|
||
|
||
#define MAX_CHANNEL 64u // <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||
|
||
#define JUSTFLOAT_TAIL 0x7F800000
|
||
/* Private macro ------------------------------------------------------------ */
|
||
/* Private typedef ---------------------------------------------------------- */
|
||
/* Private variables -------------------------------------------------------- */
|
||
static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)];
|
||
static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER; // Ĭ<><C4AC>Э<EFBFBD><D0AD>
|
||
|
||
/* Private function -------------------------------------------------------- */
|
||
|
||
/************************ RawData *************************/
|
||
void VOFA_RawData_Send(const char* data, bool dma) {
|
||
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t*)data, strlen(data), dma);
|
||
}
|
||
|
||
/************************ FireWater *************************/
|
||
void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma)
|
||
{
|
||
if (channel_count == 0 || channel_count > MAX_CHANNEL)
|
||
return;
|
||
|
||
char *buf = (char *)vofa_tx_buf;
|
||
size_t len = 0;
|
||
|
||
for (uint8_t i = 0; i < channel_count; ++i) {
|
||
len += snprintf(buf + len,
|
||
sizeof(vofa_tx_buf) - len,
|
||
"%s%.2f",
|
||
(i ? "," : ""),
|
||
channels[i]);
|
||
}
|
||
snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n");
|
||
|
||
BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, strlen(buf), dma);
|
||
}
|
||
|
||
/************************ JustFloat *************************/
|
||
void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma)
|
||
{
|
||
if (channel_count == 0 || channel_count > MAX_CHANNEL)
|
||
return;
|
||
memcpy(vofa_tx_buf, channels, channel_count * sizeof(float));
|
||
|
||
uint32_t tail = JUSTFLOAT_TAIL; // 0x7F800000
|
||
memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail));
|
||
|
||
BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma);
|
||
}
|
||
|
||
/* Exported functions ------------------------------------------------------- */
|
||
int8_t VOFA_init(VOFA_Protocol_t protocol) {
|
||
current_protocol = protocol;
|
||
return DEVICE_OK;
|
||
}
|
||
|
||
int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) {
|
||
switch (current_protocol) {
|
||
case VOFA_PROTOCOL_RAWDATA:
|
||
{
|
||
char data[256];
|
||
if (channel_count >= 1) {
|
||
sprintf(data, "Channel1: %.2f", channels[0]);
|
||
if (channel_count >= 2) {
|
||
sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]);
|
||
}
|
||
strcat(data, "\n");
|
||
VOFA_RawData_Send(data, dma);
|
||
}
|
||
}
|
||
break;
|
||
case VOFA_PROTOCOL_FIREWATER:
|
||
VOFA_FireWater_Send(channels, channel_count, dma);
|
||
break;
|
||
case VOFA_PROTOCOL_JUSTFLOAT:
|
||
VOFA_JustFloat_Send(channels, channel_count, dma);
|
||
break;
|
||
default:
|
||
return DEVICE_ERR;
|
||
}
|
||
return DEVICE_OK;
|
||
}
|