Line |
Branch |
Decision |
Exec |
Source |
1 |
|
|
|
/** |
2 |
|
|
|
* @file can_sensor.h |
3 |
|
|
|
* |
4 |
|
|
|
* @date March 31, 2020 |
5 |
|
|
|
* @author Matthew Kennedy, (c) 2020 |
6 |
|
|
|
*/ |
7 |
|
|
|
|
8 |
|
|
|
#pragma once |
9 |
|
|
|
|
10 |
|
|
|
#include "stored_value_sensor.h" |
11 |
|
|
|
#include "efi_scaled_channel.h" |
12 |
|
|
|
#include "can_msg_tx.h" |
13 |
|
|
|
#include "obd2.h" |
14 |
|
|
|
#include "can.h" |
15 |
|
|
|
#include "can_listener.h" |
16 |
|
|
|
|
17 |
|
|
|
/** |
18 |
|
|
|
* Sensor which reads it's value from CAN |
19 |
|
|
|
*/ |
20 |
|
|
|
class CanSensorBase : public StoredValueSensor, public CanListener { |
21 |
|
|
|
public: |
22 |
|
|
8 |
CanSensorBase(uint32_t eid, SensorType type, efidur_t timeout) |
23 |
|
|
8 |
: StoredValueSensor(type, timeout) |
24 |
|
|
8 |
, CanListener(eid) |
25 |
|
|
|
{ |
26 |
|
|
8 |
} |
27 |
|
|
|
|
28 |
|
|
|
void showInfo(const char* sensorName) const override; |
29 |
|
|
|
}; |
30 |
|
|
|
|
31 |
|
|
|
template <typename TStorage, int TScale> |
32 |
|
|
|
class CanSensor : public CanSensorBase { |
33 |
|
|
|
public: |
34 |
|
|
|
CanSensor(uint32_t eid, uint8_t offset, SensorType type, efidur_t timeout) |
35 |
|
|
|
: CanSensorBase(eid, type, timeout) |
36 |
|
|
|
, m_offset(offset) |
37 |
|
|
|
{ |
38 |
|
|
|
} |
39 |
|
|
|
|
40 |
|
|
|
void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override { |
41 |
|
|
|
// Compute the location of our data within the frame |
42 |
|
|
|
const uint8_t* dataLocation = &frame.data8[m_offset]; |
43 |
|
|
|
|
44 |
|
|
|
// Reinterpret as a scaled_channel - it already has the logic for decoding a scaled integer to a float |
45 |
|
|
|
const auto scaler = reinterpret_cast<const scaled_channel<TStorage, TScale>*>(dataLocation); |
46 |
|
|
|
|
47 |
|
|
|
// Actually do the conversion |
48 |
|
|
|
float value = *scaler; |
49 |
|
|
|
setValidValue(value, nowNt); |
50 |
|
|
|
} |
51 |
|
|
|
|
52 |
|
|
|
private: |
53 |
|
|
|
const uint8_t m_offset; |
54 |
|
|
|
}; |
55 |
|
|
|
|
56 |
|
|
|
#if EFI_PROD_CODE |
57 |
|
|
|
|
58 |
|
|
|
template <int Size, int Offset> |
59 |
|
|
|
class ObdCanSensor: public CanSensorBase { |
60 |
|
|
|
public: |
61 |
|
|
|
ObdCanSensor(int p_PID, float p_Scale, SensorType type) : |
62 |
|
|
|
CanSensorBase(OBD_TEST_RESPONSE, type, /* timeout, never expire */ 0) { |
63 |
|
|
|
this->PID = p_PID; |
64 |
|
|
|
this->Scale = p_Scale; |
65 |
|
|
|
} |
66 |
|
|
|
|
67 |
|
|
|
void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override { |
68 |
|
|
|
if (frame.data8[2] != PID) { |
69 |
|
|
|
return; |
70 |
|
|
|
} |
71 |
|
|
|
|
72 |
|
|
|
int iValue; |
73 |
|
|
|
if (Size == 2) { |
74 |
|
|
|
iValue = frame.data8[3] * 256 + frame.data8[4]; |
75 |
|
|
|
} else { |
76 |
|
|
|
iValue = frame.data8[3]; |
77 |
|
|
|
} |
78 |
|
|
|
|
79 |
|
|
|
float fValue = (1.0 * iValue / Scale) - Offset; |
80 |
|
|
|
setValidValue(fValue, nowNt); |
81 |
|
|
|
} |
82 |
|
|
|
|
83 |
|
|
|
CanListener* request() override { |
84 |
|
|
|
{ |
85 |
|
|
|
CanTxMessage msg(CanCategory::OBD, OBD_TEST_REQUEST); |
86 |
|
|
|
msg[0] = _OBD_2; |
87 |
|
|
|
msg[1] = OBD_CURRENT_DATA; |
88 |
|
|
|
msg[2] = PID; |
89 |
|
|
|
} |
90 |
|
|
|
// let's sleep on write update after each OBD request, this would give read thread a chance to read response |
91 |
|
|
|
// todo: smarter logic of all this with with semaphore not just sleep |
92 |
|
|
|
chThdSleepMilliseconds(300); |
93 |
|
|
|
return CanListener::request(); |
94 |
|
|
|
} |
95 |
|
|
|
|
96 |
|
|
|
int PID; |
97 |
|
|
|
float Scale; |
98 |
|
|
|
}; |
99 |
|
|
|
|
100 |
|
|
|
#endif // EFI_PROD_CODE |
101 |
|
|
|
|