Skip to content

Commit

Permalink
vtx: centralize buffer, make akk workaround optional, refactor sa parser
Browse files Browse the repository at this point in the history
  • Loading branch information
@bkleiner
bkleiner committed Aug 3, 2024
1 parent c111ec1 commit 4802fbf
Show file tree
Hide file tree
Showing 4 changed files with 84 additions and 100 deletions.
9 changes: 6 additions & 3 deletions src/config/config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -243,16 +243,19 @@
// x (micro)seconds after loss of tx or low bat before buzzer starts
#define BUZZER_DELAY 30e6

// The airbot made ab7456 osd chip will not support blink commands
// #define AIRBOT_OSD_PATCH

// some vtxes eg. from akk need an extra dummy byte
// #define USE_AKK_SA_WORKAROUND

// #############################################################################################################################
// #############################################################################################################################
// debug / other things
// this should not be usually changed or still need work
// #############################################################################################################################
// #############################################################################################################################

// The airbot made ab7456 osd chip will not support blink commands
// #define AIRBOT_OSD_PATCH

// failsafe time in uS
#define FAILSAFE_TIME_US 1000000
#define FAILSAFE_LOCK_TIME_MS 5000
Expand Down
147 changes: 62 additions & 85 deletions src/driver/vtx/sa.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ typedef enum {
PARSER_READ_MAGIC_1,
PARSER_READ_MAGIC_2,
PARSER_READ_CMD,
PARSER_READ_PAYLOAD,
PARSER_READ_LENGTH,
PARSER_READ_PAYLOAD,
PARSER_READ_CRC,
} smart_audio_parser_state_t;

Expand All @@ -43,6 +43,9 @@ static smart_audio_parser_state_t parser_state = PARSER_IDLE;
extern uint32_t vtx_last_valid_read;
extern uint32_t vtx_last_request;

extern uint8_t vtx_payload[32];
extern uint8_t vtx_payload_offset;

