From 6bb6f9c792c6009fd631977cead326a69a184b3e Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Sun, 10 May 2020 17:30:22 -0400 Subject: [PATCH] Fix confusing math I fixed the math for getting the bytes for the Angles to make a bit easier to understand. I also clarfied my comments a bit to try and make them clearer with the explanation. --- .../io_procs/AHRSInputIOProcessor.cpp | 26 +++++++++++-------- 1 file changed, 15 insertions(+), 11 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 7aeccff..970d6af 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -120,11 +120,15 @@ void AHRSInputIOProcessor::loop() std::cerr << "Error occured while trying to read bytes." << std::endl; } - /* This gets the index of the bytes for the Euler angles in captured buffer. - * The 22 index of the message is where the last byte of the pitch angle is. - * It finds the first byte of the message and then finds where the data is. - * This allows me loop backward through each group of bytes since the order - * needs to be reversed because the AHRS is in Big Endian. + /* This gets the index of the bytes for the Euler angles in the captured buffer + * szBuff. The 14 index of the message is where the last byte of the roll angle is. + * It finds the first byte of the message and then finds where the data is. This + * allows me loop backward through each group of bytes since the order needs to + * be reversed because the AHRS is in Big Endian. + + * i is where the message starts in the buffer. + * index = i + 14, which is where the data we want starts (for Roll). + * To find Pitch and Yaw, we add 8 and 16 respectivly. * * MESSAGE STRUCTURE: * Roll Angle Bytes = RX, Pitch Angle Bytes = PX, Yaw Angle Bytes = YX @@ -134,8 +138,8 @@ void AHRSInputIOProcessor::loop() * | R1 R2 R3 R4 R5 R6 R7 R8 | P1 P2 P3 P4 P5 P6 P7 P8 | Y1 Y2 Y3 Y4 Y5 Y6 Y7 Y8 | * +-------------------------+-------------------------+-------------------------+ * ... | 7 8 9 10 11 12 13 14 | 15 16 17 18 19 20 21 22 | 23 24 25 26 27 28 29 30 | ... - * | | ^ | | - * | | Index| | + * | ^ | ^ | ^ | + * | index| index + 8| index + 16| * +-------------------------+-------------------------+-------------------------+ */ int index = -1; @@ -143,7 +147,7 @@ void AHRSInputIOProcessor::loop() { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { - index = i + 22; + index = i + 14; break; } } @@ -156,9 +160,9 @@ void AHRSInputIOProcessor::loop() { for (int i = 0; i < 8; i++) { - rollArray[i] = szBuff[index - (i + 8)]; - pitchArray[i] = szBuff[index - i]; - yawArray[i] = szBuff[index + (8 - i)]; + rollArray[i] = szBuff[index - i]; + pitchArray[i] = szBuff[index - i + 8]; + yawArray[i] = szBuff[index - i + 16]; } }