From cd7dc516d01344238e3db67d79a74efd8ac6ddcb Mon Sep 17 00:00:00 2001 From: ene9ba Date: Fri, 19 Apr 2024 12:03:39 +0200 Subject: [PATCH] average calculation added voltage and current --- Firmware/LowLevel/include/floating_average.h | 46 ++++++++++++++++ Firmware/LowLevel/src/floating_average.cpp | 58 ++++++++++++++++++++ Firmware/LowLevel/src/main.cpp | 49 ++++++++++++++--- 3 files changed, 145 insertions(+), 8 deletions(-) create mode 100644 Firmware/LowLevel/include/floating_average.h create mode 100644 Firmware/LowLevel/src/floating_average.cpp diff --git a/Firmware/LowLevel/include/floating_average.h b/Firmware/LowLevel/include/floating_average.h new file mode 100644 index 00000000..bcb2e19b --- /dev/null +++ b/Firmware/LowLevel/include/floating_average.h @@ -0,0 +1,46 @@ +// Created by Elmar Elflein on 18/05/23. +// Copyright (c) 2022 Elmar Elflein. All rights reserved. +// +// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. +// +// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based on it without getting my consent first. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// +#ifndef _FLOATING_AVERAGE_H_ +#define _FLOATING_AVERAGE_H_ + +#include + + +class FloatingAverage +{ + + public: + FloatingAverage(); + + void begin(uint32_t nrvalues); + + void AddToFloatAvg(float value); + + float GetFloatAvg(); + + + private: + + uint32_t numberfloatingvalues; + float* valuearray; + uint32_t IndexNextvalue; + + +}; + + +#endif // _FLOATING_AVERAGE_H_ HEADER_FILE diff --git a/Firmware/LowLevel/src/floating_average.cpp b/Firmware/LowLevel/src/floating_average.cpp new file mode 100644 index 00000000..02a68768 --- /dev/null +++ b/Firmware/LowLevel/src/floating_average.cpp @@ -0,0 +1,58 @@ +// Created by Elmar Elflein on 18/07/22. +// Copyright (c) 2022 Elmar Elflein. All rights reserved. +// +// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. +// +// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based on it without getting my consent first. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// + +#include + + +FloatingAverage::FloatingAverage() +{ + +} + + +// Init floating values single value array and set to 0 +void FloatingAverage::begin(uint32_t nrvalues) +{ + + this->numberfloatingvalues = nrvalues; + this->valuearray = new float[this->numberfloatingvalues]; + for (uint32_t i=0; i < numberfloatingvalues; i++) + { + valuearray[i] = 0.0; + } + this->IndexNextvalue = 0; +} + +// Add an new single value to the array +void FloatingAverage::AddToFloatAvg(float value) +{ + valuearray[IndexNextvalue] = value; + IndexNextvalue++; + IndexNextvalue %= numberfloatingvalues; +} + +// calculate mean value of single values from array +float FloatingAverage::GetFloatAvg() +{ + float TempSum = 0; + for (uint32_t i=0; i < numberfloatingvalues; ++i) + { + TempSum += valuearray[i]; + } + return TempSum / numberfloatingvalues; +} + + diff --git a/Firmware/LowLevel/src/main.cpp b/Firmware/LowLevel/src/main.cpp index 5204a613..d64c2d08 100644 --- a/Firmware/LowLevel/src/main.cpp +++ b/Firmware/LowLevel/src/main.cpp @@ -14,6 +14,11 @@ // SOFTWARE. // // +// Changes 18.04.2024 Elmar + +// floating average integrated for UBat, UCharge and ICharge Hardware added 3,9 kohm between F2 and GND to prevent voltage injection without loading voltage +// added offset for analog voltage + #include #include #include @@ -24,6 +29,7 @@ #include "imu.h" #include "debug.h" #include "nv_config.h" +#include #ifdef ENABLE_SOUND_MODULE #include @@ -37,7 +43,7 @@ #define TILT_EMERGENCY_MILLIS 2500 // Time for a single wheel to be lifted in order to count as emergency (0 disable). This is to filter uneven ground. #define LIFT_EMERGENCY_MILLIS 100 // Time for both wheels to be lifted in order to count as emergency (0 disable). This is to filter uneven ground. #define BUTTON_EMERGENCY_MILLIS 20 // Time for button emergency to activate. This is to debounce the button. - +#define ANALOG_MEAN_COUNT 20 // size of array for calculation meanvalues #define PACKET_SERIAL Serial1 SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250); @@ -52,6 +58,7 @@ SerialPIO uiSerial(PIN_UI_TX, PIN_UI_RX, 250); #define VIN_R1 10000.0f #define VIN_R2 1000.0f #define R_SHUNT 0.003f +#define ANALOG_VOLTAGE_OFFSET 21 // not investigated, but a/d shows an Offset #define CURRENT_SENSE_GAIN 100.0f #define BATT_ABS_MAX 28.7f @@ -69,6 +76,10 @@ uint8_t led_blink_counter = 0; PacketSerial packetSerial; // COBS communication PICO <> Raspi PacketSerial UISerial; // COBS communication PICO UI-Board FastCRC16 CRC16; +// floating average analog values objects +FloatingAverage Vcharge_Mean; +FloatingAverage VBatt_Mean; +FloatingAverage Icharge_Mean; unsigned long last_imu_millis = 0; unsigned long last_status_update_millis = 0; @@ -414,8 +425,13 @@ void setup() { sound_available = soundSystem::begin(); if (sound_available) { p.neoPixelSetValue(0, 0, 0, 255, true); + soundSystem::setDFPis5V(nv_cfg->config_bitmask & NV_CONFIG_BIT_DFPIS5V); + soundSystem::setDFPis5V(true); soundSystem::setLanguage(&nv_cfg->language, true); + iso639_1 mylang = {'d', 'e'}; + soundSystem::setLanguage(&mylang, true); + soundSystem::setVolume(nv_cfg->volume); // Do NOT play any initial sound now, as we've to handle the special case of // old DFPlayer SD-Card format @ DFROBOT LISP3. See soundSystem::processSounds() @@ -480,6 +496,11 @@ void setup() { status_message.status_bitmask |= 1; + // Init anlog values floating average + Vcharge_Mean.begin(ANALOG_MEAN_COUNT); + VBatt_Mean.begin(ANALOG_MEAN_COUNT); + Icharge_Mean.begin(ANALOG_MEAN_COUNT); + rp2040.resumeOtherCore(); // Cover UI board clear all LEDs @@ -619,6 +640,8 @@ void onPacketReceived(const uint8_t *buffer, size_t size) { (config_bitmask & LL_HIGH_LEVEL_CONFIG_BIT_DFPIS5V) ? nv_cfg->config_bitmask |= NV_CONFIG_BIT_DFPIS5V : nv_cfg->config_bitmask &= ~NV_CONFIG_BIT_DFPIS5V; #ifdef ENABLE_SOUND_MODULE soundSystem::setDFPis5V(nv_cfg->config_bitmask & NV_CONFIG_BIT_DFPIS5V); + soundSystem::setDFPis5V(true); + #endif // Volume if (pkt->volume >= 0) { @@ -634,6 +657,8 @@ void onPacketReceived(const uint8_t *buffer, size_t size) { } #ifdef ENABLE_SOUND_MODULE soundSystem::setLanguage(&nv_cfg->language); + iso639_1 mylang = {'d', 'e'}; + soundSystem::setLanguage(&mylang, true); #endif // Sender requested a config-response packet if (buffer[0] == PACKET_ID_LL_HIGH_LEVEL_CONFIG_REQ) @@ -715,15 +740,19 @@ void loop() { if (now - last_status_update_millis > STATUS_CYCLETIME) { updateNeopixel(); - status_message.v_battery = - (float) analogRead(PIN_ANALOG_BATTERY_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2); - status_message.v_charge = - (float) analogRead(PIN_ANALOG_CHARGE_VOLTAGE) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2); + float ad_value = (float) (analogRead(PIN_ANALOG_BATTERY_VOLTAGE)-ANALOG_VOLTAGE_OFFSET) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2); + // insert currtent value to mean floating array + VBatt_Mean.AddToFloatAvg(ad_value); + ad_value = (float) (analogRead(PIN_ANALOG_CHARGE_VOLTAGE)-ANALOG_VOLTAGE_OFFSET) * (3.3f / 4096.0f) * ((VIN_R1 + VIN_R2) / VIN_R2); + // insert currtent value to mean floating array + Vcharge_Mean.AddToFloatAvg(ad_value); + #ifndef IGNORE_CHARGING_CURRENT - status_message.charging_current = - (float) analogRead(PIN_ANALOG_CHARGE_CURRENT) * (3.3f / 4096.0f) / (CURRENT_SENSE_GAIN * R_SHUNT); + ad_value = (float) (analogRead(PIN_ANALOG_CHARGE_CURRENT)-ANALOG_VOLTAGE_OFFSET) * (3.3f / 4096.0f) / (CURRENT_SENSE_GAIN * R_SHUNT); + // insert currtent value to mean floating array + Icharge_Mean.AddToFloatAvg(ad_value); #else - status_message.charging_current = -1.0f; + ad_value = -1.0f; #endif status_message.status_bitmask = (status_message.status_bitmask & 0b11111011) | ((charging_allowed & 0b1) << 2); status_message.status_bitmask = (status_message.status_bitmask & 0b11011111) | ((sound_available & 0b1) << 5); @@ -763,6 +792,10 @@ void loop() { if (now > next_ui_msg_millis) { next_ui_msg_millis = now + ui_interval; + // calculate mean floating + if (float temp = Vcharge_Mean.GetFloatAvg(); temp < 0.02f) status_message.v_charge = 0.0f; else status_message.v_charge = temp; + if (float temp = Icharge_Mean.GetFloatAvg(); temp < 0.02f) status_message.charging_current = 0.0f; else status_message.charging_current = temp; + status_message.v_battery =VBatt_Mean.GetFloatAvg(); manageUISubscriptions(); } #ifdef ENABLE_SOUND_MODULE