File content as of revision 85:0cc5931bb9ef:

#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->x_accel  = pos1->x_accel;
    output->y_accel  = pos1->y_accel;
    output->z_accel  = pos1->z_accel;
    output->x_gyro  = pos1->x_gyro;
    output->y_gyro  = pos1->y_gyro;
    output->z_gyro  = pos1->z_gyro;

    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;
}