rusefi
/
custom-board-bundle-sample-fork-of-rusefi
mirror of https://github.com/rusefi/custom-board-bundle-sample-fork-of-rusefi.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

imu: add support for Mercedes pn: A 006 542 26 18 (#3902)

This commit is contained in:
Andrey G 2022-02-09 00:29:01 +03:00 committed by GitHub
parent c5a71ed66b
commit 76e3622ce6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 51 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
firmware/controllers/can/can_rx.cpp
View File

@ -100,13 +100,28 @@ void registerCanSensor(CanSensorBase& sensor) {
sensor.Register(); sensor.Register();
} }
/*
* TODO:
* - convert to CanListener
* - move to hw_layer/sensors/yaw_rate_sensor.cpp | accelerometer.cpp ?
*/
#define VAG_YAW_RATE_G_LAT 0x130 #define VAG_YAW_RATE_G_LAT 0x130
#define VAG_YAW_ACCEL_G_LONG 0x131 #define VAG_YAW_ACCEL_G_LONG 0x131
/* Bosch Acceleration Sensor MM5.10 quantizations */
#define MM5_10_RATE_QUANT 0.005
#define MM5_10_ACC_QUANT 0.0001274
/* Bosch Acceleration Sensor MM5.10 CAN IDs */
#define MM5_10_YAW_Y 0x174 #define MM5_10_YAW_Y 0x174
#define MM5_10_ROLL_X 0x178 #define MM5_10_ROLL_X 0x178
#define MM5_10_Z 0x17C #define MM5_10_Z 0x17C
/* Mercedes pn: A 006 542 26 18 CAN IDs */
#define MM5_10_MB_YAW_Y_CANID 0x150
#define MM5_10_MB_ROLL_X_CANID 0x151
static uint16_t getLSB_intel(const CANRxFrame& frame, int offset) { static uint16_t getLSB_intel(const CANRxFrame& frame, int offset) {
return (frame.data8[offset + 1] << 8) + frame.data8[offset]; return (frame.data8[offset + 1] << 8) + frame.data8[offset];
} }
@ -115,6 +130,30 @@ static int16_t getShiftedLSB_intel(const CANRxFrame& frame, int offset) {
return getLSB_intel(frame, offset) - 0x8000; return getLSB_intel(frame, offset) - 0x8000;
} }
static void processCanRxImu_BoschM5_10_YawY(const CANRxFrame& frame) {
float yaw = getShiftedLSB_intel(frame, 0);
float accY = getShiftedLSB_intel(frame, 4);
efiPrintf("CAN_rx MM5_10_YAW_Y %f %f", yaw, accY);
engine->sensors.accelerometer.yaw = yaw * MM5_10_RATE_QUANT;
engine->sensors.accelerometer.y = accY * MM5_10_ACC_QUANT;
}
static void processCanRxImu_BoschM5_10_RollX(const CANRxFrame& frame) {
float roll = getShiftedLSB_intel(frame, 0);
float accX = getShiftedLSB_intel(frame, 4);
efiPrintf("CAN_rx MM5_10_ROLL_X %f %f", roll, accX);
engine->sensors.accelerometer.roll = roll * MM5_10_RATE_QUANT;
engine->sensors.accelerometer.x = accX * MM5_10_ACC_QUANT;
}
static void processCanRxImu_BoschM5_10_Z(const CANRxFrame& frame) {
float accZ = getShiftedLSB_intel(frame, 4);
efiPrintf("CAN_rx MM5_10_Z %f", accZ);
engine->sensors.accelerometer.z = accZ * MM5_10_ACC_QUANT;
}
static void processCanRxImu(const CANRxFrame& frame) { static void processCanRxImu(const CANRxFrame& frame) {
/* /*
if (CAN_SID(frame) == 0x130) { if (CAN_SID(frame) == 0x130) {
@ -132,30 +171,19 @@ static void processCanRxImu(const CANRxFrame& frame) {
} }
*/ */
if (engineConfiguration->imuType == IMU_MM5_10) { if (engineConfiguration->imuType == IMU_MM5_10) {
#define MM5_10_RATE_QUANT 0.005
#define MM5_10_ACC_QUANT 0.0001274
if (CAN_SID(frame) == MM5_10_YAW_Y) { if (CAN_SID(frame) == MM5_10_YAW_Y) {
float yaw = getShiftedLSB_intel(frame, 0); processCanRxImu_BoschM5_10_YawY(frame);
float accY = getShiftedLSB_intel(frame, 4);
efiPrintf("CAN_rx MM5_10_YAW_Y %f %f", yaw, accY);
engine->sensors.accelerometer.yaw = yaw * MM5_10_RATE_QUANT;
engine->sensors.accelerometer.y = accY * MM5_10_ACC_QUANT;
} else if (CAN_SID(frame) == MM5_10_ROLL_X) { } else if (CAN_SID(frame) == MM5_10_ROLL_X) {
float roll = getShiftedLSB_intel(frame, 0); processCanRxImu_BoschM5_10_RollX(frame);
float accX = getShiftedLSB_intel(frame, 4);
efiPrintf("CAN_rx MM5_10_ROLL_X %f %f", roll, accX);
engine->sensors.accelerometer.roll = roll * MM5_10_RATE_QUANT;
engine->sensors.accelerometer.x = accX * MM5_10_ACC_QUANT;
} else if (CAN_SID(frame) == MM5_10_Z) { } else if (CAN_SID(frame) == MM5_10_Z) {
float accZ = getShiftedLSB_intel(frame, 4); processCanRxImu_BoschM5_10_Z(frame);
efiPrintf("CAN_rx MM5_10_Z %f", accZ); }
engine->sensors.accelerometer.z = accZ * MM5_10_ACC_QUANT; } else if (engineConfiguration->imuType == IMU_TYPE_MB_A0065422618) {
if (CAN_SID(frame) == MM5_10_MB_YAW_Y_CANID) {
processCanRxImu_BoschM5_10_YawY(frame);
} else if (CAN_SID(frame) == MM5_10_MB_ROLL_X_CANID) {
processCanRxImu_BoschM5_10_RollX(frame);
} }
} }
} }