Dyno view proposal (#1999)

* DynoView creation

module implementation and unit_tests

* update acc algo

we only calcualte new acceleration value if speed has changed, not on every callback.

* added acceleration ts_channel

* fixed broken unit tests

* fixed accel sign

* review updates

* Update test_dynoview.cpp

fix unit_tests

* Update engine_controller.cpp

Fix .ram4 unused size

* Update test_dynoview.cpp

firmware/config/stm32f4ems/efifeatures.h

@@ -15,6 +15,8 @@
 
 #define EFI_LAUNCH_CONTROL TRUE
 
+#define EFI_DYNO_VIEW TRUE
+
 #define EFI_FSIO TRUE
 
 #ifndef EFI_CDM_INTEGRATION

firmware/console/status_loop.cpp

@@ -62,6 +62,7 @@
 #include "cdm_ion_sense.h"
 #include "binary_logging.h"
 #include "buffered_writer.h"
+#include "dynoview.h"
 
 extern bool main_loop_started;
 
@@ -611,6 +612,10 @@ void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels DECLARE_
 		tsOutputChannels->manifoldAirPressure = mapValue;
 	}
 
+#if EFI_DYNO_VIEW
+	tsOutputChannels->VssAcceleration = getDynoviewAcceleration(PASS_ENGINE_PARAMETER_SIGNATURE);
+#endif
+
 	//tsOutputChannels->knockCount = engine->knockCount;
 	//tsOutputChannels->knockLevel = engine->knockVolts;
 

firmware/controllers/algo/algo.mk

@@ -6,6 +6,7 @@ CONTROLLERS_ALGO_SRC_CPP = $(PROJECT_DIR)/controllers/algo/advance_map.cpp \
 	$(PROJECT_DIR)/controllers/algo/fuel_math.cpp \
 	$(PROJECT_DIR)/controllers/algo/accel_enrichment.cpp \
 	$(PROJECT_DIR)/controllers/algo/launch_control.cpp \
+	$(PROJECT_DIR)/controllers/algo/dynoview.cpp \
 	$(PROJECT_DIR)/controllers/algo/engine_configuration.cpp \
 	$(PROJECT_DIR)/controllers/algo/engine.cpp \
 	$(PROJECT_DIR)/controllers/algo/engine2.cpp \

firmware/controllers/algo/dynoview.cpp

