diff --git a/package/msposd/0001-wifilink-mspvtx-read-only.patch b/package/msposd/0001-wifilink-mspvtx-read-only.patch index 472e467..5ef75ae 100644 --- a/package/msposd/0001-wifilink-mspvtx-read-only.patch +++ b/package/msposd/0001-wifilink-mspvtx-read-only.patch @@ -80,7 +80,7 @@ index a8b756e..7e5ca2a 100644 break; } diff --git a/osd/msp/msp.c b/osd/msp/msp.c -index b1a538c..a527dff 100644 +index b1a538c..ad9d55d 100644 --- a/osd/msp/msp.c +++ b/osd/msp/msp.c @@ -5,6 +5,7 @@ @@ -91,15 +91,24 @@ index b1a538c..a527dff 100644 #include extern bool verbose; -@@ -38,58 +39,76 @@ uint8_t saPowerLevelsLut[NUM_POWER_LEVELS] = {20, 25, 30, 35, 45, 50, 55, 58}; - 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', ' '}; +@@ -33,12 +34,11 @@ uint16_t channelFreqTable[FREQ_TABLE_SIZE] = { + 5925 // WLAN CHANNEL: 132, 140, 144, 153, 161, 169, 177, 185 + }; --int frequency = 0; +-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 - construct_msp_command(message_buffer, msg->cmd, msg->payload, msg->size, msg->direction); +@@ -46,50 +46,94 @@ uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg) { return msg->size + 6; } @@ -156,13 +165,36 @@ index b1a538c..a527dff 100644 - payload[12] = CHANNEL_COUNT; // newChannelCount - payload[13] = NUM_POWER_LEVELS; // newPowerCount - payload[14] = 0; // vtxtable should be cleared -+static int get_wfb_vtx_config(int *band, int *channel, int *configured_frequency) { ++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) { + *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; ++ } ++ *power_index = power_index_for_txpower(txpower); ++ + *band = 1; + *channel = 1; + for (int index = 0; index < FREQ_TABLE_SIZE; index++) { @@ -178,11 +210,11 @@ index b1a538c..a527dff 100644 +} + +static void send_vtx_config(int serial_fd, int band, int channel, int configured_frequency, -+ bool clear_table) { ++ uint8_t power_index, bool clear_table) { + uint8_t payload[15] = { + configured_frequency & 0xFF, + (configured_frequency >> 8) & 0xFF, -+ 1, // power index: the active WFB profile owns actual TX power ++ power_index, // nearest configured 0..100 WFB txpower table entry + 0, // pit mode + 0, // low power when disarmed + 0, 0, // pit frequency @@ -202,15 +234,16 @@ index b1a538c..a527dff 100644 + write_msp_packet(serial_fd, buffer, sizeof(payload) + 6); +} + -+static void wipeVtxTable(int serial_fd, int band, int channel, int configured_frequency) { ++static void wipeVtxTable(int serial_fd, int band, int channel, int configured_frequency, ++ uint8_t power_index) { + // 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, true); -+ send_vtx_config(serial_fd, band, channel, configured_frequency, false); ++ send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, true); ++ send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, false); } void setVtxTableBand(int serial_fd, uint8_t band) { -@@ -119,7 +138,7 @@ void setVtxTableBand(int serial_fd, uint8_t band) { +@@ -119,7 +163,7 @@ void setVtxTableBand(int serial_fd, uint8_t band) { } construct_msp_command(buffer, MSP_SET_VTXTABLE_BAND, payload2, sizeof(payload2), MSP_OUTBOUND); @@ -219,7 +252,7 @@ index b1a538c..a527dff 100644 } void setVtxTablePowerLevel(int serial_fd, uint8_t idx) { -@@ -139,31 +158,24 @@ void setVtxTablePowerLevel(int serial_fd, uint8_t idx) { +@@ -139,31 +183,25 @@ void setVtxTablePowerLevel(int serial_fd, uint8_t idx) { construct_msp_command( buffer, MSP_SET_VTXTABLE_POWERLEVEL, payload2, sizeof(payload2), MSP_OUTBOUND); @@ -231,12 +264,13 @@ index b1a538c..a527dff 100644 + int band; + int channel; + int configured_frequency; ++ uint8_t power_index; - // mspVTX read current frequency setting - query_interface_for_available_frequencies(); - - frequency = read_current_freq_from_interface(read_setting("/etc/wfb.conf", "wlan")); -+ if (get_wfb_vtx_config(&band, &channel, &configured_frequency) != 0) { ++ if (get_wfb_vtx_config(&band, &channel, &configured_frequency, &power_index) != 0) { + return; + } @@ -258,11 +292,11 @@ index b1a538c..a527dff 100644 } - wipeVtxTable(serial_fd, band, channel); -+ wipeVtxTable(serial_fd, band, channel, configured_frequency); ++ wipeVtxTable(serial_fd, band, channel, configured_frequency, power_index); for (int i = 1; i <= BAND_COUNT; i++) { setVtxTableBand(serial_fd, i); -@@ -174,6 +186,18 @@ void msp_set_vtx_config(int serial_fd) { +@@ -174,6 +212,19 @@ void msp_set_vtx_config(int serial_fd) { vtxInitDone = true; } @@ -270,12 +304,13 @@ index b1a538c..a527dff 100644 + int band; + int channel; + int configured_frequency; ++ uint8_t power_index; + -+ if (get_wfb_vtx_config(&band, &channel, &configured_frequency) != 0) { ++ if (get_wfb_vtx_config(&band, &channel, &configured_frequency, &power_index) != 0) { + return; + } + -+ send_vtx_config(serial_fd, band, channel, configured_frequency, false); ++ send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, false); +} + msp_error_e construct_msp_command(uint8_t message_buffer[], uint8_t command, uint8_t payload[], @@ -306,10 +341,10 @@ index 3e60a31..1fe1c28 100644 +// 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..49d60fc 100644 +index 089fdc2..1ddd350 100644 --- a/osd/util/interface.c +++ b/osd/util/interface.c -@@ -153,3 +153,41 @@ void set_frequency(char *interface, int channel) { +@@ -153,3 +153,69 @@ void set_frequency(char *interface, int channel) { printf("Failed to set channel on %s\n", interface); } } @@ -351,11 +386,39 @@ index 089fdc2..49d60fc 100644 + + return -1; +} ++ ++int read_wfb_txpower(void) { ++ FILE *fp = fopen("/etc/wfb.yaml", "r"); ++ char buffer[256]; ++ int in_wireless = 0; ++ int txpower = -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, " 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..134b0d5 100644 +index 983971d..854c9e7 100644 --- a/osd/util/interface.h +++ b/osd/util/interface.h -@@ -22,3 +22,7 @@ int query_interface_for_available_frequencies(); +@@ -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); @@ -363,3 +426,6 @@ index 983971d..134b0d5 100644 +// 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);