1
0
Fork 0

Various improvements for the AnnePro2 (#16579)

This commit is contained in:
jpe230 2022-03-08 23:49:29 -06:00 committed by GitHub
parent 6ab5a7d048
commit dc67fd9b87
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
19 changed files with 526 additions and 304 deletions

View file

@ -20,38 +20,38 @@
/* UART communication protocol state */
protocol_t proto;
void protoInit(protocol_t *proto, void (*callback)(const message_t *)) {
proto->previousId = 0;
void proto_init(protocol_t *proto, void (*callback)(const message_t *)) {
proto->previous_id = 0;
proto->callback = callback;
proto->state = STATE_SYNC_1;
proto->errors = 0;
}
static uint8_t msgId = 0;
void protoTx(uint8_t cmd, const unsigned char *buf, int payloadSize, int retries) {
chDbgCheck(payloadSize <= MAX_PAYLOAD_SIZE);
static uint8_t msg_id = 0;
void proto_tx(uint8_t cmd, const unsigned char *buf, int payload_size, int retries) {
chDbgCheck(payload_size <= MAX_PAYLOAD_SIZE);
const uint8_t header[5] = {
0x7A, 0x1D, cmd, ++msgId, payloadSize,
0x7A, 0x1D, cmd, ++msg_id, payload_size,
};
/* We don't implement ACKs, yet some messages should not be lost. */
for (int i = 0; i < retries; i++) {
sdWrite(&PROTOCOL_SD, header, sizeof(header));
if (payloadSize) sdWrite(&PROTOCOL_SD, buf, payloadSize);
if (payload_size) sdWrite(&PROTOCOL_SD, buf, payload_size);
}
}
static inline void messageReceived(protocol_t *proto) {
if (proto->buffer.msgId != proto->previousId) {
if (proto->buffer.msg_id != proto->previous_id) {
/* It's not a resend / duplicate */
proto->callback(&proto->buffer);
proto->previousId = proto->buffer.msgId;
proto->previous_id = proto->buffer.msg_id;
}
proto->state = STATE_SYNC_1;
}
void protoConsume(protocol_t *proto, uint8_t byte) {
void proto_consume(protocol_t *proto, uint8_t byte) {
switch (proto->state) {
case STATE_SYNC_1:
if (byte == 0x7A) {
@ -76,18 +76,18 @@ void protoConsume(protocol_t *proto, uint8_t byte) {
return;
case STATE_ID:
proto->buffer.msgId = byte;
proto->buffer.msg_id = byte;
proto->state = STATE_PAYLOAD_SIZE;
return;
case STATE_PAYLOAD_SIZE:
proto->buffer.payloadSize = byte;
if (proto->buffer.payloadSize > MAX_PAYLOAD_SIZE) {
proto->buffer.payloadSize = MAX_PAYLOAD_SIZE;
proto->buffer.payload_size = byte;
if (proto->buffer.payload_size > MAX_PAYLOAD_SIZE) {
proto->buffer.payload_size = MAX_PAYLOAD_SIZE;
proto->errors++;
}
proto->payloadPosition = 0;
if (proto->buffer.payloadSize == 0) {
proto->payload_position = 0;
if (proto->buffer.payload_size == 0) {
/* No payload - whole message received */
messageReceived(proto);
} else {
@ -98,9 +98,9 @@ void protoConsume(protocol_t *proto, uint8_t byte) {
case STATE_PAYLOAD:
/* NOTE: This could be read with sdReadTimeout probably, but that breaks
* abstraction */
proto->buffer.payload[proto->payloadPosition] = byte;
proto->payloadPosition++;
if (proto->payloadPosition == proto->buffer.payloadSize) {
proto->buffer.payload[proto->payload_position] = byte;
proto->payload_position++;
if (proto->payload_position == proto->buffer.payload_size) {
/* Payload read - message received */
messageReceived(proto);
}
@ -108,7 +108,7 @@ void protoConsume(protocol_t *proto, uint8_t byte) {
}
}
void protoSilence(protocol_t *proto) {
void proto_silence(protocol_t *proto) {
if (proto->state != STATE_SYNC_1) {
proto->state = STATE_SYNC_1;
proto->errors++;