AP_Periph: add position locate function

mirkix · Dec 31, 2023 · a39c1a9 · a39c1a9
1 parent 687c588
commit a39c1a9
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 0 deletions.
diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
@@ -391,13 +391,24 @@ void AP_Periph_FW::update()
 
  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;
+ if (now - last_led_ms > (g.locate != 0 ? 50 : 1000)) {
+ last_led_ms = now;
  if (!no_iface_finished_dna) {
  palToggleLine(HAL_GPIO_PIN_LED);
  }
+ }
 #endif
+
+ static uint32_t last_print_ms;
+ if (now - last_print_ms > 1000) {
+ last_print_ms = now;
 #if 0
 #ifdef HAL_PERIPH_ENABLE_GPS
  hal.serial(0)->printf("GPS status: %u\n", (unsigned)gps.status());

diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
@@ -209,6 +209,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
  // @User: Advanced
  GSCALAR(serial_number, "BRD_SERIAL_NUM", 0),
 
+#ifdef HAL_GPIO_PIN_LED
+ // @Param: LOCATE
+ // @DisplayName: Locate
+ // @Description: Enable LED flashing to locate node
+ // @Bitmask: 0:Disabled, 1:Enable
+ // @User: Advanced
+ GSCALAR(locate, "LOCATE", 0),
+#endif
+
 #ifdef HAL_PERIPH_ENABLE_BUZZER_WITHOUT_NOTIFY
  // @Param: BUZZER_VOLUME
  // @DisplayName: Buzzer volume

diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
@@ -89,6 +89,7 @@ class Parameters {
  k_param_can_terminate1,
  k_param_can_terminate2,
  k_param_serial_options,
+ k_param_locate,
  };
 
  AP_Int16 format_version;
@@ -210,6 +211,10 @@ class Parameters {
  static constexpr uint8_t can_fdmode = 0;
 #endif
 
+#ifdef HAL_GPIO_PIN_LED
+ AP_Int8 locate;
+#endif
+
  AP_Int8 can_terminate[HAL_NUM_CAN_IFACES];
 
  AP_Int8 node_stats;