TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

CPM: Object data0 format handling

This commit is contained in:
emanuel 2022-11-28 18:35:22 +00:00
parent 0a1e33c5af
commit 4f3657494a
1 changed files with 54 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
src/cpm.c
View File

@ -285,60 +285,70 @@ void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_
if ((u32_can_id >= 0x502) && (u32_can_id <= 0x57F)) {
u_int16_t u16_objectIndex = (u_int16_t)u32_can_id - 0x502;
as_objects[u16_objectIndex].u8_modeSignal = au8_can_data[0] & 0x1;
if (s_objectControl.u8_objectData0Format == 005) // without Update Flag
{
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
switch (s_objectControl.u8_objectData0Format) {
/* data without updated flag */
case 3:
case 5: /* with 7 classes */
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024.0f;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
} else if (s_objectControl.u8_objectData0Format == 4) {
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
break;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
/* data with updated flag */
case 4:
case 6: /* with 7 classes */
as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
as_objects[u16_objectIndex].f_xPoint -= 4096;
as_objects[u16_objectIndex].f_xPoint *= 0.128;
as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
as_objects[u16_objectIndex].f_yPoint -= 4096;
as_objects[u16_objectIndex].f_yPoint *= 0.128;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024.0f;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5);
as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
as_objects[u16_objectIndex].f_xSpeed -= 1024;
as_objects[u16_objectIndex].f_xSpeed *= 0.1;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2f;
as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
as_objects[u16_objectIndex].f_ySpeed -= 1024;
as_objects[u16_objectIndex].f_ySpeed *= 0.1;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7;
as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2;
as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7;
break;
default:
log_warn("[cp] unhandled object data0 format");
}
}
}