trap rule numeric integration

trapezoid_numeric_integration.c

static void integrate_gyro(FlowData_t* flow_data)
{
    uint8_t idx = flow_data->gyro_integ_idx;
    
    if (idx >= NUM_GYRO_INTEG_STEPS) {
        //shuffle newer data down to oldest buckets
        size_t buck_size = sizeof(flow_data->gyro_x_traps[0]);
        size_t total_size = (NUM_GYRO_INTEG_STEPS -1)*buck_size;
        memcpy( (flow_data->gyro_x_traps), (flow_data->gyro_x_traps + buck_size), total_size);
        memcpy( (flow_data->gyro_y_traps), (flow_data->gyro_y_traps + buck_size), total_size);
        memcpy( (flow_data->gyro_z_traps), (flow_data->gyro_z_traps + buck_size), total_size);
        idx--;
    }
    
    //use trapezoid rule to integrate gyro rate to gyro angle over time
    // dA = 0.5*dT*( f(i) + f(i-1) )
    // dA = time_factor * ( rate[1] + rate [0] )
    float time_factor = 0.5f * 1.0f; //TODO get actual time delta

    flow_data->gyro_x_traps[idx] = time_factor*(flow_data->gyro_x_rate[1] + flow_data->gyro_x_rate[0]);
    flow_data->gyro_y_traps[idx] = time_factor*(flow_data->gyro_y_rate[1] + flow_data->gyro_y_rate[0]);
    flow_data->gyro_z_traps[idx] = time_factor*(flow_data->gyro_z_rate[1] + flow_data->gyro_z_rate[0]);
    
    idx++;

}