| 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 |