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@8:961bb15570a1, 2021-02-16 (annotated)

Committer:
AndyA
Date:
Tue Feb 16 09:53:54 2021 +0000
Revision:
8:961bb15570a1
Parent:
3:14d241e29be3
Child:
10:053bac3e326b
Added support for status block in VIPS data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AndyA 8:961bb15570a1 1 #include "position.h"
AndyA 0:97661408d0f9 2 #include <cstring>
AndyA 8:961bb15570a1 3
AndyA 8:961bb15570a1 4 position::position()
AndyA 8:961bb15570a1 5 {
AndyA 0:97661408d0f9 6 }
AndyA 0:97661408d0f9 7
AndyA 8:961bb15570a1 8 float position::interpAngle(float value1, float weight1, float value2, float weight2)
AndyA 8:961bb15570a1 9 {
AndyA 0:97661408d0f9 10 float delta = (value1-value2);
AndyA 0:97661408d0f9 11 if ((delta >= -180) && (delta <= 180)) // no wraparound
AndyA 0:97661408d0f9 12 return value1*weight1 + value2*weight2;
AndyA 8:961bb15570a1 13
AndyA 8:961bb15570a1 14 if (delta < -180) { // val2 far larger than val1
AndyA 0:97661408d0f9 15 float result = (value1+360)*weight1 + value2*weight2;
AndyA 0:97661408d0f9 16 if (result >= 360)
AndyA 0:97661408d0f9 17 return result-360;
AndyA 0:97661408d0f9 18 return result;
AndyA 0:97661408d0f9 19 } else {
AndyA 0:97661408d0f9 20 float result = (value1-360)*weight1 + value2*weight2;
AndyA 0:97661408d0f9 21 if (result < 0)
AndyA 0:97661408d0f9 22 return result+360;
AndyA 0:97661408d0f9 23 return result;
AndyA 8:961bb15570a1 24 }
AndyA 0:97661408d0f9 25 }
AndyA 0:97661408d0f9 26
AndyA 8:961bb15570a1 27 bool position::interp(position* output, position *pos1, position *pos2)
AndyA 8:961bb15570a1 28 {
AndyA 8:961bb15570a1 29 if (pos1->ID != pos2->ID)
AndyA 8:961bb15570a1 30 return false;
AndyA 8:961bb15570a1 31
AndyA 8:961bb15570a1 32 if (output==NULL) {
AndyA 8:961bb15570a1 33 printf(" BO ");
AndyA 8:961bb15570a1 34 return false;
AndyA 8:961bb15570a1 35 }
AndyA 8:961bb15570a1 36 if (pos1==NULL) {
AndyA 8:961bb15570a1 37 printf(" B1 ");
AndyA 8:961bb15570a1 38 return false;
AndyA 8:961bb15570a1 39 }
AndyA 8:961bb15570a1 40 if (pos2==NULL) {
AndyA 8:961bb15570a1 41 printf(" B2 ");
AndyA 8:961bb15570a1 42 return false;
AndyA 8:961bb15570a1 43 }
AndyA 8:961bb15570a1 44
AndyA 8:961bb15570a1 45 if (output->time == pos1->time) {
AndyA 8:961bb15570a1 46 memcpy(output,pos1,sizeof(position));
AndyA 8:961bb15570a1 47 printf(" C1 ");
AndyA 8:961bb15570a1 48 return true;
AndyA 8:961bb15570a1 49 }
AndyA 2:a79201e302d7 50
AndyA 8:961bb15570a1 51 if (output->time == pos2->time) {
AndyA 8:961bb15570a1 52 memcpy(output,pos2,sizeof(position));
AndyA 8:961bb15570a1 53 printf(" C2 ");
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 printf(" E %ld",pos1->time);
AndyA 8:961bb15570a1 59 return false;
AndyA 8:961bb15570a1 60 }
AndyA 0:97661408d0f9 61
AndyA 8:961bb15570a1 62 float pos2Weight = (pos1->time - pos2->time)/(float)(pos1->time - output->time);
AndyA 8:961bb15570a1 63 float pos1Weight = 1-pos2Weight;
AndyA 8:961bb15570a1 64 output->X = pos1->X*pos1Weight + pos2->X*pos2Weight;
AndyA 8:961bb15570a1 65 output->Y = pos1->Y*pos1Weight + pos2->Y*pos2Weight;
AndyA 8:961bb15570a1 66 output->Height = pos1->Height*pos1Weight + pos2->Height*pos2Weight;
AndyA 8:961bb15570a1 67 output->roll = position::interpAngle(pos1->roll,pos1Weight,pos2->roll,pos2Weight);
AndyA 8:961bb15570a1 68 output->pitch = position::interpAngle(pos1->pitch,pos1Weight,pos2->pitch,pos2Weight);
AndyA 8:961bb15570a1 69 output->yaw =position::interpAngle(pos1->yaw,pos1Weight,pos2->yaw,pos2Weight);
AndyA 8:961bb15570a1 70 output->ID = pos1->ID;
AndyA 8:961bb15570a1 71 output->focus = pos1->focus*pos1Weight + pos2->focus*pos2Weight +0.5f;
AndyA 8:961bb15570a1 72 output->iris = pos1->iris*pos1Weight + pos2->iris*pos2Weight +0.5f;
AndyA 8:961bb15570a1 73 output->zoom = pos1->zoom*pos1Weight + pos2->zoom*pos2Weight +0.5f;
AndyA 8:961bb15570a1 74
AndyA 8:961bb15570a1 75 if (pos1Weight>pos2Weight) {
AndyA 8:961bb15570a1 76 output->beacons = pos1->beacons;
AndyA 8:961bb15570a1 77 output->solutionType = pos1->solutionType;
AndyA 8:961bb15570a1 78 output->KFStatus = pos1->KFStatus;
AndyA 8:961bb15570a1 79 } else {
AndyA 8:961bb15570a1 80 output->beacons = pos2->beacons;
AndyA 8:961bb15570a1 81 output->solutionType = pos2->solutionType;
AndyA 8:961bb15570a1 82 output->KFStatus = pos2->KFStatus;
AndyA 8:961bb15570a1 83 }
AndyA 8:961bb15570a1 84
AndyA 0:97661408d0f9 85 return true;
AndyA 0:97661408d0f9 86 }
AndyA 0:97661408d0f9 87