From 2ee0e5d390d4ec207254ed27a830b9ba3ed85ec3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 7 Feb 2024 18:47:53 +1030 Subject: [PATCH] Squash 1 --- Tools/AP_Periph/FSOPowerStack.cpp | 31 ++++++++++++++++++++----------- Tools/AP_Periph/FSOPowerStack.h | 3 +-- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/Tools/AP_Periph/FSOPowerStack.cpp b/Tools/AP_Periph/FSOPowerStack.cpp index 7c009c55981113..2665d5cfc8e32c 100644 --- a/Tools/AP_Periph/FSOPowerStack.cpp +++ b/Tools/AP_Periph/FSOPowerStack.cpp @@ -190,15 +190,9 @@ void FSOPowerStack::update_fans(void) if (fans[3].freq_hz < fan_4_min_Hz){ GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Fan 4 failure"); } - - if (now_ms - last_fan_report_ms > 5000) { - last_fan_report_ms = now_ms; - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fans: %.1f %.1f %.1f %.1f", - fans[0].freq_hz, fans[1].freq_hz, fans[2].freq_hz, fans[3].freq_hz); - } } -void FSOPowerStack::report_voltages(void) +void FSOPowerStack::report(void) { uint32_t now_ms = AP_HAL::millis(); if (now_ms - last_report_ms < 5000) { @@ -207,14 +201,29 @@ void FSOPowerStack::report_voltages(void) last_report_ms = now_ms; auto &batt = AP::battery(); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Voltages: 1: %.2f, 2: %.2f, 3: %.2f, 4: %.2f, 5: %.2f, 6: %.2f, 7: %.2f, 8:%.2f, 9: %.2f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Volt - B1:%.1f, B2:%.1f, PHV:%.1f, P1:%.2f, P2:%.2f, IHC:%.2f, I1:%.2f, I2:%.2f, O:%.2f", batt.voltage(0), batt.voltage(1), batt.voltage(2), batt.voltage(3), batt.voltage(4), batt.voltage(5), batt.voltage(6), batt.voltage(7), batt.voltage(8)); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Current Limits: PHV: %.2f, P1: %.2f, P2: %.2f, IHC: %.2f, I1: %.2f, I2: %.2f", + + float B1_C; + float B2_C; + float PHV_C; + float P1_C; + float P2_C; + float IHC_C; + float I1_C; + float I2_C; + if (batt.current_amps(B1_C, 0) || batt.current_amps(B2_C, 1) || batt.current_amps(PHV_C, 2) || batt.current_amps(P1_C, 3) || batt.current_amps(P2_C, 4) || batt.current_amps(IHC_C, 5) || batt.current_amps(I1_C, 6) || batt.current_amps(I2_C, 7)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Current - B1:%.4f, B2:%.4f, PHV:%.2f, P1:%.2f, P2:%.2f, IHC:%.2f, I1:%.2f, I2:%.2f", + B1_C, B2_C, PHV_C, P1_C, P2_C, IHC_C, I1_C, I2_C); + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Current Limit - PHV:%.2f, P1:%.2f, P2:%.2f, IHC:%.2f, I1:%.2f, I2:%.2f", payload_HV_current_filter.get(), payload_1_current_filter.get(), payload_2_current_filter.get(), internal_HC_current_filter.get(), internal_1_current_filter.get(), internal_2_current_filter.get()); - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Temperature Limits: P1: %.2f, P2: %.2f, IHC: %.2f, I1: %.2f, I2: %.2f", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Temp Limit - P1:%.1f, P2:%.1f, IHC:%.1f, I1:%.1f, I2:%.1f", payload_1_temp, payload_2_temp, internal_HC_temp, internal_1_temp, internal_2_temp); + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Fans - F1: %.0f, F2: %.0f, F3: %.0f, F4: %.0f", + fans[0].freq_hz, fans[1].freq_hz, fans[2].freq_hz, fans[3].freq_hz); } @@ -508,7 +517,7 @@ void FSOPowerStack::update() update_fans(); - report_voltages(); + report(); update_main_power(); diff --git a/Tools/AP_Periph/FSOPowerStack.h b/Tools/AP_Periph/FSOPowerStack.h index 4666be9dc5e2ea..ef7cbc7335d842 100644 --- a/Tools/AP_Periph/FSOPowerStack.h +++ b/Tools/AP_Periph/FSOPowerStack.h @@ -45,7 +45,6 @@ class FSOPowerStack { }; uint32_t last_fan_ms; - uint32_t last_fan_report_ms; FAN fans[4]; void fan_handler(uint8_t pin, @@ -60,7 +59,7 @@ class FSOPowerStack { void update_internal_BEC(); uint32_t last_report_ms; - void report_voltages(); + void report(); bool switch_1_on;