hilink: synchronize live WFB state over MSP VTX
This commit is contained in:
@@ -1,52 +1,28 @@
|
||||
diff --git a/osd.c b/osd.c
|
||||
index a8b756e..ac9adf5 100644
|
||||
index a8b756e..7e5ca2a 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) {
|
||||
@@ -959,48 +959,37 @@ 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: {
|
||||
- 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;
|
||||
- mspVtxConfigStruct *in_mspVtxConfigStruct = (mspVtxConfigStruct *)msp_message->payload;
|
||||
- uint16_t frequency =
|
||||
+ uint16_t requested_frequency =
|
||||
(in_mspVtxConfigStruct->freqMSB << 8) | in_mspVtxConfigStruct->freqLSB;
|
||||
- (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",
|
||||
@@ -77,40 +53,142 @@ index a8b756e..ac9adf5 100644
|
||||
- "vtx table in your fc\n");
|
||||
- }
|
||||
- }
|
||||
- } else {
|
||||
+ 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");
|
||||
+ printf("mspVTX read-only: FC requested %u MHz, configured WFB frequency %d MHz\n",
|
||||
+ requested_frequency, read_wfb_frequency());
|
||||
+ 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());
|
||||
}
|
||||
+ // 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
|
||||
index b1a538c..26171aa 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];
|
||||
@@ -38,58 +38,62 @@ 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', ' '};
|
||||
|
||||
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);
|
||||
-int frequency = 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);
|
||||
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 int get_wfb_vtx_config(int *band, int *channel, int *configured_frequency) {
|
||||
+ *configured_frequency = read_wfb_frequency();
|
||||
+ if (*configured_frequency < 0) {
|
||||
+ fprintf(stderr, "mspVTX: cannot read /etc/wfb.yaml channel\n");
|
||||
+ return -1;
|
||||
+ }
|
||||
|
||||
// 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
|
||||
- uint8_t buffer[256];
|
||||
+ *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;
|
||||
+ }
|
||||
+ }
|
||||
|
||||
- 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
|
||||
+ 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,
|
||||
+ 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
|
||||
+ 0, // 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(serial_fd, buffer, sizeof(payload) + 6);
|
||||
+}
|
||||
+
|
||||
+static void wipeVtxTable(int serial_fd, int band, int channel, int configured_frequency) {
|
||||
+ // 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);
|
||||
}
|
||||
|
||||
void setVtxTableBand(int serial_fd, uint8_t band) {
|
||||
@@ -119,7 +119,7 @@ void setVtxTableBand(int serial_fd, uint8_t band) {
|
||||
@@ -119,7 +123,7 @@ void setVtxTableBand(int serial_fd, uint8_t band) {
|
||||
}
|
||||
|
||||
construct_msp_command(buffer, MSP_SET_VTXTABLE_BAND, payload2, sizeof(payload2), MSP_OUTBOUND);
|
||||
@@ -119,7 +197,7 @@ index b1a538c..bb8c44e 100644
|
||||
}
|
||||
|
||||
void setVtxTablePowerLevel(int serial_fd, uint8_t idx) {
|
||||
@@ -139,27 +139,31 @@ void setVtxTablePowerLevel(int serial_fd, uint8_t idx) {
|
||||
@@ -139,31 +143,24 @@ void setVtxTablePowerLevel(int serial_fd, uint8_t idx) {
|
||||
|
||||
construct_msp_command(
|
||||
buffer, MSP_SET_VTXTABLE_POWERLEVEL, payload2, sizeof(payload2), MSP_OUTBOUND);
|
||||
@@ -128,39 +206,61 @@ index b1a538c..bb8c44e 100644
|
||||
}
|
||||
|
||||
void msp_set_vtx_config(int serial_fd) {
|
||||
+ int band;
|
||||
+ int channel;
|
||||
+ int configured_frequency;
|
||||
|
||||
- // 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");
|
||||
+ if (get_wfb_vtx_config(&band, &channel, &configured_frequency) != 0) {
|
||||
+ 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
|
||||
- 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);
|
||||
-
|
||||
- if (verbose)
|
||||
- printf("Frequency %d found in band %i, channel %d\n", frequency, band, channel);
|
||||
- break; // Exit after finding the frequency
|
||||
+ break;
|
||||
}
|
||||
- }
|
||||
+ 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);
|
||||
|
||||
for (int i = 1; i <= BAND_COUNT; i++) {
|
||||
setVtxTableBand(serial_fd, i);
|
||||
@@ -174,6 +171,18 @@ 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;
|
||||
+
|
||||
+ if (get_wfb_vtx_config(&band, &channel, &configured_frequency) != 0) {
|
||||
+ return;
|
||||
+ }
|
||||
+
|
||||
+ send_vtx_config(serial_fd, band, channel, configured_frequency, 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..772aaed 100644
|
||||
index 3e60a31..1fe1c28 100644
|
||||
--- a/osd/msp/msp.h
|
||||
+++ b/osd/msp/msp.h
|
||||
@@ -40,10 +40,10 @@
|
||||
@@ -177,6 +277,12 @@ index 3e60a31..772aaed 100644
|
||||
|
||||
#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..49d60fc 100644
|
||||
--- a/osd/util/interface.c
|
||||
|
||||
Reference in New Issue
Block a user