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@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?

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