56 lines
1.0 KiB
C
56 lines
1.0 KiB
C
/*
|
|
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
|
|
*/
|
|
|
|
|
|
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
|
|
#include <sbus.h>
|
|
|
|
#define SBUS_STARTFR_BYTE 0x0F
|
|
|
|
void sbus_init(sbus_t* bus) {
|
|
memset(bus->iframe, 0, sizeof(bus->iframe));
|
|
memset(bus->rcvalue, 0, sizeof(bus->rcvalue));
|
|
bus->findex = 0;
|
|
}
|
|
|
|
|
|
bool sbus_recv(sbus_t* bus, uint8_t ibyte) {
|
|
|
|
printf("0x%02x ", ibyte);
|
|
|
|
if(bus->findex == 0 && ibyte != SBUS_STARTFR_BYTE) {
|
|
return false;
|
|
}
|
|
if(bus->findex < SBUS_FRAMESIZE) {
|
|
bus->iframe[bus->findex] = ibyte;
|
|
bus->findex++;
|
|
|
|
//printf("0x%02x ", ibyte);
|
|
|
|
return false;
|
|
}
|
|
|
|
if(bus->findex == SBUS_FRAMESIZE) {
|
|
bus->findex = 0;
|
|
//printf("0x%02x\r\n", bus->iframe[0]);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int sbus_chcount(sbus_t* bus) {
|
|
return SBUS_CHANNELS;
|
|
}
|
|
|
|
int16_t sbus_getch(sbus_t* bus, int num) {
|
|
if (num < SBUS_CHANNELS) {
|
|
return (int16_t)bus->rcvalue[num];
|
|
}
|
|
return WRONG_RCVAL;
|
|
}
|