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@79:1910ae03cb2e, 22 months ago (annotated)
- Committer:
- JamieB
- Date:
- Thu Dec 15 05:53:28 2022 +0000
- Revision:
- 79:1910ae03cb2e
- Parent:
- 47:8d577dfb7eb1
Working State RAW IMU output in message
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
AndyA | 8:961bb15570a1 | 1 | #include "position.h" |
AndyA | 10:053bac3e326b | 2 | #include "LTCApp.h" |
AndyA | 0:97661408d0f9 | 3 | #include <cstring> |
AndyA | 8:961bb15570a1 | 4 | |
AndyA | 8:961bb15570a1 | 5 | position::position() |
AndyA | 8:961bb15570a1 | 6 | { |
AndyA | 0:97661408d0f9 | 7 | } |
AndyA | 0:97661408d0f9 | 8 | |
AndyA | 10:053bac3e326b | 9 | float position::interpAngle(float value1, float value2, float gradientWeight) |
AndyA | 8:961bb15570a1 | 10 | { |
AndyA | 10:053bac3e326b | 11 | |
AndyA | 0:97661408d0f9 | 12 | float delta = (value1-value2); |
AndyA | 0:97661408d0f9 | 13 | if ((delta >= -180) && (delta <= 180)) // no wraparound |
AndyA | 10:053bac3e326b | 14 | return delta*gradientWeight + value1; |
AndyA | 8:961bb15570a1 | 15 | |
AndyA | 8:961bb15570a1 | 16 | if (delta < -180) { // val2 far larger than val1 |
AndyA | 10:053bac3e326b | 17 | float result = (delta+360)*gradientWeight + value1; |
AndyA | 0:97661408d0f9 | 18 | if (result >= 360) |
AndyA | 0:97661408d0f9 | 19 | return result-360; |
AndyA | 0:97661408d0f9 | 20 | return result; |
AndyA | 0:97661408d0f9 | 21 | } else { |
AndyA | 10:053bac3e326b | 22 | float result = (delta-360)*gradientWeight + value1; |
AndyA | 0:97661408d0f9 | 23 | if (result < 0) |
AndyA | 0:97661408d0f9 | 24 | return result+360; |
AndyA | 0:97661408d0f9 | 25 | return result; |
AndyA | 8:961bb15570a1 | 26 | } |
AndyA | 0:97661408d0f9 | 27 | } |
AndyA | 0:97661408d0f9 | 28 | |
AndyA | 8:961bb15570a1 | 29 | bool position::interp(position* output, position *pos1, position *pos2) |
AndyA | 8:961bb15570a1 | 30 | { |
AndyA | 8:961bb15570a1 | 31 | if (pos1->ID != pos2->ID) |
AndyA | 8:961bb15570a1 | 32 | return false; |
AndyA | 8:961bb15570a1 | 33 | |
AndyA | 8:961bb15570a1 | 34 | if (output==NULL) { |
AndyA | 8:961bb15570a1 | 35 | printf(" BO "); |
AndyA | 8:961bb15570a1 | 36 | return false; |
AndyA | 8:961bb15570a1 | 37 | } |
AndyA | 8:961bb15570a1 | 38 | if (pos1==NULL) { |
AndyA | 8:961bb15570a1 | 39 | printf(" B1 "); |
AndyA | 8:961bb15570a1 | 40 | return false; |
AndyA | 8:961bb15570a1 | 41 | } |
AndyA | 8:961bb15570a1 | 42 | if (pos2==NULL) { |
AndyA | 8:961bb15570a1 | 43 | printf(" B2 "); |
AndyA | 8:961bb15570a1 | 44 | return false; |
AndyA | 8:961bb15570a1 | 45 | } |
AndyA | 8:961bb15570a1 | 46 | |
AndyA | 8:961bb15570a1 | 47 | if (output->time == pos1->time) { |
AndyA | 8:961bb15570a1 | 48 | memcpy(output,pos1,sizeof(position)); |
AndyA | 8:961bb15570a1 | 49 | return true; |
AndyA | 8:961bb15570a1 | 50 | } |
AndyA | 2:a79201e302d7 | 51 | |
AndyA | 8:961bb15570a1 | 52 | if (output->time == pos2->time) { |
AndyA | 8:961bb15570a1 | 53 | memcpy(output,pos2,sizeof(position)); |
AndyA | 8:961bb15570a1 | 54 | return true; |
AndyA | 8:961bb15570a1 | 55 | } |
AndyA | 8:961bb15570a1 | 56 | |
AndyA | 8:961bb15570a1 | 57 | if (pos1->time == pos2->time) { |
AndyA | 8:961bb15570a1 | 58 | return false; |
AndyA | 8:961bb15570a1 | 59 | } |
AndyA | 0:97661408d0f9 | 60 | |
AndyA | 13:c2d9e975841b | 61 | float GradientWeight = ((int)(output->time-pos1->time))/(float)((int)(pos1->time - pos2->time)); |
AndyA | 10:053bac3e326b | 62 | |
AndyA | 10:053bac3e326b | 63 | // pc.printf("%lu %lu %lu %.3f\r\n",pos1->time,pos2->time,output->time,GradientWeight); |
AndyA | 10:053bac3e326b | 64 | |
AndyA | 10:053bac3e326b | 65 | output->X = (pos1->X-pos2->X)*GradientWeight + pos1->X; |
AndyA | 10:053bac3e326b | 66 | output->Y = (pos1->Y-pos2->Y)*GradientWeight + pos1->Y; |
AndyA | 10:053bac3e326b | 67 | output->Height = (pos1->Height-pos2->Height)*GradientWeight + pos1->Height; |
AndyA | 10:053bac3e326b | 68 | output->roll = position::interpAngle(pos1->roll,pos2->roll,GradientWeight); |
AndyA | 10:053bac3e326b | 69 | output->pitch = position::interpAngle(pos1->pitch,pos2->pitch,GradientWeight); |
AndyA | 10:053bac3e326b | 70 | output->yaw =position::interpAngle(pos1->yaw,pos2->yaw,GradientWeight); |
AndyA | 8:961bb15570a1 | 71 | output->ID = pos1->ID; |
AndyA | 10:053bac3e326b | 72 | output->focus = pos1->focus; |
AndyA | 10:053bac3e326b | 73 | output->iris = pos1->iris; |
AndyA | 10:053bac3e326b | 74 | output->zoom = pos1->zoom; |
JamieB | 79:1910ae03cb2e | 75 | output->x_accel = pos1->x_accel; |
JamieB | 79:1910ae03cb2e | 76 | output->y_accel = pos1->y_accel; |
JamieB | 79:1910ae03cb2e | 77 | output->z_accel = pos1->z_accel; |
JamieB | 79:1910ae03cb2e | 78 | output->x_gyro = pos1->x_gyro; |
JamieB | 79:1910ae03cb2e | 79 | output->y_gyro = pos1->y_gyro; |
JamieB | 79:1910ae03cb2e | 80 | output->z_gyro = pos1->z_gyro; |
AndyA | 8:961bb15570a1 | 81 | |
AndyA | 10:053bac3e326b | 82 | output->beacons = pos1->beacons; |
AndyA | 10:053bac3e326b | 83 | output->solutionType = pos1->solutionType; |
AndyA | 10:053bac3e326b | 84 | output->KFStatus = pos1->KFStatus; |
AndyA | 47:8d577dfb7eb1 | 85 | output->UsedBeaconsValid = pos1->UsedBeaconsValid; |
AndyA | 47:8d577dfb7eb1 | 86 | if (output->UsedBeaconsValid) |
AndyA | 47:8d577dfb7eb1 | 87 | memcpy(output->UsedBeacons,pos1->UsedBeacons,12); |
AndyA | 8:961bb15570a1 | 88 | |
AndyA | 0:97661408d0f9 | 89 | return true; |
AndyA | 0:97661408d0f9 | 90 | } |
AndyA | 0:97661408d0f9 | 91 |