position.cpp

00001 #include "position.h"
00002 #include "LTCApp.h"
00003 #include <cstring>
00004 
00005 position::position()
00006 {
00007 }
00008 
00009 float position::interpAngle(float value1, float value2, float gradientWeight)
00010 {
00011 
00012     float delta = (value1-value2);
00013     if ((delta >= -180) && (delta <= 180)) // no wraparound
00014         return delta*gradientWeight + value1;
00015 
00016     if (delta < -180) { // val2 far larger than val1
00017         float result = (delta+360)*gradientWeight + value1;
00018         if (result >= 360)
00019             return result-360;
00020         return result;
00021     } else {
00022         float result = (delta-360)*gradientWeight + value1;
00023         if (result < 0)
00024             return result+360;
00025         return result;
00026     }
00027 }
00028 
00029 bool position::interp(position* output, position *pos1, position *pos2)
00030 {
00031     if (pos1->ID != pos2->ID)
00032         return false;
00033 
00034     if (output==NULL) {
00035         printf(" BO ");
00036         return false;
00037     }
00038     if (pos1==NULL) {
00039         printf(" B1 ");
00040         return false;
00041     }
00042     if (pos2==NULL) {
00043         printf(" B2 ");
00044         return false;
00045     }
00046 
00047     if (output->time == pos1->time) {
00048         memcpy(output,pos1,sizeof(position));
00049         return true;
00050     }
00051 
00052     if (output->time == pos2->time) {
00053         memcpy(output,pos2,sizeof(position));
00054         return true;
00055     }
00056 
00057     if (pos1->time == pos2->time) {
00058         return false;
00059     }
00060 
00061     float GradientWeight = ((int)(output->time-pos1->time))/(float)((int)(pos1->time - pos2->time));
00062     
00063  //   pc.printf("%lu  %lu %lu %.3f\r\n",pos1->time,pos2->time,output->time,GradientWeight);
00064     
00065     output->X = (pos1->X-pos2->X)*GradientWeight + pos1->X;
00066     output->Y = (pos1->Y-pos2->Y)*GradientWeight + pos1->Y;
00067     output->Height = (pos1->Height-pos2->Height)*GradientWeight + pos1->Height;
00068     output->roll = position::interpAngle(pos1->roll,pos2->roll,GradientWeight);
00069     output->pitch = position::interpAngle(pos1->pitch,pos2->pitch,GradientWeight);
00070     output->yaw =position::interpAngle(pos1->yaw,pos2->yaw,GradientWeight);
00071     output->ID = pos1->ID;
00072     output->focus = pos1->focus;
00073     output->iris = pos1->iris;
00074     output->zoom = pos1->zoom;
00075     output->x_accel  = pos1->x_accel;
00076     output->y_accel  = pos1->y_accel;
00077     output->z_accel  = pos1->z_accel;
00078     output->x_gyro  = pos1->x_gyro;
00079     output->y_gyro  = pos1->y_gyro;
00080     output->z_gyro  = pos1->z_gyro;
00081 
00082     output->beacons = pos1->beacons;
00083     output->solutionType = pos1->solutionType;
00084     output->KFStatus = pos1->KFStatus;
00085     output->UsedBeaconsValid = pos1->UsedBeaconsValid;
00086     if (output->UsedBeaconsValid)
00087         memcpy(output->UsedBeacons,pos1->UsedBeacons,12);
00088 
00089     return true;
00090 }
00091