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@0:97661408d0f9, 2021-01-15 (annotated)

Committer:
AndyA
Date:
Fri Jan 15 11:49:01 2021 +0000
Revision:
0:97661408d0f9
Child:
2:a79201e302d7
first;

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 0:97661408d0f9 26 if (output==NULL) {
AndyA 0:97661408d0f9 27 printf(" BO ");
AndyA 0:97661408d0f9 28 return false;
AndyA 0:97661408d0f9 29 }
AndyA 0:97661408d0f9 30 if (pos1==NULL) {
AndyA 0:97661408d0f9 31 printf(" B1 ");
AndyA 0:97661408d0f9 32 return false;
AndyA 0:97661408d0f9 33 }
AndyA 0:97661408d0f9 34 if (pos2==NULL) {
AndyA 0:97661408d0f9 35 printf(" B2 ");
AndyA 0:97661408d0f9 36 return false;
AndyA 0:97661408d0f9 37 }
AndyA 0:97661408d0f9 38
AndyA 0:97661408d0f9 39 if (output->time == pos1->time) {
AndyA 0:97661408d0f9 40 memcpy(output,pos1,sizeof(position));
AndyA 0:97661408d0f9 41 printf(" C1 ");
AndyA 0:97661408d0f9 42 return true;
AndyA 0:97661408d0f9 43 }
AndyA 0:97661408d0f9 44
AndyA 0:97661408d0f9 45 if (output->time == pos2->time) {
AndyA 0:97661408d0f9 46 memcpy(output,pos2,sizeof(position));
AndyA 0:97661408d0f9 47 printf(" C2 ");
AndyA 0:97661408d0f9 48 return true;
AndyA 0:97661408d0f9 49 }
AndyA 0:97661408d0f9 50
AndyA 0:97661408d0f9 51 if (pos1->time == pos2->time) {
AndyA 0:97661408d0f9 52 printf(" E %ld",pos1->time);
AndyA 0:97661408d0f9 53 return false;
AndyA 0:97661408d0f9 54 }
AndyA 0:97661408d0f9 55
AndyA 0:97661408d0f9 56 float pos2Weight = (pos1->time - pos2->time)/(float)(pos1->time - output->time);
AndyA 0:97661408d0f9 57 float pos1Weight = 1-pos2Weight;
AndyA 0:97661408d0f9 58 output->X = pos1->X*pos1Weight + pos2->X*pos2Weight;
AndyA 0:97661408d0f9 59 output->Y = pos1->Y*pos1Weight + pos2->Y*pos2Weight;
AndyA 0:97661408d0f9 60 output->Height = pos1->Height*pos1Weight + pos2->Height*pos2Weight;
AndyA 0:97661408d0f9 61 output->roll = position::interpAngle(pos1->roll,pos1Weight,pos2->roll,pos2Weight);
AndyA 0:97661408d0f9 62 output->pitch = position::interpAngle(pos1->pitch,pos1Weight,pos2->pitch,pos2Weight);
AndyA 0:97661408d0f9 63 output->yaw =position::interpAngle(pos1->yaw,pos1Weight,pos2->yaw,pos2Weight);
AndyA 0:97661408d0f9 64 return true;
AndyA 0:97661408d0f9 65 }
AndyA 0:97661408d0f9 66