From 6c3787288e14bc5541e823c7880f219a64789560 Mon Sep 17 00:00:00 2001 From: Matt Scalzo Date: Thu, 14 May 2020 19:43:19 -0400 Subject: [PATCH] Bug with how I reimplemented Arithmatic (#20) * Add AHRS IO Proc files into newer pull from dev * Fix names and built with python script * Fix pull request comments * Make edits for the pull request There is a problem that may be cause by these changes, that being not closing the handle, since the loop will run forever, I would have to close the loop on some sort of exit condition. I am not sure if the handle closes if the process is killed, if so, then I guess it should be fine. * Untabify AHRSInputIOProcessor.cpp * Hmmm * Revert "Hmmm" This reverts commit 5a9cb089a1776d449f8ef04eec6f8d10589bbea5. * Add functionality to get roll and pitch angles This reverts commit e2ea108b7b70deb8aa88b4c6dcdfd3edac5db2cf. * Change order of #includes fixed build errors... I have no idea why exactly but the build errors only got fixed when I changed the order of the includes in AHRSInputIOProcessor.hpp. * Add Comments Explaining Byte Capture I added some comments explaining how I am capturing the bytes of the Euler Angles from the message in the buffer. Quick version here, I first capute a 2 times the size of the message into a buffer, I then search for the begining of the message, when I find it, I then jump to the index where the Angles are stored. I then add each group of 8 bytes for each angle backwords into arrays to then be converted. I add them in backwords since the AHRS is big endian and the computer we are converting them to doubles on is little endian. After each angle's 8 bytes are storred in an array, I then cast them into doubles, where they can then be sent. Hopefully that makes sense, feel free to ask me any questions about it. * 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. * Fix Comments More I further clarified my comments and also renamed index to dataStart to make it more clear what it was. * Rename Attribute Keys * Fix type in comments Changed "index" to "dataStart" in diagram to make it more clear. * Add more specifics to Config message comments * Change arround data byte capture math Changed the for loop to loop backwords itself so that the equations for actually getting the bytes only use +'s. They are still basically the same equations, but hopefully they are a little more intuitive... * Revert "Add more specifics to Config message comments" This reverts commit 13c66ec7331c46fc40c8e3605e354cb8d3ff7827. * Revert "Change arround data byte capture math" This reverts commit 99886151af9334ff1805b008da323f9e2966041f. * Revert "Revert "Add more specifics to Config message comments"" This reverts commit f50c2f10127ccd76ca0808be4bfc343d2eba1e90. * Revert "Revert "Change arround data byte capture math"" This reverts commit 596b9af0a6a24065ad0d5919ca6be2964194ca71. * Revert "Revert "Revert "Change arround data byte capture math""" This reverts commit 361589dd38487f3d5d4c38f5f417ea351935dbf4. * Revert changes with arithmetic When I changed the arithmetic around it messed up the indexing of the byte arrays. The reason i went up(forward) was so that I could put it in the array, not to do some funky math. If we don't revert these changes the byte order will simply be wrong. --- .../io_procs/AHRSInputIOProcessor.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp index 4d06a36..69bb90d 100644 --- a/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp +++ b/bfs/implementations/breadcrumbs/io_procs/AHRSInputIOProcessor.cpp @@ -127,13 +127,13 @@ void AHRSInputIOProcessor::loop() /* This gets the index of the bytes for the Euler angles in the captured * buffer (szBuff). First, it finds the first byte of the message (i). Then, - * it finds the 7th index of the message (dataStart). This is where the first + * it finds the 14th index of the message (dataStart). This is where the last * byte of the roll angle 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. - * dataStart = i + 7, which is where the data we want starts (for Roll). + * dataStart = 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: @@ -144,8 +144,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 | ... - * | ^ | ^ | ^ | - * |dataStart |dataStart + 8 |dataStart + 16 | + * | ^ | ^ | ^ | + * | dataStart| dataStart + 8| dataStart + 16| * +-------------------------+-------------------------+-------------------------+ */ int dataStart = -1; @@ -153,7 +153,7 @@ void AHRSInputIOProcessor::loop() { if (szBuff[i] == 0xFA && szBuff[i+1] == 0xFF && szBuff[i+2] == 0x36) { - dataStart = i + 7; + dataStart = i + 14; break; } } @@ -164,11 +164,11 @@ void AHRSInputIOProcessor::loop() // Puts the bytes for each angle in their own array backwards, since they are sent in big endian if (dataStart != -1) { - for (int i = 7; i > -1; i--) + for (int i = 0; i < 8; i++) { - rollArray[i] = szBuff[dataStart + i]; - pitchArray[i] = szBuff[dataStart + i + 8]; - yawArray[i] = szBuff[dataStart + i + 16]; + rollArray[i] = szBuff[dataStart - i]; + pitchArray[i] = szBuff[dataStart - i + 8]; + yawArray[i] = szBuff[dataStart - i + 16]; } }