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-01-18
- Revision:
- 2:a79201e302d7
- Parent:
- 0:97661408d0f9
- Child:
- 3:14d241e29be3
File content as of revision 2:a79201e302d7:
#include "position.h" #include <cstring> position::position() { } float position::interpAngle(float value1, float weight1, float value2, float weight2) { float delta = (value1-value2); if ((delta >= -180) && (delta <= 180)) // no wraparound return value1*weight1 + value2*weight2; if (delta < -180) { // val2 far larger than val1 float result = (value1+360)*weight1 + value2*weight2; if (result >= 360) return result-360; return result; } else { float result = (value1-360)*weight1 + value2*weight2; 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)); printf(" C1 "); return true; } if (output->time == pos2->time) { memcpy(output,pos2,sizeof(position)); printf(" C2 "); return true; } if (pos1->time == pos2->time) { printf(" E %ld",pos1->time); return false; } float pos2Weight = (pos1->time - pos2->time)/(float)(pos1->time - output->time); float pos1Weight = 1-pos2Weight; output->X = pos1->X*pos1Weight + pos2->X*pos2Weight; output->Y = pos1->Y*pos1Weight + pos2->Y*pos2Weight; output->Height = pos1->Height*pos1Weight + pos2->Height*pos2Weight; output->roll = position::interpAngle(pos1->roll,pos1Weight,pos2->roll,pos2Weight); output->pitch = position::interpAngle(pos1->pitch,pos1Weight,pos2->pitch,pos2Weight); output->yaw =position::interpAngle(pos1->yaw,pos1Weight,pos2->yaw,pos2Weight); output->ID = pos1->ID; return true; }