diff --git a/src/config/config.h b/src/config/config.h index 7f99cad0e..1caa27d6d 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -243,6 +243,12 @@ // 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 @@ -250,9 +256,6 @@ // ############################################################################################################################# // ############################################################################################################################# -// 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 diff --git a/src/driver/vtx/sa.c b/src/driver/vtx/sa.c index ceee9a880..897a4a39f 100644 --- a/src/driver/vtx/sa.c +++ b/src/driver/vtx/sa.c @@ -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; @@ -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, @@ -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: @@ -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; @@ -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; @@ -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(); diff --git a/src/driver/vtx/tramp.c b/src/driver/vtx/tramp.c index a03d0a655..35a214e2c 100644 --- a/src/driver/vtx/tramp.c +++ b/src/driver/vtx/tramp.c @@ -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++) { @@ -96,32 +99,29 @@ 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; @@ -129,12 +129,12 @@ vtx_update_result_t serial_tramp_update() { 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; } diff --git a/src/driver/vtx/vtx.c b/src/driver/vtx/vtx.c index 06f18175f..99669784f 100644 --- a/src/driver/vtx/vtx.c +++ b/src/driver/vtx/vtx.c @@ -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" @@ -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; }