34 lines
960 B
C++
34 lines
960 B
C++
#include "pch.h"
|
|
|
|
void GearDetector::onSlowCallback() {
|
|
m_gearboxRatio = computeGearboxRatio();
|
|
|
|
// TODO: solve for which gear this is
|
|
}
|
|
|
|
float GearDetector::computeGearboxRatio() const {
|
|
auto vssKph = Sensor::getOrZero(SensorType::VehicleSpeed);
|
|
|
|
if (vssKph < 5) {
|
|
// Vehicle too slow to determine gearbox ratio, avoid div/0
|
|
return 0;
|
|
}
|
|
|
|
auto engineRpm = Sensor::getOrZero(SensorType::Rpm);
|
|
|
|
// Convert to wheel RPM
|
|
// km rev 1 hr
|
|
// ------ * ------------ * __________
|
|
// hr km 60 min
|
|
float wheelRpm = vssKph * engineConfiguration->driveWheelRevPerKm * (1 / 60.0f);
|
|
|
|
// Convert to driveshaft RPM
|
|
auto driveshaftRpm = wheelRpm * engineConfiguration->finalGearRatio;
|
|
|
|
return engineRpm / driveshaftRpm;
|
|
}
|
|
|
|
float GearDetector::getGearboxRatio() const {
|
|
return m_gearboxRatio;
|
|
}
|