hilink: report WFB txpower on 0 to 100 VTX scale
This commit is contained in:
@@ -80,7 +80,7 @@ index a8b756e..7e5ca2a 100644
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
diff --git a/osd/msp/msp.c b/osd/msp/msp.c
|
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
|
--- a/osd/msp/msp.c
|
||||||
+++ b/osd/msp/msp.c
|
+++ b/osd/msp/msp.c
|
||||||
@@ -5,6 +5,7 @@
|
@@ -5,6 +5,7 @@
|
||||||
@@ -91,15 +91,24 @@ index b1a538c..a527dff 100644
|
|||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
|
||||||
extern bool verbose;
|
extern bool verbose;
|
||||||
@@ -38,58 +39,76 @@ uint8_t saPowerLevelsLut[NUM_POWER_LEVELS] = {20, 25, 30, 35, 45, 50, 55, 58};
|
@@ -33,12 +34,11 @@ uint16_t channelFreqTable[FREQ_TABLE_SIZE] = {
|
||||||
uint8_t saPowerLevelsLabel[NUM_POWER_LEVELS * POWER_LEVEL_LABEL_LENGTH] = {'2', '0', ' ', '2', '5',
|
5925 // WLAN CHANNEL: 132, 140, 144, 153, 161, 169, 177, 185
|
||||||
' ', '3', '0', ' ', '3', '5', ' ', '4', '5', ' ', '5', '0', ' ', '5', '5', ' ', '5', '8', ' '};
|
};
|
||||||
|
|
||||||
-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) {
|
uint16_t msp_data_from_msg(uint8_t message_buffer[], msp_msg_t *msg) {
|
||||||
// return size
|
// 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;
|
return msg->size + 6;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -156,13 +165,36 @@ index b1a538c..a527dff 100644
|
|||||||
- payload[12] = CHANNEL_COUNT; // newChannelCount
|
- payload[12] = CHANNEL_COUNT; // newChannelCount
|
||||||
- payload[13] = NUM_POWER_LEVELS; // newPowerCount
|
- payload[13] = NUM_POWER_LEVELS; // newPowerCount
|
||||||
- payload[14] = 0; // vtxtable should be cleared
|
- 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();
|
+ *configured_frequency = read_wfb_frequency();
|
||||||
+ if (*configured_frequency < 0) {
|
+ if (*configured_frequency < 0) {
|
||||||
+ fprintf(stderr, "mspVTX: cannot read /etc/wfb.yaml channel\n");
|
+ fprintf(stderr, "mspVTX: cannot read /etc/wfb.yaml channel\n");
|
||||||
+ return -1;
|
+ 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;
|
+ *band = 1;
|
||||||
+ *channel = 1;
|
+ *channel = 1;
|
||||||
+ for (int index = 0; index < FREQ_TABLE_SIZE; index++) {
|
+ 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,
|
+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] = {
|
+ uint8_t payload[15] = {
|
||||||
+ configured_frequency & 0xFF,
|
+ configured_frequency & 0xFF,
|
||||||
+ (configured_frequency >> 8) & 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, // pit mode
|
||||||
+ 0, // low power when disarmed
|
+ 0, // low power when disarmed
|
||||||
+ 0, 0, // pit frequency
|
+ 0, 0, // pit frequency
|
||||||
@@ -202,15 +234,16 @@ index b1a538c..a527dff 100644
|
|||||||
+ write_msp_packet(serial_fd, buffer, sizeof(payload) + 6);
|
+ 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
|
+ // First tell Betaflight to discard any stale table, then publish the
|
||||||
+ // configured WFB frequency and the table dimensions.
|
+ // 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, power_index, true);
|
||||||
+ send_vtx_config(serial_fd, band, channel, configured_frequency, false);
|
+ send_vtx_config(serial_fd, band, channel, configured_frequency, power_index, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
void setVtxTableBand(int serial_fd, uint8_t band) {
|
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);
|
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) {
|
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(
|
construct_msp_command(
|
||||||
buffer, MSP_SET_VTXTABLE_POWERLEVEL, payload2, sizeof(payload2), MSP_OUTBOUND);
|
buffer, MSP_SET_VTXTABLE_POWERLEVEL, payload2, sizeof(payload2), MSP_OUTBOUND);
|
||||||
@@ -231,12 +264,13 @@ index b1a538c..a527dff 100644
|
|||||||
+ int band;
|
+ int band;
|
||||||
+ int channel;
|
+ int channel;
|
||||||
+ int configured_frequency;
|
+ int configured_frequency;
|
||||||
|
+ uint8_t power_index;
|
||||||
|
|
||||||
- // mspVTX read current frequency setting
|
- // mspVTX read current frequency setting
|
||||||
- query_interface_for_available_frequencies();
|
- query_interface_for_available_frequencies();
|
||||||
-
|
-
|
||||||
- frequency = read_current_freq_from_interface(read_setting("/etc/wfb.conf", "wlan"));
|
- 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;
|
+ return;
|
||||||
+ }
|
+ }
|
||||||
|
|
||||||
@@ -258,11 +292,11 @@ index b1a538c..a527dff 100644
|
|||||||
}
|
}
|
||||||
|
|
||||||
- wipeVtxTable(serial_fd, band, channel);
|
- 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++) {
|
for (int i = 1; i <= BAND_COUNT; i++) {
|
||||||
setVtxTableBand(serial_fd, 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;
|
vtxInitDone = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -270,12 +304,13 @@ index b1a538c..a527dff 100644
|
|||||||
+ int band;
|
+ int band;
|
||||||
+ int channel;
|
+ int channel;
|
||||||
+ int configured_frequency;
|
+ 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;
|
+ 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[],
|
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.
|
+// Advertise the actual local WFB configuration without resetting the FC table.
|
||||||
+void msp_send_vtx_runtime_config(int serial_fd);
|
+void msp_send_vtx_runtime_config(int serial_fd);
|
||||||
diff --git a/osd/util/interface.c b/osd/util/interface.c
|
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
|
--- a/osd/util/interface.c
|
||||||
+++ b/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);
|
printf("Failed to set channel on %s\n", interface);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -351,11 +386,39 @@ index 089fdc2..49d60fc 100644
|
|||||||
+
|
+
|
||||||
+ return -1;
|
+ 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
|
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
|
--- a/osd/util/interface.h
|
||||||
+++ b/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);
|
void set_frequency(char *interface, int channel);
|
||||||
|
|
||||||
int read_current_freq_from_interface(char *interface);
|
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.
|
+// Read WiFiLink/OpenIPC WFB channel from the YAML runtime configuration.
|
||||||
+// Returns the centre frequency in MHz, or -1 when no valid channel is present.
|
+// Returns the centre frequency in MHz, or -1 when no valid channel is present.
|
||||||
+int read_wfb_frequency(void);
|
+int read_wfb_frequency(void);
|
||||||
|
+
|
||||||
|
+// Read the configured WFB txpower scalar (0..100 convention used by this profile).
|
||||||
|
+int read_wfb_txpower(void);
|
||||||
|
|||||||
Reference in New Issue
Block a user