diff --git a/osd.c b/osd.c index a8b756e..ac9adf5 100644 --- a/osd.c +++ b/osd.c @@ -190,6 +190,8 @@ extern bool vtxMenuActive; extern bool vtxMenuEnabled; extern bool armed; extern bool vtxInitDone; +static uint8_t mspVtxConfigFramesReceived; +static bool mspVtxTableResentAfterFcTraffic; extern bool DrawOSD; // SRT/OSD @@ -959,49 +961,38 @@ static void rx_msp_callback(msp_msg_t *msp_message) { break; } + case MSP_SET_VTX_CONFIG: { + // A new FC configuration frame is our lifecycle marker: it occurs after + // FC boot and after a Configurator save. Require a fresh short stream of + // VTX state frames before re-advertising the read-only table. + mspVtxConfigFramesReceived = 0; + mspVtxTableResentAfterFcTraffic = false; + if (verbose) { + printf("mspVTX: FC configuration changed; table sync re-armed\n"); + } + break; + } + case MSP_GET_VTX_CONFIG: { - if (vtxInitDone) { + // The FC publishes its VTX state only after its MSP descriptor is active. + // Wait for several frames, then send the table once. This avoids losing + // the boot-time burst while the FC is still bringing UART/MSP online. + if (vtxInitDone && !mspVtxTableResentAfterFcTraffic && + ++mspVtxConfigFramesReceived >= 5) { + msp_set_vtx_config(serial_fd); + mspVtxTableResentAfterFcTraffic = true; + } + + if (vtxInitDone && verbose) { mspVtxConfigStruct *in_mspVtxConfigStruct = (mspVtxConfigStruct *)msp_message->payload; - uint16_t frequency = + uint16_t requested_frequency = (in_mspVtxConfigStruct->freqMSB << 8) | in_mspVtxConfigStruct->freqLSB; - double current_frequency = - read_current_freq_from_interface(read_setting("/etc/wfb.conf", "wlan")); - - if (verbose) - printf("mspVTX Band: %i, Channel: %i, wanted Frequency: %u, set " - "Frequency: %.0f\n", - in_mspVtxConfigStruct->band, in_mspVtxConfigStruct->channel, frequency, - current_frequency); - - if (frequency != (uint16_t)current_frequency) { - int channel = 0; - for (int i = 0; i < MAX_ENTRIES; i++) { - if (fc_list[i].frequency == frequency) - channel = fc_list[i].channel; - } - if (channel > 0) { - if (verbose) - printf("mspVTX executing channel change to channel " - "%d\n", - channel); - - set_frequency(read_setting("/etc/wfb.conf", "wlan"), channel); - - // store new channel to wfb.conf - char cha_str[32]; - sprintf(cha_str, "%i", channel); - write_setting("/etc/wfb.conf", "channel", cha_str); - } else { - printf("Never change to channel 0, we should not reach " - "here, check " - "vtx table in your fc\n"); - } - } - } else { - if (verbose) - printf("vtxInitDone not finished, should never happen\n"); + printf("mspVTX read-only: FC requested %u MHz, configured WFB frequency %d MHz\n", + requested_frequency, read_wfb_frequency()); } + // Channel changes are intentionally ignored. The active raw-radio link is + // controlled by link-mode/WFB profiles, not legacy iwconfig commands. break; } diff --git a/osd/msp/msp.c b/osd/msp/msp.c index b1a538c..bb8c44e 100644 --- a/osd/msp/msp.c +++ b/osd/msp/msp.c @@ -69,7 +69,7 @@ void wipeVtxTable(int serial_fd, int band, int channel) { uint8_t buffer[256]; construct_msp_command(buffer, MSP_SET_VTX_CONFIG, payload, sizeof(payload), MSP_OUTBOUND); - write(serial_fd, &buffer, sizeof(buffer)); + write(serial_fd, buffer, sizeof(payload) + 6); // now prepare the table payload[0] = 0; // idx LSB @@ -89,7 +89,7 @@ void wipeVtxTable(int serial_fd, int band, int channel) { payload[14] = 0; // vtxtable should be cleared construct_msp_command(buffer, MSP_SET_VTX_CONFIG, payload, sizeof(payload), MSP_OUTBOUND); - write(serial_fd, &buffer, sizeof(buffer)); + write(serial_fd, buffer, sizeof(payload) + 6); } void setVtxTableBand(int serial_fd, uint8_t band) { @@ -119,7 +119,7 @@ void setVtxTableBand(int serial_fd, uint8_t band) { } construct_msp_command(buffer, MSP_SET_VTXTABLE_BAND, payload2, sizeof(payload2), MSP_OUTBOUND); - write(serial_fd, &buffer, sizeof(buffer)); + write(serial_fd, buffer, sizeof(payload2) + 6); } void setVtxTablePowerLevel(int serial_fd, uint8_t idx) { @@ -139,27 +139,31 @@ void setVtxTablePowerLevel(int serial_fd, uint8_t idx) { construct_msp_command( buffer, MSP_SET_VTXTABLE_POWERLEVEL, payload2, sizeof(payload2), MSP_OUTBOUND); - write(serial_fd, &buffer, sizeof(buffer)); + write(serial_fd, buffer, sizeof(payload2) + 6); } void msp_set_vtx_config(int serial_fd) { - // mspVTX read current frequency setting - query_interface_for_available_frequencies(); - - frequency = read_current_freq_from_interface(read_setting("/etc/wfb.conf", "wlan")); + // WiFiLink stores the configured WFB channel in YAML. Do not use + // iwconfig: on the RTL8812EU it can report stale management-mode data + // while raw WFB is active. + frequency = read_wfb_frequency(); + if (frequency < 0) { + fprintf(stderr, "mspVTX: cannot read /etc/wfb.yaml channel\n"); + return; + } int band = 6; int channel = 3; for (int index = 0; index < FREQ_TABLE_SIZE; index++) { if (channelFreqTable[index] == frequency) { // Determine band and channel using division and modulo - band = index / 8 + 1; // Which band (integer division) - channel = (index % 8) + 1; // Channel within the band (modulo) + band = index / 8 + 1; + channel = (index % 8) + 1; if (verbose) printf("Frequency %d found in band %i, channel %d\n", frequency, band, channel); - break; // Exit after finding the frequency + break; } } diff --git a/osd/msp/msp.h b/osd/msp/msp.h index 3e60a31..772aaed 100644 --- a/osd/msp/msp.h +++ b/osd/msp/msp.h @@ -40,10 +40,10 @@ // wfb-ng list #define CHANNEL_COUNT 8 -#define BAND_COUNT 8 +#define BAND_COUNT 7 #define IS_FACTORY_BAND 0 -#define FREQ_TABLE_SIZE 64 -#define FREQ_LABEL_SIZE 64 +#define FREQ_TABLE_SIZE 56 +#define FREQ_LABEL_SIZE 56 #define MSP_REBOOT 68 // in message reboot settings diff --git a/osd/util/interface.c b/osd/util/interface.c index 089fdc2..49d60fc 100644 --- a/osd/util/interface.c +++ b/osd/util/interface.c @@ -153,3 +153,41 @@ void set_frequency(char *interface, int channel) { printf("Failed to set channel on %s\n", interface); } } + +int read_wfb_frequency(void) { + FILE *fp = fopen("/etc/wfb.yaml", "r"); + char buffer[256]; + int in_wireless = 0; + int channel = -1; + + if (fp == NULL) { + return -1; + } + + while (fgets(buffer, sizeof(buffer), fp) != NULL) { + if (strncmp(buffer, "wireless:", 9) == 0) { + in_wireless = 1; + continue; + } + + if (in_wireless && buffer[0] != ' ' && buffer[0] != ' ' && + buffer[0] != '\n') { + break; + } + + if (in_wireless && sscanf(buffer, " channel: %d", &channel) == 1) { + break; + } + } + + fclose(fp); + + if (channel >= 1 && channel <= 14) { + return 2407 + channel * 5; + } + if (channel > 14 && channel <= 196) { + return 5000 + channel * 5; + } + + return -1; +} diff --git a/osd/util/interface.h b/osd/util/interface.h index 983971d..134b0d5 100644 --- a/osd/util/interface.h +++ b/osd/util/interface.h @@ -22,3 +22,7 @@ int query_interface_for_available_frequencies(); void set_frequency(char *interface, int channel); int read_current_freq_from_interface(char *interface); + +// Read WiFiLink/OpenIPC WFB channel from the YAML runtime configuration. +// Returns the centre frequency in MHz, or -1 when no valid channel is present. +int read_wfb_frequency(void);