101 lines
2.3 KiB
C++
101 lines
2.3 KiB
C++
/**
|
|
* @file can_sensor.h
|
|
*
|
|
* @date March 31, 2020
|
|
* @author Matthew Kennedy, (c) 2020
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "stored_value_sensor.h"
|
|
#include "efi_scaled_channel.h"
|
|
#include "can_msg_tx.h"
|
|
#include "obd2.h"
|
|
#include "can.h"
|
|
#include "can_listener.h"
|
|
|
|
/**
|
|
* Sensor which reads it's value from CAN
|
|
*/
|
|
class CanSensorBase : public StoredValueSensor, public CanListener {
|
|
public:
|
|
CanSensorBase(uint32_t eid, SensorType type, efitick_t timeout)
|
|
: StoredValueSensor(type, timeout)
|
|
, CanListener(eid)
|
|
{
|
|
}
|
|
|
|
void showInfo(const char* sensorName) const override;
|
|
};
|
|
|
|
template <typename TStorage, int TScale>
|
|
class CanSensor : public CanSensorBase {
|
|
public:
|
|
CanSensor(uint32_t eid, uint8_t offset, SensorType type, efitick_t timeout)
|
|
: CanSensorBase(eid, type, timeout)
|
|
, m_offset(offset)
|
|
{
|
|
}
|
|
|
|
void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override {
|
|
// Compute the location of our data within the frame
|
|
const uint8_t* dataLocation = &frame.data8[m_offset];
|
|
|
|
// Reinterpret as a scaled_channel - it already has the logic for decoding a scaled integer to a float
|
|
const auto scaler = reinterpret_cast<const scaled_channel<TStorage, TScale>*>(dataLocation);
|
|
|
|
// Actually do the conversion
|
|
float value = *scaler;
|
|
setValidValue(value, nowNt);
|
|
}
|
|
|
|
private:
|
|
const uint8_t m_offset;
|
|
};
|
|
|
|
#if EFI_PROD_CODE
|
|
|
|
template <int Size, int Offset>
|
|
class ObdCanSensor: public CanSensorBase {
|
|
public:
|
|
ObdCanSensor(int PID, float Scale, SensorType type) :
|
|
CanSensorBase(OBD_TEST_RESPONSE, type, /* timeout, never expire */ 0) {
|
|
this->PID = PID;
|
|
this->Scale = Scale;
|
|
}
|
|
|
|
void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override {
|
|
if (frame.data8[2] != PID) {
|
|
return;
|
|
}
|
|
|
|
int iValue;
|
|
if (Size == 2) {
|
|
iValue = frame.data8[3] * 256 + frame.data8[4];
|
|
} else {
|
|
iValue = frame.data8[3];
|
|
}
|
|
|
|
float fValue = (1.0 * iValue / Scale) - Offset;
|
|
setValidValue(fValue, nowNt);
|
|
}
|
|
|
|
CanListener* request() override {
|
|
{
|
|
CanTxMessage msg(CanCategory::OBD, OBD_TEST_REQUEST);
|
|
msg[0] = _OBD_2;
|
|
msg[1] = OBD_CURRENT_DATA;
|
|
msg[2] = PID;
|
|
}
|
|
// let's sleep on write update after each OBD request, this would give read thread a chance to read response
|
|
// todo: smarter logic of all this with with semaphore not just sleep
|
|
chThdSleepMilliseconds(300);
|
|
return CanListener::request();
|
|
}
|
|
|
|
int PID;
|
|
float Scale;
|
|
};
|
|
|
|
#endif // EFI_PROD_CODE
|