40 lines
678 B
C++
40 lines
678 B
C++
#include "pch.h"
|
|
|
|
#include "speedometer.h"
|
|
|
|
static SimplePwm speedoPwm("speedo");
|
|
|
|
static bool hasSpeedoInit = false;
|
|
|
|
void speedoUpdate() {
|
|
if (!hasSpeedoInit) {
|
|
return;
|
|
}
|
|
|
|
float kph = Sensor::getOrZero(SensorType::VehicleSpeed);
|
|
float kps = kph * (1. / 3600);
|
|
float freq = kps * engineConfiguration->speedometerPulsePerKm;
|
|
|
|
if (freq < 1) {
|
|
freq = NAN;
|
|
}
|
|
|
|
speedoPwm.setFrequency(freq);
|
|
}
|
|
|
|
void initSpeedometer() {
|
|
hasSpeedoInit = false;
|
|
|
|
if (!isBrainPinValid(engineConfiguration->speedometerOutputPin)) {
|
|
return;
|
|
}
|
|
|
|
startSimplePwm(&speedoPwm,
|
|
"Speedometer",
|
|
&engine->scheduler,
|
|
&enginePins.speedoOut,
|
|
NAN, 0.5f);
|
|
|
|
hasSpeedoInit = true;
|
|
}
|