@@ -0,0 +1,179 @@
+/*
+ * @file dynoview.cpp
+ *
+ * @date Nov 29, 2020
+ * @author Alexandru Miculescu, (c) 2012-2020
+ */
+
+#include "engine.h"
+
+#if EFI_DYNO_VIEW
+#include "dynoview.h"
+#include "vehicle_speed.h"
+
+static Logging *logger;
+
+#if EFI_TUNER_STUDIO
+#include "tunerstudio_outputs.h"
+extern TunerStudioOutputChannels tsOutputChannels;
+#endif /* EFI_TUNER_STUDIO */
+
+EXTERN_ENGINE;
+
+DynoView dynoInstance;
+
+void DynoView::update(vssSrc src) {
+
+    efitimeus_t timeNow, deltaTime = 0.0;
+    float speed,deltaSpeed = 0.0;
+    timeNow = getTimeNowUs();
+    speed = getVehicleSpeed();
+    if (src == ICU) {
+        speed = efiRound(speed,1.0);
+    } else {
+        //use speed with 0.001 precision from source CAN
+        speed = efiRound(speed,0.001);
+    }
+
+    if(timeStamp != 0) {
+
+        if (vss != speed) {
+            deltaTime = timeNow - timeStamp;
+            if (vss > speed) {
+                deltaSpeed = (vss - speed);
+                direction = 1; //decceleration
+            } else {
+                deltaSpeed = speed - vss;
+                direction = 0; //acceleration
+            }
+
+            //save data
+            timeStamp = timeNow;
+            vss = speed;
+        }
+        
+        //updating here would display acceleration = 0 at constant speed
+        updateAcceleration(deltaTime, deltaSpeed);
+#if EFI_TUNER_STUDIO
+	    if (CONFIG(debugMode) == DBG_44) {
+		    tsOutputChannels.debugIntField1 = deltaTime;
+		    tsOutputChannels.debugFloatField1 = vss;
+		    tsOutputChannels.debugFloatField2 = speed;
+		    tsOutputChannels.debugFloatField3 = deltaSpeed;
+            tsOutputChannels.debugFloatField4 = acceleration;
+	    }
+#endif /* EFI_TUNER_STUDIO */        
+        updateHP();
+
+    } else {
+        //ensure we grab init values
+        timeStamp = timeNow;
+        vss = speed;
+    }
+}
+
+/**
+ * input units: deltaSpeed in km/h
+ *              deltaTime in uS
+ */
+void DynoView::updateAcceleration(efitimeus_t deltaTime, float deltaSpeed) {
+    if (deltaSpeed != 0.0) {
+        acceleration = ((deltaSpeed / 3.6) / (deltaTime / 1000000.0));
+        if (direction) {
+            //decceleration
+            acceleration *= -1;
+        }
+    } else {
+        acceleration = 0.0;
+    }
+}
+
+/**
+ * E = m*a
+ * ex. 900 (kg) * 1.5 (m/s^2) = 1350N
+ * P = F*V
+ * 1350N * 35(m/s) = 47250Watt (35 m/s is the final velocity)
+ * 47250 * (1HP/746W) = 63HP
+ * https://www.youtube.com/watch?v=FnN2asvFmIs
+ * we do not take resistence into account right now.
+ */
+void DynoView::updateHP() {
+
+    //these are actually at the wheel
+    //we would need final drive to calcualte the correct torque at the wheel
+    if (acceleration != 0) {
+        engineForce = CONFIG(vehicleWeight) * acceleration;
+        enginePower = engineForce * (vss / 3.6);
+        engineHP = enginePower / 746;
+        if (isValidRpm(GET_RPM())) { 
+            engineTorque = ((engineHP * 5252) / GET_RPM());  
+        }
+    } else {
+        //we should calculate static power
+    }
+
+}
+
+#if EFI_UNIT_TEST
+void DynoView::setAcceleration(float a) {
+    acceleration = a;
+}
+#endif
+
+float DynoView::getAcceleration() {
+    return acceleration;
+}
+
+int DynoView::getEngineForce() {
+    return engineForce;
+}
+
+int DynoView::getEnginePower() {
+    return (enginePower/1000);
+}
+
+int DynoView::getEngineHP() {
+    return engineHP;
+}
+
+int DynoView::getEngineTorque() {
+    return (engineTorque/0.73756);
+}
+
+
+float getDynoviewAcceleration(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+    return dynoInstance.getAcceleration();
+}
+
+int getDynoviewPower(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+    return dynoInstance.getEnginePower();
+}
+
+/**
+ * Periodic update function called from SlowCallback.
+ * Only updates if we have Vss from input pin.
+ */
+void updateDynoView(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+    if ((CONFIG(vehicleSpeedSensorInputPin) != GPIO_UNASSIGNED) &&
+        (!CONFIG(enableCanVss))) {
+        dynoInstance.update(ICU);
+    }
+}
+
+/**
+ * This function is called after every CAN msg received, we process it
+ * as soon as we can to be more acurate.
+ */ 
+void updateDynoViewCan(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
+    if (!CONFIG(enableCanVss)) {
+        return;
+    }
+    
+    dynoInstance.update(CAN);
+}
+
+void initDynoView(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX) {
+	logger = sharedLogger;
+}
+
+#endif /* EFI_DYNO_VIEW */

firmware/controllers/algo/dynoview.h

