diff --git a/main/can.h b/main/can.h index 8a6bc9b..6bfa824 100644 --- a/main/can.h +++ b/main/can.h @@ -22,6 +22,11 @@ #include CAN_PLUGIN #endif +namespace { + float fixFrontBatVoltage(int16_t value); + float fixBackBatVoltage(int16_t value); +} + namespace can { namespace { @@ -252,6 +257,7 @@ bool tryParseCanInput() front.lastCanFeedback = espchrono::millis_clock::now(); front.feedbackValid = true; + front.calibrated.batVoltage = fixFrontBatVoltage(front.feedback.batVoltage); return true; } else @@ -264,6 +270,7 @@ bool tryParseCanInput() { back.lastCanFeedback = espchrono::millis_clock::now(); back.feedbackValid = true; + back.calibrated.batVoltage = fixBackBatVoltage(back.feedback.batVoltage); return true; } else diff --git a/main/controller.h b/main/controller.h index ca2772a..148d74f 100644 --- a/main/controller.h +++ b/main/controller.h @@ -52,8 +52,12 @@ struct Controller { bool feedbackValid{}; bobbycar::protocol::serial::Feedback feedback{}; + #ifdef FEATURE_SERIAL FeedbackParser parser{serial, feedbackValid, feedback}; #endif + struct Calibrated { + float batVoltage; + } calibrated; }; } diff --git a/main/displays/statusdisplay.h b/main/displays/statusdisplay.h index 71a5e0e..fffc344 100644 --- a/main/displays/statusdisplay.h +++ b/main/displays/statusdisplay.h @@ -297,7 +297,14 @@ void StatusDisplay::BoardStatus::redraw(const Controller &controller) if (controller.feedbackValid) { - m_labelVoltage.redraw(fmt::format("{:.2f}V", fixBatVoltage(controller.feedback.batVoltage))); + if (settings.battery.applyCalibration) + { + m_labelVoltage.redraw(fmt::format("{:.2f}V", controller.calibrated.batVoltage)); + } + else + { + m_labelVoltage.redraw(fmt::format("{:.2f}V", fixBatVoltage(controller.feedback.batVoltage))); + } m_labelTemperature.redraw(fmt::format("{:.2f}C", fixBoardTemp(controller.feedback.boardTemp))); m_leftMotor.redraw(controller.feedback.left); m_rightMotor.redraw(controller.feedback.right); diff --git a/main/utils.h b/main/utils.h index 742012c..c970cb8 100644 --- a/main/utils.h +++ b/main/utils.h @@ -67,6 +67,7 @@ float fixBatVoltage(int16_t value) float fixFrontBatVoltage(int16_t value) { float frontVoltage = fixBatVoltage(value); + if (!settings.battery.applyCalibration) return frontVoltage; frontVoltage = ((frontVoltage - fixBatVoltage(settings.battery.front30VoltCalibration)) * (20.f / (fixBatVoltage(settings.battery.front50VoltCalibration) - fixBatVoltage(settings.battery.front30VoltCalibration))) + 30.f); return frontVoltage; } @@ -74,6 +75,7 @@ float fixFrontBatVoltage(int16_t value) float fixBackBatVoltage(int16_t value) { float backVoltage = fixBatVoltage(value); + if (!settings.battery.applyCalibration) return backVoltage; backVoltage = ((backVoltage - fixBatVoltage(settings.battery.back30VoltCalibration)) * (20.f / (fixBatVoltage(settings.battery.back50VoltCalibration) - fixBatVoltage(settings.battery.back30VoltCalibration))) + 30.f); return backVoltage; }