I messed up the merge, so pushing it over to another repo so I don't lose it. Will tidy up and remove later
Dependencies: BufferedSerial FatFileSystemCpp mbed
position.cpp@2:a79201e302d7, 2021-01-18 (annotated)
- Committer:
- AndyA
- Date:
- Mon Jan 18 09:15:04 2021 +0000
- Revision:
- 2:a79201e302d7
- Parent:
- 0:97661408d0f9
- Child:
- 3:14d241e29be3
Added ID
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AndyA | 0:97661408d0f9 | 1 | #include "position.h" |
AndyA | 0:97661408d0f9 | 2 | #include <cstring> |
AndyA | 0:97661408d0f9 | 3 | |
AndyA | 0:97661408d0f9 | 4 | position::position() { |
AndyA | 0:97661408d0f9 | 5 | } |
AndyA | 0:97661408d0f9 | 6 | |
AndyA | 0:97661408d0f9 | 7 | float position::interpAngle(float value1, float weight1, float value2, float weight2) { |
AndyA | 0:97661408d0f9 | 8 | float delta = (value1-value2); |
AndyA | 0:97661408d0f9 | 9 | if ((delta >= -180) && (delta <= 180)) // no wraparound |
AndyA | 0:97661408d0f9 | 10 | return value1*weight1 + value2*weight2; |
AndyA | 0:97661408d0f9 | 11 | |
AndyA | 0:97661408d0f9 | 12 | if (delta < -180) { // val2 far larger than val1 |
AndyA | 0:97661408d0f9 | 13 | float result = (value1+360)*weight1 + value2*weight2; |
AndyA | 0:97661408d0f9 | 14 | if (result >= 360) |
AndyA | 0:97661408d0f9 | 15 | return result-360; |
AndyA | 0:97661408d0f9 | 16 | return result; |
AndyA | 0:97661408d0f9 | 17 | } else { |
AndyA | 0:97661408d0f9 | 18 | float result = (value1-360)*weight1 + value2*weight2; |
AndyA | 0:97661408d0f9 | 19 | if (result < 0) |
AndyA | 0:97661408d0f9 | 20 | return result+360; |
AndyA | 0:97661408d0f9 | 21 | return result; |
AndyA | 0:97661408d0f9 | 22 | } |
AndyA | 0:97661408d0f9 | 23 | } |
AndyA | 0:97661408d0f9 | 24 | |
AndyA | 0:97661408d0f9 | 25 | bool position::interp(position* output, position *pos1, position *pos2) { |
AndyA | 2:a79201e302d7 | 26 | if (pos1->ID != pos2->ID) |
AndyA | 2:a79201e302d7 | 27 | return false; |
AndyA | 2:a79201e302d7 | 28 | |
AndyA | 0:97661408d0f9 | 29 | if (output==NULL) { |
AndyA | 0:97661408d0f9 | 30 | printf(" BO "); |
AndyA | 0:97661408d0f9 | 31 | return false; |
AndyA | 0:97661408d0f9 | 32 | } |
AndyA | 0:97661408d0f9 | 33 | if (pos1==NULL) { |
AndyA | 0:97661408d0f9 | 34 | printf(" B1 "); |
AndyA | 0:97661408d0f9 | 35 | return false; |
AndyA | 0:97661408d0f9 | 36 | } |
AndyA | 0:97661408d0f9 | 37 | if (pos2==NULL) { |
AndyA | 0:97661408d0f9 | 38 | printf(" B2 "); |
AndyA | 0:97661408d0f9 | 39 | return false; |
AndyA | 0:97661408d0f9 | 40 | } |
AndyA | 0:97661408d0f9 | 41 | |
AndyA | 0:97661408d0f9 | 42 | if (output->time == pos1->time) { |
AndyA | 0:97661408d0f9 | 43 | memcpy(output,pos1,sizeof(position)); |
AndyA | 0:97661408d0f9 | 44 | printf(" C1 "); |
AndyA | 0:97661408d0f9 | 45 | return true; |
AndyA | 0:97661408d0f9 | 46 | } |
AndyA | 0:97661408d0f9 | 47 | |
AndyA | 0:97661408d0f9 | 48 | if (output->time == pos2->time) { |
AndyA | 0:97661408d0f9 | 49 | memcpy(output,pos2,sizeof(position)); |
AndyA | 0:97661408d0f9 | 50 | printf(" C2 "); |
AndyA | 0:97661408d0f9 | 51 | return true; |
AndyA | 0:97661408d0f9 | 52 | } |
AndyA | 0:97661408d0f9 | 53 | |
AndyA | 0:97661408d0f9 | 54 | if (pos1->time == pos2->time) { |
AndyA | 0:97661408d0f9 | 55 | printf(" E %ld",pos1->time); |
AndyA | 0:97661408d0f9 | 56 | return false; |
AndyA | 0:97661408d0f9 | 57 | } |
AndyA | 0:97661408d0f9 | 58 | |
AndyA | 0:97661408d0f9 | 59 | float pos2Weight = (pos1->time - pos2->time)/(float)(pos1->time - output->time); |
AndyA | 0:97661408d0f9 | 60 | float pos1Weight = 1-pos2Weight; |
AndyA | 0:97661408d0f9 | 61 | output->X = pos1->X*pos1Weight + pos2->X*pos2Weight; |
AndyA | 0:97661408d0f9 | 62 | output->Y = pos1->Y*pos1Weight + pos2->Y*pos2Weight; |
AndyA | 0:97661408d0f9 | 63 | output->Height = pos1->Height*pos1Weight + pos2->Height*pos2Weight; |
AndyA | 0:97661408d0f9 | 64 | output->roll = position::interpAngle(pos1->roll,pos1Weight,pos2->roll,pos2Weight); |
AndyA | 0:97661408d0f9 | 65 | output->pitch = position::interpAngle(pos1->pitch,pos1Weight,pos2->pitch,pos2Weight); |
AndyA | 0:97661408d0f9 | 66 | output->yaw =position::interpAngle(pos1->yaw,pos1Weight,pos2->yaw,pos2Weight); |
AndyA | 2:a79201e302d7 | 67 | output->ID = pos1->ID; |
AndyA | 2:a79201e302d7 | 68 | |
AndyA | 0:97661408d0f9 | 69 | return true; |
AndyA | 0:97661408d0f9 | 70 | } |
AndyA | 0:97661408d0f9 | 71 |