diff --git a/osd.c b/osd.c index a8b756e..7e5ca2a 100644 --- a/osd.c +++ b/osd.c @@ -959,48 +959,37 @@ static void rx_msp_callback(msp_msg_t *msp_message) { break; } - case MSP_GET_VTX_CONFIG: { - if (vtxInitDone) { - mspVtxConfigStruct *in_mspVtxConfigStruct = (mspVtxConfigStruct *)msp_message->payload; - uint16_t frequency = - (in_mspVtxConfigStruct->freqMSB << 8) | in_mspVtxConfigStruct->freqLSB; + case MSP_SET_VTX_CONFIG: + // FC-side desired settings are read-only for this VTX. The next + // MSP_VTX_CONFIG reply will re-advertise the actual WFB runtime. + break; - double current_frequency = - read_current_freq_from_interface(read_setting("/etc/wfb.conf", "wlan")); + case MSP_GET_VTX_CONFIG: { + if (msp_message->size < 15) { + break; + } - 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"); - } - } + mspVtxConfigStruct *fc_vtx = (mspVtxConfigStruct *)msp_message->payload; + bool table_ready = fc_vtx->vtxTableAvailable && + fc_vtx->bands == BAND_COUNT && + fc_vtx->channels == CHANNEL_COUNT && + fc_vtx->powerLevels == NUM_POWER_LEVELS; + + // This is an actual FC reply to our periodic MSP_VTX_CONFIG request. + // Keep publishing the true local WFB configuration. If the FC reset or + // discarded its table, rebuild it here instead of depending on boot order. + if (table_ready) { + msp_send_vtx_runtime_config(serial_fd); } else { - if (verbose) - printf("vtxInitDone not finished, should never happen\n"); + msp_set_vtx_config(serial_fd); + } + + if (verbose) { + uint16_t fc_frequency = + (fc_vtx->freqMSB << 8) | fc_vtx->freqLSB; + printf("mspVTX: FC %u MHz, table %u/%u/%u; local WFB %d MHz\n", + fc_frequency, fc_vtx->bands, fc_vtx->channels, fc_vtx->powerLevels, + read_wfb_frequency()); } break; } diff --git a/osd/msp/msp.c b/osd/msp/msp.c index b1a538c..d834780 100644 --- a/osd/msp/msp.c +++ b/osd/msp/msp.c @@ -5,6 +5,7 @@ #include #include #include +#include #include extern bool verbose; @@ -33,12 +34,11 @@ uint16_t channelFreqTable[FREQ_TABLE_SIZE] = { 5925 // WLAN CHANNEL: 132, 140, 144, 153, 161, 169, 177, 185 }; -uint8_t saPowerLevelsLut[NUM_POWER_LEVELS] = {20, 25, 30, 35, 45, 50, 55, 58}; +uint8_t saPowerLevelsLut[NUM_POWER_LEVELS] = {0, 10, 25, 40, 55, 70, 85, 100}; -uint8_t saPowerLevelsLabel[NUM_POWER_LEVELS * POWER_LEVEL_LABEL_LENGTH] = {'2', '0', ' ', '2', '5', - ' ', '3', '0', ' ', '3', '5', ' ', '4', '5', ' ', '5', '0', ' ', '5', '5', ' ', '5', '8', ' '}; - -int frequency = 0; +uint8_t saPowerLevelsLabel[NUM_POWER_LEVELS * POWER_LEVEL_LABEL_LENGTH] = { + '0', ' ', ' ', '1', '0', ' ', '2', '5', ' ', '4', '0', ' ', + '5', '5', ' ', '7', '0', ' ', '8', '5', ' ', '1', '0', '0'}; uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg) { // return size @@ -46,50 +46,103 @@ uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg) { return msg->size + 6; } -void wipeVtxTable(int serial_fd, int band, int channel) { - - // lets first wipe all - uint8_t payload[15]; - payload[0] = 0; // idx LSB - payload[1] = 0; // idx MSB - payload[2] = 0; // power - payload[3] = 0; // pitmode - payload[4] = 0; // lowPowerDisarm - payload[5] = 0; // pitModeFreq LSB - payload[6] = 0; // pitModeFreq MSB - payload[7] = 0; // newBand - WLAN Channel - payload[8] = 0; // newChannel - 161 - payload[9] = 0; // newFreq LSB - payload[10] = 0; // newFreq MSB - payload[11] = 0; // newBandCount - payload[12] = 0; // newChannelCount - payload[13] = 0; // newPowerCount - payload[14] = 1; // vtxtable should be cleared +static void write_msp_packet(int serial_fd, const uint8_t *buffer, size_t size) { + ssize_t written = write(serial_fd, buffer, size); + if (written != (ssize_t)size) { + fprintf(stderr, "mspVTX: short serial write (%zd/%zu)\n", written, size); + return; + } - uint8_t buffer[256]; + // Betaflight's MSP parser is also carrying DisplayPort traffic on this + // UART. Drain and pace table frames so a cold boot cannot overrun its + // receive path before it has processed the preceding configuration frame. + tcdrain(serial_fd); + usleep(20 * 1000); +} - construct_msp_command(buffer, MSP_SET_VTX_CONFIG, payload, sizeof(payload), MSP_OUTBOUND); - write(serial_fd, &buffer, sizeof(buffer)); - - // now prepare the table - payload[0] = 0; // idx LSB - payload[1] = 0; // idx MSB - payload[2] = 7; // 55, default wfb-ng - payload[3] = 0; // pitmode - payload[4] = 0; // lowPowerDisarm - payload[5] = 0; // pitModeFreq LSB - payload[6] = 0; // pitModeFreq MSB - payload[7] = band; // newBand - WLAN Channel - payload[8] = channel; // newChannel - 161 - payload[9] = 0; // newFreq LSB - payload[10] = 0; // newFreq MSB - payload[11] = BAND_COUNT; // newBandCount - payload[12] = CHANNEL_COUNT; // newChannelCount - payload[13] = NUM_POWER_LEVELS; // newPowerCount - payload[14] = 0; // vtxtable should be cleared +static bool wfb_runtime_active(void) { + return system("pidof wfb_tx wfb_rx wfb_tun wifilink >/dev/null 2>&1") == 0; +} + +static uint8_t power_index_for_txpower(int txpower) { + uint8_t best = 0; + int smallest_delta = 101; + + for (uint8_t i = 0; i < NUM_POWER_LEVELS; i++) { + int delta = abs(txpower - saPowerLevelsLut[i]); + if (delta < smallest_delta) { + best = i; + smallest_delta = delta; + } + } + + return best + 1; +} + +static int get_wfb_vtx_config(int *band, int *channel, int *configured_frequency, + uint8_t *power_index, bool *pit_mode) { + *configured_frequency = read_wfb_frequency(); + if (*configured_frequency < 0) { + fprintf(stderr, "mspVTX: cannot read /etc/wfb.yaml channel\n"); + return -1; + } + + int txpower = read_wfb_txpower(); + if (txpower < 0) { + fprintf(stderr, "mspVTX: cannot read /etc/wfb.yaml txpower\n"); + return -1; + } + bool active = wfb_runtime_active(); + *pit_mode = !active; + if (!active) { + txpower = 0; + } + *power_index = power_index_for_txpower(txpower); + + *band = 1; + *channel = 1; + for (int index = 0; index < FREQ_TABLE_SIZE; index++) { + if (channelFreqTable[index] == *configured_frequency) { + *band = index / CHANNEL_COUNT + 1; + *channel = index % CHANNEL_COUNT + 1; + return 0; + } + } + + fprintf(stderr, "mspVTX: %d MHz is absent from the VTX table\n", *configured_frequency); + return -1; +} + +static void send_vtx_config(int serial_fd, int band, int channel, int configured_frequency, + uint8_t power_index, bool pit_mode, bool clear_table) { + uint8_t payload[15] = { + configured_frequency & 0xFF, + (configured_frequency >> 8) & 0xFF, + power_index, // nearest configured 0..100 WFB txpower table entry + pit_mode, // management standby is represented as VTX pit mode + 0, // low power when disarmed + 0, 0, // pit frequency + band, + channel, + configured_frequency & 0xFF, + (configured_frequency >> 8) & 0xFF, + BAND_COUNT, + CHANNEL_COUNT, + NUM_POWER_LEVELS, + clear_table ? 1 : 0, + }; + uint8_t buffer[21]; construct_msp_command(buffer, MSP_SET_VTX_CONFIG, payload, sizeof(payload), MSP_OUTBOUND); - write(serial_fd, &buffer, sizeof(buffer)); + write_msp_packet(serial_fd, buffer, sizeof(payload) + 6); +} + +static void wipeVtxTable(int serial_fd, int band, int channel, int configured_frequency, + uint8_t power_index, bool pit_mode) { + // First tell Betaflight to discard any stale table, then publish the + // configured WFB frequency and the table dimensions. + send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, pit_mode, true); + send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, pit_mode, false); } void setVtxTableBand(int serial_fd, uint8_t band) { @@ -119,7 +172,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_msp_packet(serial_fd, buffer, sizeof(payload2) + 6); } void setVtxTablePowerLevel(int serial_fd, uint8_t idx) { @@ -139,31 +192,26 @@ 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_msp_packet(serial_fd, buffer, sizeof(payload2) + 6); } void msp_set_vtx_config(int serial_fd) { + int band; + int channel; + int configured_frequency; + uint8_t power_index; + bool pit_mode; + + if (get_wfb_vtx_config(&band, &channel, &configured_frequency, &power_index, &pit_mode) != 0) { + return; + } - // mspVTX read current frequency setting - query_interface_for_available_frequencies(); - - frequency = read_current_freq_from_interface(read_setting("/etc/wfb.conf", "wlan")); - - 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) - - if (verbose) - printf("Frequency %d found in band %i, channel %d\n", frequency, band, channel); - break; // Exit after finding the frequency - } + if (verbose) { + printf("mspVTX table: %d MHz, band %d, channel %d\n", + configured_frequency, band, channel); } - wipeVtxTable(serial_fd, band, channel); + wipeVtxTable(serial_fd, band, channel, configured_frequency, power_index, pit_mode); for (int i = 1; i <= BAND_COUNT; i++) { setVtxTableBand(serial_fd, i); @@ -174,6 +222,20 @@ void msp_set_vtx_config(int serial_fd) { vtxInitDone = true; } +void msp_send_vtx_runtime_config(int serial_fd) { + int band; + int channel; + int configured_frequency; + uint8_t power_index; + bool pit_mode; + + if (get_wfb_vtx_config(&band, &channel, &configured_frequency, &power_index, &pit_mode) != 0) { + return; + } + + send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, pit_mode, false); +} + msp_error_e construct_msp_command(uint8_t message_buffer[], uint8_t command, uint8_t payload[], uint8_t size, msp_direction_e direction) { uint8_t checksum; diff --git a/osd/msp/msp.h b/osd/msp/msp.h index 3e60a31..1fe1c28 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 @@ -83,3 +83,5 @@ msp_error_e construct_msp_command(uint8_t message_buffer[], uint8_t command, uin uint8_t size, msp_direction_e direction); msp_error_e msp_process_data(msp_state_t *msp_state, uint8_t dat); void msp_set_vtx_config(int serial_fd); +// Advertise the actual local WFB configuration without resetting the FC table. +void msp_send_vtx_runtime_config(int serial_fd); diff --git a/osd/util/interface.c b/osd/util/interface.c index 089fdc2..a783431 100644 --- a/osd/util/interface.c +++ b/osd/util/interface.c @@ -153,3 +153,81 @@ 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; +} + +int read_wfb_txpower(void) { + FILE *fp = fopen("/run/wfb-power", "r"); + int txpower = -1; + + // CH6 knob state wins while WFB is active; it is intentionally kept in + // tmpfs so power changes do not wear the overlay flash. + if (fp != NULL) { + if (fscanf(fp, "%d", &txpower) == 1 && txpower >= 0 && txpower <= 100) { + fclose(fp); + return txpower; + } + fclose(fp); + } + + fp = fopen("/etc/wfb.yaml", "r"); + char buffer[256]; + int in_wireless = 0; + + 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, " txpower: %d", &txpower) == 1) { + break; + } + } + + fclose(fp); + return txpower >= 0 && txpower <= 100 ? txpower : -1; +} diff --git a/osd/util/interface.h b/osd/util/interface.h index 983971d..854c9e7 100644 --- a/osd/util/interface.h +++ b/osd/util/interface.h @@ -22,3 +22,10 @@ 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); + +// Read the configured WFB txpower scalar (0..100 convention used by this profile). +int read_wfb_txpower(void);