diff --git a/RemoteIDModule/BLE_TX.cpp b/RemoteIDModule/BLE_TX.cpp index 97a869e..a89458e 100644 --- a/RemoteIDModule/BLE_TX.cpp +++ b/RemoteIDModule/BLE_TX.cpp @@ -91,7 +91,7 @@ bool BLE_TX::init(void) if (initialised) { return true; } - initialised = true; + BLEDevice::init(""); // setup power levels @@ -111,20 +111,22 @@ bool BLE_TX::init(void) // set as a bluetooth random static address mac_addr[0] |= 0xc0; - advert.setAdvertisingParams(0, &legacy_adv_params); - advert.setInstanceAddress(0, mac_addr); + if (!advert.setAdvertisingParams(0, &legacy_adv_params)) { return false; } + if (!advert.setInstanceAddress(0, mac_addr)) { return false; } advert.setDuration(0); - advert.setAdvertisingParams(1, &ext_adv_params_coded); + if (!advert.setAdvertisingParams(1, &ext_adv_params_coded)) { return false; } advert.setDuration(1); - advert.setInstanceAddress(1, mac_addr); + if (!advert.setInstanceAddress(1, mac_addr)) { return false; } // prefer S8 coding if (esp_ble_gap_set_prefered_default_phy(ESP_BLE_GAP_PHY_OPTIONS_PREF_S8_CODING, ESP_BLE_GAP_PHY_OPTIONS_PREF_S8_CODING) != ESP_OK) { Serial.printf("Failed to setup S8 coding\n"); + return false; } memset(&msg_counters,0, sizeof(msg_counters)); + initialised = true; return true; } @@ -132,7 +134,10 @@ bool BLE_TX::init(void) bool BLE_TX::transmit_longrange(ODID_UAS_Data &UAS_data) { - init(); + if (!init()) { + return false; + } + // create a packed UAS data message uint8_t payload[250]; int length = odid_message_build_pack(&UAS_data, payload, 255); @@ -148,11 +153,15 @@ bool BLE_TX::transmit_longrange(ODID_UAS_Data &UAS_data) memcpy(&longrange_payload[sizeof(header)], payload, length); int longrange_length = sizeof(header) + length; - advert.setAdvertisingData(1, longrange_length, longrange_payload); + if (!advert.setAdvertisingData(1, longrange_length, longrange_payload)) { + return false; + } // we start advertising when we have the first lot of data to send if (!started) { - advert.start(); + if (!advert.start()) { + return false; + } } started = true; @@ -161,7 +170,10 @@ bool BLE_TX::transmit_longrange(ODID_UAS_Data &UAS_data) bool BLE_TX::transmit_legacy(ODID_UAS_Data &UAS_data) { - init(); + if (!init()) { + return false; + } + static uint8_t legacy_phase = 0; int legacy_length = 0; // setup ASTM header @@ -307,10 +319,14 @@ bool BLE_TX::transmit_legacy(ODID_UAS_Data &UAS_data) legacy_phase %= 6; } - advert.setAdvertisingData(0, legacy_length, legacy_payload); + if (!advert.setAdvertisingData(0, legacy_length, legacy_payload)) { + return false; + } if (!started) { - advert.start(); + if (!advert.start()) { + return false; + } } started = true; diff --git a/RemoteIDModule/RemoteIDModule.ino b/RemoteIDModule/RemoteIDModule.ino index fd3bcd7..8fc9fb1 100644 --- a/RemoteIDModule/RemoteIDModule.ino +++ b/RemoteIDModule/RemoteIDModule.ino @@ -326,6 +326,7 @@ static void set_data(Transport &t) const char *reason = check_parse(); t.arm_status_check(reason); + if (reason == nullptr && !pfst_check_ok) { reason = "pfst failed"; } t.set_parse_fail(reason); arm_check_ok = (reason==nullptr); @@ -334,7 +335,7 @@ static void set_data(Transport &t) arm_check_ok = true; } - led.set_state(pfst_check_ok && arm_check_ok? Led::LedState::ARM_OK : Led::LedState::ARM_FAIL); + led.set_state(arm_check_ok? Led::LedState::ARM_OK : Led::LedState::ARM_FAIL); uint32_t now_ms = millis(); uint32_t location_age_ms = now_ms - t.get_last_location_ms(); @@ -416,30 +417,46 @@ void loop() static uint32_t last_update_wifi_nan_ms; if (g.wifi_nan_rate > 0 && now_ms - last_update_wifi_nan_ms > 1000/g.wifi_nan_rate) { - last_update_wifi_nan_ms = now_ms; - wifi.transmit_nan(UAS_data); + if (wifi.transmit_nan(UAS_data)) { + last_update_wifi_nan_ms = now_ms; + } } static uint32_t last_update_wifi_beacon_ms; if (g.wifi_beacon_rate > 0 && now_ms - last_update_wifi_beacon_ms > 1000/g.wifi_beacon_rate) { - last_update_wifi_beacon_ms = now_ms; - wifi.transmit_beacon(UAS_data); + if(wifi.transmit_beacon(UAS_data)) { + last_update_wifi_beacon_ms = now_ms; + } } static uint32_t last_update_bt5_ms; if (g.bt5_rate > 0 && now_ms - last_update_bt5_ms > 1000/g.bt5_rate) { - last_update_bt5_ms = now_ms; - ble.transmit_longrange(UAS_data); + if(ble.transmit_longrange(UAS_data)) { + last_update_bt5_ms = now_ms; + } } static uint32_t last_update_bt4_ms; int bt4_states = UAS_data.BasicIDValid[1] ? 7 : 6; if (g.bt4_rate > 0 && now_ms - last_update_bt4_ms > (1000.0f/bt4_states)/g.bt4_rate) { - last_update_bt4_ms = now_ms; - ble.transmit_legacy(UAS_data); + if(ble.transmit_legacy(UAS_data)) { + last_update_bt4_ms = now_ms; + } + } + + if (now_ms - last_update_wifi_nan_ms > 5000.0 && + now_ms - last_update_wifi_beacon_ms > 5000.0 && + (now_ms - last_update_bt5_ms > 5000.0 || + now_ms - last_update_bt4_ms > 5000.0)) { + // transmitter failure + pfst_check_ok = false; + + } else { + // transmitter is healthy if wifi nan or wifi beacon or both Bluetooth transmissions work successfully + pfst_check_ok = true; } // sleep for a bit for power saving diff --git a/RemoteIDModule/WiFi_TX.cpp b/RemoteIDModule/WiFi_TX.cpp index 6cf06db..c173d2b 100644 --- a/RemoteIDModule/WiFi_TX.cpp +++ b/RemoteIDModule/WiFi_TX.cpp @@ -15,7 +15,6 @@ bool WiFi_TX::init(void) if (initialised) { return true; } - initialised = true; //use a local MAC address to avoid tracking transponders based on their MAC address uint8_t mac_addr[6]; @@ -25,12 +24,18 @@ bool WiFi_TX::init(void) mac_addr[0] &= 0xFE; // unset MAC multicast bit //set MAC address - esp_base_mac_addr_set(mac_addr); + if (esp_base_mac_addr_set(mac_addr) != ESP_OK) { + return false; + } if (g.webserver_enable == 0) { - WiFi.softAP(g.wifi_ssid, g.wifi_password, g.wifi_channel, false, 0); //make it visible and allow no connection + if (!WiFi.softAP(g.wifi_ssid, g.wifi_password, g.wifi_channel, false, 0) /* make it visible and allow no connection */ ) { + return false; + } } else { - WiFi.softAP(g.wifi_ssid, g.wifi_password, g.wifi_channel, false, 1); //make it visible and allow only 1 connection + if (!WiFi.softAP(g.wifi_ssid, g.wifi_password, g.wifi_channel, false, 1) /* make it visible and allow only 1 connection */ ) { + return false; + } } if (esp_wifi_set_bandwidth(WIFI_IF_AP, WIFI_BW_HT20) != ESP_OK) { @@ -39,14 +44,20 @@ bool WiFi_TX::init(void) memcpy(WiFi_mac_addr,mac_addr,6); //use generated random MAC address for OpenDroneID messages - esp_wifi_set_max_tx_power(dBm_to_tx_power(g.wifi_power)); + if (esp_wifi_set_max_tx_power(dBm_to_tx_power(g.wifi_power)) != ESP_OK) { + return false; + } + + initialised = true; return true; } bool WiFi_TX::transmit_nan(ODID_UAS_Data &UAS_data) { - init(); + if (!init()) { + return false; + } uint8_t buffer[1024] {}; @@ -72,7 +83,9 @@ bool WiFi_TX::transmit_nan(ODID_UAS_Data &UAS_data) //update the payload of the beacon frames in this function bool WiFi_TX::transmit_beacon(ODID_UAS_Data &UAS_data) { - init(); + if (!init()) { + return false; + } uint8_t buffer[1024] {};