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
- Committer:
- AndyA
- Date:
- 2021-08-17
- Revision:
- 47:8d577dfb7eb1
- Parent:
- 44:fe7fcf62c75d
- Child:
- 79:1910ae03cb2e
File content as of revision 47:8d577dfb7eb1:
#include "position.h" #include "LTCApp.h" #include <cstring> position::position() { } float position::interpAngle(float value1, float value2, float gradientWeight) { float delta = (value1-value2); if ((delta >= -180) && (delta <= 180)) // no wraparound return delta*gradientWeight + value1; if (delta < -180) { // val2 far larger than val1 float result = (delta+360)*gradientWeight + value1; if (result >= 360) return result-360; return result; } else { float result = (delta-360)*gradientWeight + value1; if (result < 0) return result+360; return result; } } bool position::interp(position* output, position *pos1, position *pos2) { if (pos1->ID != pos2->ID) return false; if (output==NULL) { printf(" BO "); return false; } if (pos1==NULL) { printf(" B1 "); return false; } if (pos2==NULL) { printf(" B2 "); return false; } if (output->time == pos1->time) { memcpy(output,pos1,sizeof(position)); return true; } if (output->time == pos2->time) { memcpy(output,pos2,sizeof(position)); return true; } if (pos1->time == pos2->time) { return false; } float GradientWeight = ((int)(output->time-pos1->time))/(float)((int)(pos1->time - pos2->time)); // pc.printf("%lu %lu %lu %.3f\r\n",pos1->time,pos2->time,output->time,GradientWeight); output->X = (pos1->X-pos2->X)*GradientWeight + pos1->X; output->Y = (pos1->Y-pos2->Y)*GradientWeight + pos1->Y; output->Height = (pos1->Height-pos2->Height)*GradientWeight + pos1->Height; output->roll = position::interpAngle(pos1->roll,pos2->roll,GradientWeight); output->pitch = position::interpAngle(pos1->pitch,pos2->pitch,GradientWeight); output->yaw =position::interpAngle(pos1->yaw,pos2->yaw,GradientWeight); output->ID = pos1->ID; output->focus = pos1->focus; output->iris = pos1->iris; output->zoom = pos1->zoom; output->beacons = pos1->beacons; output->solutionType = pos1->solutionType; output->KFStatus = pos1->KFStatus; output->UsedBeaconsValid = pos1->UsedBeaconsValid; if (output->UsedBeaconsValid) memcpy(output->UsedBeacons,pos1->UsedBeacons,12); return true; }