@@ -0,0 +1,59 @@
+/*
+ * @file dynoview.h
+ *
+ * @date Nov 29, 2020
+ * @author Alexandru Miculescu, (c) 2012-2020
+ */
+
+#pragma once
+
+#include "engine_ptr.h"
+
+class Logging;
+void initDynoView(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX);
+void updateDynoView(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+void updateDynoViewCan(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+float getDynoviewAcceleration(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+int getDynoviewPower(DECLARE_ENGINE_PARAMETER_SIGNATURE);
+
+typedef enum{
+    ICU = 0,
+    CAN,
+}vssSrc;
+
+class DynoView {
+public:
+	DECLARE_ENGINE_PTR;
+
+	// Update the state of the launch control system
+	void update(vssSrc src);
+    void updateAcceleration(efitick_t deltaTime, float deltaSpeed);
+    void updateHP();
+    float getAcceleration();
+    int getEngineForce();
+    //in KW
+    int getEnginePower();
+
+    int getEngineHP();
+    //in NM
+    int getEngineTorque();
+#if EFI_UNIT_TEST
+    void setAcceleration(float a);
+#endif
+private:
+	efitimeus_t timeStamp = 0;
+    //km/h unit
+    float vss = 0;
+    //m/s/s unit
+    float acceleration = 0;
+    //engine force in N
+    int engineForce;
+    //engine power in W
+    int enginePower;
+    //engine powerin HP
+    int engineHP;
+    //Torque in lb-ft
+    int engineTorque;
+    //sign
+    uint8_t direction;
+};

firmware/controllers/algo/engine.cpp

@@ -31,6 +31,7 @@
 #include "sensor.h"
 #include "gppwm.h"
 #include "tachometer.h"
+#include "dynoview.h"
 #if EFI_MC33816
  #include "mc33816.h"
 #endif // EFI_MC33816
@@ -209,6 +210,10 @@ void Engine::periodicSlowCallback(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	}
 #endif
 
+#if EFI_DYNO_VIEW
+	updateDynoView(PASS_ENGINE_PARAMETER_SIGNATURE);
+#endif
+
 	slowCallBackWasInvoked = true;
 
 #if HW_CHECK_MODE

firmware/controllers/can/can_vss.cpp

@@ -13,6 +13,7 @@
 #include "engine_configuration.h"
 #include "engine.h"
 #include "vehicle_speed.h"
+#include "dynoview.h"
 
 EXTERN_ENGINE;
 
@@ -98,6 +99,9 @@ void processCanRxVss(const CANRxFrame& frame, efitick_t nowNt) {
             break;
     }
 
+#if EFI_DYNO_VIEW
+    updateDynoViewCan(PASS_ENGINE_PARAMETER_SIGNATURE);
+#endif
 }
 
 float getVehicleCanSpeed(void) {

firmware/controllers/engine_controller.cpp

@@ -59,6 +59,7 @@
 #include "date_stamp.h"
 #include "buttonshift.h"
 #include "start_stop.h"
+#include "dynoview.h"
 
 #if EFI_SENSOR_CHART
 #include "sensor_chart.h"
@@ -605,6 +606,10 @@ void commonInitEngineController(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_S
 	initLaunchControl(sharedLogger PASS_ENGINE_PARAMETER_SUFFIX);
 #endif
 
+#if EFI_DYNO_VIEW
+	initDynoView(sharedLogger PASS_ENGINE_PARAMETER_SUFFIX);
+#endif
+
 #if EFI_SHAFT_POSITION_INPUT
 	/**
 	 * there is an implicit dependency on the fact that 'tachometer' listener is the 1st listener - this case

unit_tests/efifeatures.h

@@ -21,6 +21,8 @@
 
 #define EFI_LAUNCH_CONTROL TRUE
 
+#define EFI_DYNO_VIEW TRUE
+
 #define EFI_BOOST_CONTROL TRUE
 
 #define EFI_IDLE_CONTROL TRUE

unit_tests/tests/test_dynoview.cpp

@@ -0,0 +1,114 @@
+#include "engine_test_helper.h"
+#include "engine_controller.h"
+#include "dynoview.h"
+#include "vehicle_speed.h"
+
+#include <gtest/gtest.h>
+
+
+void printResults(DynoView *dut) {
+#ifdef DBG_TESTS    
+    std::cerr.precision(32);
+    std::cerr << "Acceleration m/s " << dut->getAcceleration() << std::endl; 
+    std::cerr << "Car force in N " << dut->getEngineForce() << std::endl;
+    std::cerr << "Car power in KW "<< dut->getEnginePower() << std::endl;
+    std::cerr << "Car HP " << dut->getEngineHP() << std::endl;
+    std::cerr << "Car Torque at wheel Nm " << dut->getEngineTorque() << std::endl;
+#else
+    (void)dut;
+#endif
+}
+
+
+TEST(DynoView, VSS_T1) {
+    WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+    DynoView dut;
+    INJECT_ENGINE_REFERENCE(&dut);
+
+    // Test Speed trashold
+    engineConfiguration->vehicleWeight = 900; 
+    eth.moveTimeForwardMs(50);
+	
+    setMockVehicleSpeed(18.0);
+    dut.update(ICU);
+
+    eth.smartMoveTimeForwardSeconds(20);
+    setMockVehicleSpeed(126.0);
+    dut.update(ICU);
+
+    ASSERT_EQ(1.5, dut.getAcceleration()); 
+}
+
+TEST(DynoView, algo) {
+    WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+    DynoView dut;
+    INJECT_ENGINE_REFERENCE(&dut);
+
+    // Test Speed trashold
+    engineConfiguration->vehicleWeight = 900; 
+
+    //to capture vss
+    setMockVehicleSpeed(35*3.6);
+    dut.update(ICU);
+
+    dut.setAcceleration(1.5);
+    dut.updateHP();
+
+    ASSERT_EQ(1.5, dut.getAcceleration());
+    ASSERT_EQ(1350, dut.getEngineForce());
+    ASSERT_EQ(47, dut.getEnginePower());
+    ASSERT_EQ(63, dut.getEngineHP());
+    
+    printResults(&dut);
+
+}
+
+TEST(DynoView, VSS_fast) {
+    WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+    DynoView dut;
+    INJECT_ENGINE_REFERENCE(&dut);
+
+    // Test Speed trashold
+    engineConfiguration->vehicleWeight = 900; //kg
+    engine->rpmCalculator.mockRpm = 2200;
+    eth.moveTimeForwardMs(50);
+	
+    setMockVehicleSpeed(50.0);
+    dut.update(CAN);
+
+    //delay 50ms
+    eth.moveTimeForwardMs(50);
+    setMockVehicleSpeed(50.252);
+    dut.update(CAN);
+
+    ASSERT_EQ(1259, dut.getEngineForce());
+    printResults(&dut);
+}
+
+
+TEST(DynoView, VSS_Torque) {
+    WITH_ENGINE_TEST_HELPER(TEST_ENGINE);
+
+    DynoView dut;
+    INJECT_ENGINE_REFERENCE(&dut);
+
+    // Test Speed trashold
+    engineConfiguration->vehicleWeight = 900; //kg
+    engine->rpmCalculator.mockRpm = 2200;
+    eth.moveTimeForwardMs(50);
+	
+    setMockVehicleSpeed(80.0);
+    dut.update(CAN);
+
+    //delay 50ms
+    eth.moveTimeForwardMs(50);
+    setMockVehicleSpeed(80.504);
+    dut.update(CAN);
+
+    ASSERT_EQ(242, dut.getEngineTorque());
+    printResults(&dut);
+}
+

unit_tests/tests/tests.mk

@@ -62,4 +62,5 @@ TESTS_SRC_CPP = \
 	tests/test_gppwm.cpp \
 	tests/test_fuel_math.cpp \
 	tests/test_binary_log.cpp \
+	tests/test_dynoview.cpp \