Racelogic / Mbed 2 deprecated VIPS_LTC_RAW_IMU

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@85:0cc5931bb9ef, 17 months ago (annotated)

Committer:
JamieB
Date:
Thu Dec 15 06:05:30 2022 +0000
Revision:
85:0cc5931bb9ef
Parent:
79:1910ae03cb2e 
Push to somewhere else due to merge issue

Who changed what in which revision?

UserRevisionLine numberNew 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