From 4f3657494a09d6511dfbdee47d08cf4843a5a552 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 28 Nov 2022 18:35:22 +0000 Subject: [PATCH] CPM: Object data0 format handling --- src/cpm.c | 98 ++++++++++++++++++++++++++++++------------------------- 1 file changed, 54 insertions(+), 44 deletions(-) diff --git a/src/cpm.c b/src/cpm.c index c30cbba..6e94048 100644 --- a/src/cpm.c +++ b/src/cpm.c @@ -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"); } } }