diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index a17914031c1ca..4471c6ca4180e 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -416,15 +416,29 @@ void AP_Periph_FW::update() node_stats.update(); #endif - static uint32_t last_led_ms; uint32_t now = AP_HAL::millis(); - if (now - last_led_ms > 1000) { - last_led_ms = now; + #ifdef HAL_GPIO_PIN_LED + static uint32_t last_led_ms; + static uint32_t next_led_ms = 1000; + if (now - last_led_ms > next_led_ms) { + last_led_ms = now; if (!no_iface_finished_dna) { + static uint8_t led_phase = 0; + if (debug_option_is_set(DebugOptions::FIND_NODE)) { + next_led_ms = (led_phase < 10) ? 100 : 500; + led_phase = (led_phase + 1) % 12; + } else { + next_led_ms = 1000; + } palToggleLine(HAL_GPIO_PIN_LED); } + } #endif + + static uint32_t last_1_s; + if (now - last_1_s > 1000) { + last_1_s = now; #if 0 #if AP_PERIPH_GPS_ENABLED hal.serial(0)->printf("GPS status: %u\n", (unsigned)gps.status()); diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index 482744263caf6..87840977449b4 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -472,6 +472,7 @@ class AP_Periph_FW { SHOW_STACK = 0, AUTOREBOOT = 1, ENABLE_STATS = 2, + FIND_NODE = 3, }; // check if an option is set diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index bd0a72be53cc1..77424ea62343a 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -201,7 +201,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { // @Param: DEBUG // @DisplayName: Debug // @Description: Debug - // @Bitmask: 0:Show free stack space, 1:Auto Reboot after 15sec, 2:Enable sending stats + // @Bitmask: 0:Show free stack space, 1:Auto Reboot after 15sec, 2:Enable sending stats, 3: Locate Node by LED flash pattern // @User: Advanced GSCALAR(debug, "DEBUG", 0),