162 lines
5.3 KiB
C
162 lines
5.3 KiB
C
|
/*
|
||
|
; Project: Open Vehicle Monitor System
|
||
|
; Date: 14th March 2017
|
||
|
;
|
||
|
; Changes:
|
||
|
; 1.0 Initial release
|
||
|
;
|
||
|
; (C) 2011 Michael Stegen / Stegen Electronics
|
||
|
; (C) 2011-2017 Mark Webb-Johnson
|
||
|
; (C) 2011 Sonny Chen @ EPRO/DX
|
||
|
;
|
||
|
; Permission is hereby granted, free of charge, to any person obtaining a copy
|
||
|
; of this software and associated documentation files (the "Software"), to deal
|
||
|
; in the Software without restriction, including without limitation the rights
|
||
|
; to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||
|
; copies of the Software, and to permit persons to whom the Software is
|
||
|
; furnished to do so, subject to the following conditions:
|
||
|
;
|
||
|
; The above copyright notice and this permission notice shall be included in
|
||
|
; all copies or substantial portions of the Software.
|
||
|
;
|
||
|
; THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||
|
; IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||
|
; FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||
|
; AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||
|
; LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||
|
; OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||
|
; THE SOFTWARE.
|
||
|
;
|
||
|
; Portions of this are based on the work of Thomas Barth, and licensed
|
||
|
; under MIT license.
|
||
|
; Copyright (c) 2017, Thomas Barth, barth-dev.de
|
||
|
; https://github.com/ThomasBarth/ESP32-CAN-Driver
|
||
|
*/
|
||
|
|
||
|
#ifndef __CAN_UTILS_H__
|
||
|
#define __CAN_UTILS_H__
|
||
|
|
||
|
#include "can.h"
|
||
|
|
||
|
/**
|
||
|
* canbitset<StoreType>: CAN data packed bit extraction utility
|
||
|
* Usage examples:
|
||
|
* canbitset<uint64_t> canbits(p_frame->data.u8, 8) -- load whole CAN frame
|
||
|
* canbitset<uint32_t> canbits(data+1, 3) -- load 3 bytes beginning at second
|
||
|
* val = canbits.get(12,23) -- extract bits 12-23 of bytes loaded
|
||
|
* Note: no sanity checks.
|
||
|
* Hint: for best performance on ESP32, avoid StoreType uint64_t
|
||
|
*/
|
||
|
template <typename StoreType>
|
||
|
class canbitset
|
||
|
{
|
||
|
private:
|
||
|
StoreType m_store;
|
||
|
|
||
|
public:
|
||
|
canbitset(uint8_t* src, int len)
|
||
|
{
|
||
|
load(src, len);
|
||
|
}
|
||
|
|
||
|
// load message bytes:
|
||
|
void load(uint8_t* src, int len)
|
||
|
{
|
||
|
m_store = 0;
|
||
|
for (int i=0; i<len; i++)
|
||
|
{
|
||
|
m_store <<= 8;
|
||
|
m_store |= src[i];
|
||
|
}
|
||
|
m_store <<= ((sizeof(StoreType)-len) << 3);
|
||
|
}
|
||
|
|
||
|
// extract message part:
|
||
|
// - start,end: bit positions 0…63, 0=MSB of first msg byte
|
||
|
StoreType get(int start, int end)
|
||
|
{
|
||
|
return (StoreType) ((m_store << start) >> (start + (sizeof(StoreType)<<3) - end - 1));
|
||
|
}
|
||
|
};
|
||
|
|
||
|
|
||
|
// Helper macros for filling out the CAN frame
|
||
|
|
||
|
#define FRAME_FILL_0(frame) ;
|
||
|
|
||
|
#define FRAME_FILL_1(frame,d1) \
|
||
|
frame.data.u8[0] = d1;
|
||
|
|
||
|
#define FRAME_FILL_2(frame,d1,d2) \
|
||
|
FRAME_FILL_1(frame,d1) \
|
||
|
frame.data.u8[1] = d2;
|
||
|
|
||
|
#define FRAME_FILL_3(frame,d1,d2,d3) \
|
||
|
FRAME_FILL_2(frame,d1,d2) \
|
||
|
frame.data.u8[2] = d3;
|
||
|
|
||
|
#define FRAME_FILL_4(frame,d1,d2,d3,d4) \
|
||
|
FRAME_FILL_3(frame,d1,d2,d3) \
|
||
|
frame.data.u8[3] = d4;
|
||
|
|
||
|
#define FRAME_FILL_5(frame,d1,d2,d3,d4,d5) \
|
||
|
FRAME_FILL_4(frame,d1,d2,d3,d4) \
|
||
|
frame.data.u8[4] = d5;
|
||
|
|
||
|
#define FRAME_FILL_6(frame,d1,d2,d3,d4,d5,d6) \
|
||
|
FRAME_FILL_5(frame,d1,d2,d3,d4,d5) \
|
||
|
frame.data.u8[5] = d6;
|
||
|
|
||
|
#define FRAME_FILL_7(frame,d1,d2,d3,d4,d5,d6,d7) \
|
||
|
FRAME_FILL_6(frame,d1,d2,d3,d4,d5,d6) \
|
||
|
frame.data.u8[6] = d7;
|
||
|
|
||
|
#define FRAME_FILL_8(frame,d1,d2,d3,d4,d5,d6,d7,d8) \
|
||
|
FRAME_FILL_7(frame,d1,d2,d3,d4,d5,d6,d7) \
|
||
|
frame.data.u8[7] = d8;
|
||
|
|
||
|
#define GET_9TH_ARG(x, arg1, arg2, arg3, arg4, arg5, arg6, arg7, arg8, arg9, ...) arg9
|
||
|
|
||
|
// This is a neat preprocessor trick to select the right FRAME_FILL_x macro depending on parameter count.
|
||
|
#define FRAME_FILL_MACRO_CHOOSER(...) \
|
||
|
GET_9TH_ARG(,##__VA_ARGS__, FRAME_FILL_8, FRAME_FILL_7, FRAME_FILL_6, FRAME_FILL_5, \
|
||
|
FRAME_FILL_4, FRAME_FILL_3, FRAME_FILL_2, FRAME_FILL_1, FRAME_FILL_0)
|
||
|
|
||
|
/*
|
||
|
Usage:
|
||
|
ext = 1 (extended frame), 0 (standard frame)
|
||
|
bus = bus the CAN Message is sent
|
||
|
frame = reference to the user space CAN frame structure
|
||
|
id = CAN header ID
|
||
|
Optional bytes:
|
||
|
d1, ... ,d8 (payload)
|
||
|
*/
|
||
|
#define FRAME_FILL(ext,bus,frame,id,len, ...) \
|
||
|
memset(&frame,0,sizeof(frame)); \
|
||
|
frame.origin = bus; \
|
||
|
frame.MsgID = id; \
|
||
|
frame.FIR.B.FF = (ext ? CAN_frame_ext : CAN_frame_std); \
|
||
|
frame.FIR.B.DLC = len; \
|
||
|
FRAME_FILL_MACRO_CHOOSER(__VA_ARGS__)(frame ,##__VA_ARGS__)
|
||
|
|
||
|
// Convenience macros for initializing and sending standard and exteneded frame CAN messages
|
||
|
#define SEND_STD_FRAME(bus,frame,id,len, ...) \
|
||
|
{ \
|
||
|
FRAME_FILL(0, bus,frame,id,len, ##__VA_ARGS__) \
|
||
|
bus->Write(&frame); \
|
||
|
}
|
||
|
|
||
|
#define SEND_EXT_FRAME(bus,frame,id,len, ...) \
|
||
|
{ \
|
||
|
FRAME_FILL(1, bus,frame,id,len, ##__VA_ARGS__) \
|
||
|
bus->Write(&frame); \
|
||
|
}
|
||
|
|
||
|
// Helper functions for getting 1-4 byte chunks from CAN message payload
|
||
|
#define GET8(frame, pos) (frame->data.u8[pos])
|
||
|
#define GET16(frame, pos) ((frame->data.u8[pos] << 8) | frame->data.u8[pos+1])
|
||
|
#define GET24(frame, pos) ((frame->data.u8[pos] << 16) | (frame->data.u8[pos+1] << 8) | frame->data.u8[pos+2])
|
||
|
#define GET32(frame, pos) ((frame->data.u8[pos] << 24) | (frame->data.u8[pos+1] << 16) | (frame->data.u8[pos+2] << 8) | frame->data.u8[pos+3])
|
||
|
|
||
|
#endif //#ifndef __CAN_UTILS_H__
|