From 6821349f4f16abc3e632afae74a399e811a645c3 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Thu, 14 Mar 2024 10:49:35 -0700
Subject: [PATCH] slow-down dronecan msgs. add debug when rate = -1

---
 Tools/AP_Periph/AP_Periph.cpp                 | 74 ++++++++++++-------
 Tools/AP_Periph/AP_Periph.h                   |  2 +
 Tools/AP_Periph/Parameters.cpp                |  2 +-
 libraries/AP_ADC/AP_ADC_ADS1115.cpp           |  2 +
 libraries/AP_ADC/AP_ADC_ADS1115.h             |  2 +
 .../hwdef/CubeOrange-periph/hwdef.dat         |  1 +
 6 files changed, 56 insertions(+), 27 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 5fa62dc071bd33..32be57bb3e2366 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -503,38 +503,60 @@ void AP_Periph_FW::update()
         adc.last_update_ms = now;
         IGNORE_RETURN(adc.lib.read(adc.samples, ARRAY_SIZE(adc.samples)));
     }
-    if (g.adc_send_rate > 0 && now - adc.last_send_ms >= g.adc_send_rate) {
-        adc.last_send_ms = now;
 
-        ardupilot_equipment_power_BatteryCells pkt1 {};
-        pkt1.voltages.len = MIN(ARRAY_SIZE(adc.samples),ARRAY_SIZE(pkt1.voltages.data));
-        for (uint8_t i=0; i<pkt1.voltages.len; i++) {
-            pkt1.voltages.data[i] = adc.samples[i].data;
-        }
+    if (g.adc_send_rate >= 1 || g.adc_send_rate == -1) {
+        const uint32_t last_sample_timestamp_ms = adc.lib.get_last_sample_timestamp_ms();
+        const bool new_sample_available = (adc.last_sample_timestamp_ms != last_sample_timestamp_ms) && adc.last_sample_timestamp_ms != 0;
+
+        if (g.adc_send_rate == -1) {
+            if (now - adc.last_debug_ms >= 1000) {
+                adc.last_debug_ms = now;
+                adc.last_sample_timestamp_ms = last_sample_timestamp_ms;
+
+                can_printf("ADC%s %.2f, %.2f, %.2f, %.2f, %.2f, %.2f",
+                    new_sample_available ? "" : " STALE",
+                    adc.samples[0].data,
+                    adc.samples[1].data,
+                    adc.samples[2].data,
+                    adc.samples[3].data,
+                    adc.samples[4].data,
+                    adc.samples[5].data);
+            }
 
-        uint8_t buffer1[ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_MAX_SIZE] {};
-        uint16_t total_size1 = ardupilot_equipment_power_BatteryCells_encode(&pkt1, buffer1, !periph.canfdout());
-        canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_SIGNATURE,
-                        ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_ID,
-                        CANARD_TRANSFER_PRIORITY_LOW,
-                        &buffer1[0],
-                        total_size1);
+        } else if (new_sample_available && now - adc.last_send_ms >= g.adc_send_rate) {
+            adc.last_sample_timestamp_ms = last_sample_timestamp_ms;
+            adc.last_send_ms = now;
 
+            ardupilot_equipment_power_BatteryCells pkt1 {};
+            pkt1.voltages.len = MIN(ARRAY_SIZE(adc.samples),ARRAY_SIZE(pkt1.voltages.data));
+            for (uint8_t i=0; i<pkt1.voltages.len; i++) {
+                pkt1.voltages.data[i] = adc.samples[i].data;
+            }
 
+            uint8_t buffer1[ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_MAX_SIZE] {};
+            uint16_t total_size1 = ardupilot_equipment_power_BatteryCells_encode(&pkt1, buffer1, !periph.canfdout());
+            canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_SIGNATURE,
+                            ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_ID,
+                            CANARD_TRANSFER_PRIORITY_LOW,
+                            &buffer1[0],
+                            total_size1);
 
-        rb_ADC pkt2 {};
-        pkt2.voltages.len = MIN(ARRAY_SIZE(adc.samples),ARRAY_SIZE(pkt2.voltages.data));
-        for (uint8_t i=0; i<pkt2.voltages.len; i++) {
-            pkt2.voltages.data[i] = adc.samples[i].data;
-        }
 
-        uint8_t buffer2[RB_ADC_MAX_SIZE] {};
-        uint16_t total_size2 = rb_ADC_encode(&pkt2, buffer2, !periph.canfdout());
-        canard_broadcast(RB_ADC_SIGNATURE,
-                        RB_ADC_ID,
-                        CANARD_TRANSFER_PRIORITY_LOW,
-                        &buffer2[0],
-                        total_size2);
+
+            rb_ADC pkt2 {};
+            pkt2.voltages.len = MIN(ARRAY_SIZE(adc.samples),ARRAY_SIZE(pkt2.voltages.data));
+            for (uint8_t i=0; i<pkt2.voltages.len; i++) {
+                pkt2.voltages.data[i] = adc.samples[i].data;
+            }
+
+            uint8_t buffer2[RB_ADC_MAX_SIZE] {};
+            uint16_t total_size2 = rb_ADC_encode(&pkt2, buffer2, !periph.canfdout());
+            canard_broadcast(RB_ADC_SIGNATURE,
+                            RB_ADC_ID,
+                            CANARD_TRANSFER_PRIORITY_LOW,
+                            &buffer2[0],
+                            total_size2);
+        }
     }
 #endif
 
diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index ba108887588349..a3b7205a623f03 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -237,6 +237,8 @@ class AP_Periph_FW {
         adc_report_s samples[ADS1115_CHANNELS_COUNT];
         uint32_t last_update_ms;
         uint32_t last_send_ms;
+        uint32_t last_sample_timestamp_ms;
+        uint32_t last_debug_ms;
     } adc;
 #endif
 
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 532e40ecc14fb7..ec48dd7606614d 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -517,7 +517,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Range: 0 2000
     // @Increment: 1
     // @Units: ms
-    GSCALAR(adc_send_rate, "ADC_RATE", 1),
+    GSCALAR(adc_send_rate, "ADC_RATE", 1000),
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_PROXIMITY
diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp
index 09ffbbcd340ec8..96b47b6872ef10 100644
--- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp
+++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp
@@ -237,5 +237,7 @@ void AP_ADC_ADS1115::_update()
     /* select next channel */
     _channel_to_read = (_channel_to_read + 1) % _channels_number;
     _start_conversion(_channel_to_read);
+    
+    _last_sample_timestamp_ms = AP_HAL::millis();
 }
 #endif // AP_ADC_ADS1115_ENABLED
diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.h b/libraries/AP_ADC/AP_ADC_ADS1115.h
index 39137235174789..d04f0273e2cf7e 100644
--- a/libraries/AP_ADC/AP_ADC_ADS1115.h
+++ b/libraries/AP_ADC/AP_ADC_ADS1115.h
@@ -25,6 +25,7 @@ class AP_ADC_ADS1115
 
     bool init();
     size_t read(adc_report_s *report, size_t length) const;
+    uint32_t get_last_sample_timestamp_ms() const { return _last_sample_timestamp_ms;}
 
     uint8_t get_channels_number() const
     {
@@ -39,6 +40,7 @@ class AP_ADC_ADS1115
     uint16_t            _gain;
     int                 _channel_to_read;
     adc_report_s        *_samples;
+    uint32_t            _last_sample_timestamp_ms;
 
     void _update();
     bool _start_conversion(uint8_t channel);
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat
index 836813045eb951..80a3c45a9b3c93 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubeOrange-periph/hwdef.dat
@@ -62,3 +62,4 @@ define HAL_CAN_DRIVER_DEFAULT 1
 define HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT 0
  
 define AP_ADC_ADS1115_ENABLED 1
+define ADS1115_I2C_BUS 0