const uint8_t default_dac_power_levels[VTX_POWER_LEVEL_MAX] = {
7,
16,
Expand Down Expand Up @@ -105,14 +108,6 @@ static void smart_audio_auto_baud() {
packets_recv = 0;
}

static uint8_t serial_smart_audio_read_byte_crc(uint8_t *crc, uint8_t *data) {
if (serial_vtx_read_byte(data) == 0) {
return 0;
}
*crc = crc8_dvb_s2_calc(*crc, *data);
return 1;
}

static uint8_t serial_smart_audio_parse_packet(uint8_t cmd, uint8_t *payload, uint32_t length) {
switch (cmd) {
case SA_CMD_GET_SETTINGS:
Expand Down Expand Up @@ -184,93 +179,67 @@ vtx_update_result_t serial_smart_audio_update() {
if (!serial_vtx_is_ready()) {
return VTX_WAIT;
}
if (parser_state > PARSER_IDLE && (time_millis() - vtx_last_valid_read) > 500) {
if (parser_state == PARSER_IDLE) {
return VTX_IDLE;
}
if ((time_millis() - vtx_last_valid_read) > 500) {
return VTX_ERROR;
}

static uint8_t payload_offset = 0;

static uint8_t crc = 0;
static uint8_t cmd = 0;
static uint8_t length = 0;
static uint8_t payload[32];

sa_do_more:
switch (parser_state) {
case PARSER_IDLE:
return VTX_IDLE;

case PARSER_READ_MAGIC_1: {
while (true) {
uint8_t data = 0;
if (serial_vtx_read_byte(&data) == 0) {
return VTX_WAIT;
}
quic_debugf("VTX parser_state: %d 0x%x", parser_state, data);
switch (parser_state) {
case PARSER_IDLE:
return VTX_IDLE;

case PARSER_READ_MAGIC_1:
if (data == SA_MAGIC_1) {
parser_state = PARSER_READ_MAGIC_2;
}
break;

if (data == 0x0) {
// skip leading zero
goto sa_do_more;
}

if (data == SA_MAGIC_1) {
crc = 0;
cmd = 0;
length = 0;
payload_offset = 0;
parser_state = PARSER_READ_MAGIC_2;
}
goto sa_do_more;
}
case PARSER_READ_MAGIC_2: {
uint8_t data = 0;
if (serial_vtx_read_byte(&data) == 0) {
return VTX_WAIT;
}

if (data != SA_MAGIC_2) {
parser_state = PARSER_READ_MAGIC_1;
} else {
parser_state = PARSER_READ_CMD;
}
goto sa_do_more;
}
case PARSER_READ_CMD: {
if (serial_smart_audio_read_byte_crc(&crc, &cmd) == 0) {
return VTX_WAIT;
}
parser_state = PARSER_READ_LENGTH;
goto sa_do_more;
}
case PARSER_READ_LENGTH: {
if (serial_smart_audio_read_byte_crc(&crc, &length) == 0) {
return VTX_WAIT;
}
parser_state = length ? PARSER_READ_PAYLOAD : PARSER_READ_CRC;
goto sa_do_more;
}
case PARSER_READ_PAYLOAD: {
uint8_t data = 0;
if (serial_smart_audio_read_byte_crc(&crc, &data) == 0) {
return VTX_WAIT;
}

payload[payload_offset++] = data;
if (payload_offset >= length) {
parser_state = PARSER_READ_CRC;
}
goto sa_do_more;
}
case PARSER_READ_CRC: {
uint8_t data = 0;
if (serial_vtx_read_byte(&data) == 0) {
return VTX_WAIT;
}
case PARSER_READ_MAGIC_2:
if (data != SA_MAGIC_2) {
parser_state = PARSER_READ_MAGIC_1;
} else {
parser_state = PARSER_READ_CMD;
}
break;

case PARSER_READ_CMD:
vtx_payload[0] = data;
parser_state = PARSER_READ_LENGTH;
break;

case PARSER_READ_LENGTH:
length = vtx_payload[1] = data;
vtx_payload_offset = 0;
parser_state = length ? PARSER_READ_PAYLOAD : PARSER_READ_CRC;
break;

case PARSER_READ_PAYLOAD:
vtx_payload[vtx_payload_offset + 2] = data;
if (++vtx_payload_offset == (length - 1)) {
parser_state = PARSER_READ_CRC;
}
break;

parser_state = PARSER_IDLE;
if (data == crc && serial_smart_audio_parse_packet(cmd, payload, length)) {
case PARSER_READ_CRC:
parser_state = PARSER_IDLE;
if (data != crc8_dvb_s2_data(0, vtx_payload, length + 1)) {
return VTX_ERROR;
}
if (!serial_smart_audio_parse_packet(vtx_payload[0], vtx_payload + 2, length)) {
return VTX_ERROR;
}
return VTX_SUCCESS;
}
return VTX_ERROR;
}
}

return VTX_ERROR;
Expand All @@ -281,7 +250,13 @@ void serial_smart_audio_send_payload(uint8_t cmd, const uint8_t *payload, const
return;
}

const uint32_t len = size + 2 + SA_HEADER_SIZE;
#ifdef USE_AKK_SA_WORKAROUND
#define EXTRA_DUMMY_BYTES 1
#else
#define EXTRA_DUMMY_BYTES 0
#endif

const uint32_t len = size + 1 + SA_HEADER_SIZE + EXTRA_DUMMY_BYTES;
uint8_t vtx_frame[len];

vtx_frame[0] = 0x00;
Expand All @@ -292,8 +267,10 @@ void serial_smart_audio_send_payload(uint8_t cmd, const uint8_t *payload, const
for (uint8_t i = 0; i < size; i++) {
vtx_frame[i + SA_HEADER_SIZE] = payload[i];
}
vtx_frame[size + SA_HEADER_SIZE] = crc8_dvb_s2_data(0, vtx_frame + 1, len - 3);
vtx_frame[size + SA_HEADER_SIZE] = crc8_dvb_s2_data(0, vtx_frame + 1, len - 2 - EXTRA_DUMMY_BYTES);
#ifdef USE_AKK_SA_WORKAROUND
vtx_frame[size + 1 + SA_HEADER_SIZE] = 0x00;
#endif

smart_audio_auto_baud();

Expand Down
24 changes: 12 additions & 12 deletions src/driver/vtx/tramp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,9 @@ static tramp_parser_state_t parser_state = PARSER_IDLE;
extern uint32_t vtx_last_valid_read;
extern uint32_t vtx_last_request;

extern uint8_t vtx_payload[32];
extern uint8_t vtx_payload_offset;

static uint8_t crc8_data(const uint8_t *data) {
uint8_t crc = 0;
for (int i = 0; i < 13; i++) {
Expand Down Expand Up @@ -96,45 +99,42 @@ vtx_update_result_t serial_tramp_update() {
return VTX_ERROR;
}

static uint8_t payload[32];
static uint8_t payload_offset = 0;

tramp_do_more:
switch (parser_state) {
case PARSER_IDLE:
return VTX_IDLE;

case PARSER_READ_MAGIC: {
if (serial_vtx_read_byte(&payload[0]) == 0) {
if (serial_vtx_read_byte(&vtx_payload[0]) == 0) {
return VTX_WAIT;
}

if (payload[0] == 0x0F) {
if (vtx_payload[0] == 0x0F) {
parser_state = PARSER_READ_PAYLOAD;
payload_offset = 1;
vtx_payload_offset = 1;
}
goto tramp_do_more;
}
case PARSER_READ_PAYLOAD: {
if (serial_vtx_read_byte(&payload[payload_offset]) == 0) {
if (serial_vtx_read_byte(&vtx_payload[vtx_payload_offset]) == 0) {
return VTX_WAIT;
}

payload_offset++;
if (payload_offset >= 16) {
vtx_payload_offset++;
if (vtx_payload_offset >= 16) {
parser_state = PARSER_READ_CRC;
}
goto tramp_do_more;
}
case PARSER_READ_CRC: {
parser_state = PARSER_IDLE;

const uint8_t crc = crc8_data(payload + 1);
if (payload[14] != crc || payload[15] != 0) {
const uint8_t crc = crc8_data(vtx_payload + 1);
if (vtx_payload[14] != crc || vtx_payload[15] != 0) {
return VTX_ERROR;
}

if (!tramp_parse_packet(payload + 1)) {
if (!tramp_parse_packet(vtx_payload + 1)) {
return VTX_ERROR;
}

Expand Down
4 changes: 4 additions & 0 deletions src/driver/vtx/vtx.c
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "driver/vtx/vtx.h"

#include "core/debug.h"
#include "driver/serial.h"
#include "driver/serial_soft.h"
#include "driver/time.h"
Expand Down Expand Up @@ -34,6 +35,9 @@ serial_port_t serial_vtx = {
uint32_t vtx_last_valid_read = 0;
uint32_t vtx_last_request = 0;

uint8_t vtx_payload[32];
uint8_t vtx_payload_offset = 0;

bool serial_vtx_is_ready() {
return serial_vtx.tx_done;
}
Expand Down

0 comments on commit 4802fbf

Please sign in to comment.