File content as of revision 167:bedc0a9d559a:

#include "Device.h"
extern Device device;

void InitPathLengthControlSystemDefaultSettings(void){
  device.plcs.sequencer.settings.enabled = 1;
  device.plcs.sequencer.settings.analog = 0;
  device.plcs.sequencer.settings.logic = 1;
  
  device.plcs.sequencer.settings.amplitude = 0x1e0000; //30V
  device.plcs.sequencer.settings.position[0] = 0;  //starting output sample position
  device.plcs.sequencer.settings.position[1] = 16; //starting reference (delayed) sample position
  
  device.plcs.sequencer.settings.sequence[0] = 0;
  device.plcs.sequencer.settings.sequence[1] = 6424;
  device.plcs.sequencer.settings.sequence[2] = 12785;
  device.plcs.sequencer.settings.sequence[3] = 19024;
  device.plcs.sequencer.settings.sequence[4] = 25080;
  device.plcs.sequencer.settings.sequence[5] = 30893;
  device.plcs.sequencer.settings.sequence[6] = 36410;
  device.plcs.sequencer.settings.sequence[7] = 41576;
  device.plcs.sequencer.settings.sequence[8] = 46341;
  device.plcs.sequencer.settings.sequence[9] = 50660;
  device.plcs.sequencer.settings.sequence[10] = 54491;
  device.plcs.sequencer.settings.sequence[11] = 57798;
  device.plcs.sequencer.settings.sequence[12] = 60547;
  device.plcs.sequencer.settings.sequence[13] = 62714;
  device.plcs.sequencer.settings.sequence[14] = 64277;
  device.plcs.sequencer.settings.sequence[15] = 65220;
  device.plcs.sequencer.settings.sequence[16] = 65536;
  device.plcs.sequencer.settings.sequence[17] = 65220;
  device.plcs.sequencer.settings.sequence[18] = 64277;
  device.plcs.sequencer.settings.sequence[19] = 62714;
  device.plcs.sequencer.settings.sequence[20] = 60547;
  device.plcs.sequencer.settings.sequence[21] = 57798;
  device.plcs.sequencer.settings.sequence[22] = 54491;
  device.plcs.sequencer.settings.sequence[23] = 50660;
  device.plcs.sequencer.settings.sequence[24] = 46341;
  device.plcs.sequencer.settings.sequence[25] = 41576;
  device.plcs.sequencer.settings.sequence[26] = 36410;
  device.plcs.sequencer.settings.sequence[27] = 30893;
  device.plcs.sequencer.settings.sequence[28] = 25080;
  device.plcs.sequencer.settings.sequence[29] = 19024;
  device.plcs.sequencer.settings.sequence[30] = 12785;
  device.plcs.sequencer.settings.sequence[31] = 6424;
  device.plcs.sequencer.settings.sequence[32] = 0;
  device.plcs.sequencer.settings.sequence[33] = -6424;
  device.plcs.sequencer.settings.sequence[34] = -12785;
  device.plcs.sequencer.settings.sequence[35] = -19024;
  device.plcs.sequencer.settings.sequence[36] = -25080;
  device.plcs.sequencer.settings.sequence[37] = -30893;
  device.plcs.sequencer.settings.sequence[38] = -36410;
  device.plcs.sequencer.settings.sequence[39] = -41576;
  device.plcs.sequencer.settings.sequence[40] = -46341;
  device.plcs.sequencer.settings.sequence[41] = -50660;
  device.plcs.sequencer.settings.sequence[42] = -54491;
  device.plcs.sequencer.settings.sequence[43] = -57798;
  device.plcs.sequencer.settings.sequence[44] = -60547;
  device.plcs.sequencer.settings.sequence[45] = -62714;
  device.plcs.sequencer.settings.sequence[46] = -64277;
  device.plcs.sequencer.settings.sequence[47] = -65220;
  device.plcs.sequencer.settings.sequence[48] = -65536;
  device.plcs.sequencer.settings.sequence[49] = -65220;
  device.plcs.sequencer.settings.sequence[50] = -64277;
  device.plcs.sequencer.settings.sequence[51] = -62714;
  device.plcs.sequencer.settings.sequence[52] = -60547;
  device.plcs.sequencer.settings.sequence[53] = -57798;
  device.plcs.sequencer.settings.sequence[54] = -54491;
  device.plcs.sequencer.settings.sequence[55] = -50660;
  device.plcs.sequencer.settings.sequence[56] = -46341;
  device.plcs.sequencer.settings.sequence[57] = -41576;
  device.plcs.sequencer.settings.sequence[58] = -36410;
  device.plcs.sequencer.settings.sequence[59] = -30893;
  device.plcs.sequencer.settings.sequence[60] = -25080;
  device.plcs.sequencer.settings.sequence[61] = -19024;
  device.plcs.sequencer.settings.sequence[62] = -12785;
  device.plcs.sequencer.settings.sequence[63] = -6424;
    
  device.plcs.feedback.settings.input = 1;
  device.plcs.feedback.settings.output = 0;
  
  device.plcs.reference.settings.sequencer = 1;
  device.plcs.reference.settings.delta = 0;
  
  device.plcs.reset.levels.upper = 0xc8000; //12.5V
  device.plcs.reset.levels.lower = 0x18000; //1.5V
  device.plcs.reset.up.points = 16;
  device.plcs.reset.up.temperature[0] = -0x3c0000; //-60C
  device.plcs.reset.up.voltage[0] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[0] = 0x30000; //3s
  device.plcs.reset.up.temperature[1] = -0x320000; //-50C
  device.plcs.reset.up.voltage[1] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[1] = 0x30000; //3s
  device.plcs.reset.up.temperature[2] = -0x280000; //-40C
  device.plcs.reset.up.voltage[2] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[2] = 0x30000; //3s
  device.plcs.reset.up.temperature[3] = -0x1e0000; //-30C
  device.plcs.reset.up.voltage[3] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[3] = 0x30000; //3s
  device.plcs.reset.up.temperature[4] = -0x140000; //-20C
  device.plcs.reset.up.voltage[4] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[4] = 0x30000; //3s
  device.plcs.reset.up.temperature[5] = -0xa0000; //-10C
  device.plcs.reset.up.voltage[5] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[5] = 0x30000; //3s
  device.plcs.reset.up.temperature[6] = 0x0; //0C
  device.plcs.reset.up.voltage[6] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[6] = 0x30000; //3s
  device.plcs.reset.up.temperature[7] = 0xa0000; //10C
  device.plcs.reset.up.voltage[7] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[7] = 0x30000; //3s
  device.plcs.reset.up.temperature[8] = 0x140000; //20C
  device.plcs.reset.up.voltage[8] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[8] = 0x30000; //3s
  device.plcs.reset.up.temperature[9] = 0x1e0000; //30C
  device.plcs.reset.up.voltage[9] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[9] = 0x30000; //3s
  device.plcs.reset.up.temperature[10] = 0x280000; //40C
  device.plcs.reset.up.voltage[10] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[10] = 0x30000; //3s
  device.plcs.reset.up.temperature[11] = 0x320000; //50C
  device.plcs.reset.up.voltage[11] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[11] = 0x30000; //3s
  device.plcs.reset.up.temperature[12] = 0x3c0000; //60C
  device.plcs.reset.up.voltage[12] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[12] = 0x30000; //3s
  device.plcs.reset.up.temperature[13] = 0x460000; //70C
  device.plcs.reset.up.voltage[13] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[13] = 0x30000; //3s
  device.plcs.reset.up.temperature[14] = 0x500000; //80C
  device.plcs.reset.up.voltage[14] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[14] = 0x30000; //3s
  device.plcs.reset.up.temperature[15] = 0x5a0000; //90C
  device.plcs.reset.up.voltage[15] = 0xc8000; //12.5V
  device.plcs.reset.up.duration[15] = 0x30000; //3s
  device.plcs.reset.down.points = 16;
  device.plcs.reset.down.temperature[0] = -0x3c0000; //-60C
  device.plcs.reset.down.voltage[0] = 0x18000; //1.5V
  device.plcs.reset.down.duration[0] = 0x30000; //3s
  device.plcs.reset.down.temperature[1] = -0x320000; //-50C
  device.plcs.reset.down.voltage[1] = 0x18000; //1.5V
  device.plcs.reset.down.duration[1] = 0x30000; //3s
  device.plcs.reset.down.temperature[2] = -0x280000; //-40C
  device.plcs.reset.down.voltage[2] = 0x18000; //1.5V
  device.plcs.reset.down.duration[2] = 0x30000; //3s
  device.plcs.reset.down.temperature[3] = -0x1e0000; //-30C
  device.plcs.reset.down.voltage[3] = 0x18000; //1.5V
  device.plcs.reset.down.duration[3] = 0x30000; //3s
  device.plcs.reset.down.temperature[4] = -0x140000; //-20C
  device.plcs.reset.down.voltage[4] = 0x18000; //1.5V
  device.plcs.reset.down.duration[4] = 0x30000; //3s
  device.plcs.reset.down.temperature[5] = -0xa0000; //-10C
  device.plcs.reset.down.voltage[5] = 0x18000; //1.5V
  device.plcs.reset.down.duration[5] = 0x30000; //3s
  device.plcs.reset.down.temperature[6] = 0x0; //0C
  device.plcs.reset.down.voltage[6] = 0x18000; //1.5V
  device.plcs.reset.down.duration[6] = 0x30000; //3s
  device.plcs.reset.down.temperature[7] = 0xa0000; //10C
  device.plcs.reset.down.voltage[7] = 0x18000; //1.5V
  device.plcs.reset.down.duration[7] = 0x30000; //3s
  device.plcs.reset.down.temperature[8] = 0x140000; //20C
  device.plcs.reset.down.voltage[8] = 0x18000; //1.5V
  device.plcs.reset.down.duration[8] = 0x30000; //3s
  device.plcs.reset.down.temperature[9] = 0x1e0000; //30C
  device.plcs.reset.down.voltage[9] = 0x18000; //1.5V
  device.plcs.reset.down.duration[9] = 0x30000; //3s
  device.plcs.reset.down.temperature[10] = 0x280000; //40C
  device.plcs.reset.down.voltage[10] = 0x18000; //1.5V
  device.plcs.reset.down.duration[10] = 0x30000; //3s
  device.plcs.reset.down.temperature[11] = 0x320000; //50C
  device.plcs.reset.down.voltage[11] = 0x18000; //1.5V
  device.plcs.reset.down.duration[11] = 0x30000; //3s
  device.plcs.reset.down.temperature[12] = 0x3c0000; //60C
  device.plcs.reset.down.voltage[12] = 0x18000; //1.5V
  device.plcs.reset.down.duration[12] = 0x30000; //3s
  device.plcs.reset.down.temperature[13] = 0x460000; //70C
  device.plcs.reset.down.voltage[13] = 0x18000; //1.5V
  device.plcs.reset.down.duration[13] = 0x30000; //3s
  device.plcs.reset.down.temperature[14] = 0x500000; //80C
  device.plcs.reset.down.voltage[14] = 0x18000; //1.5V
  device.plcs.reset.down.duration[14] = 0x30000; //3s
  device.plcs.reset.down.temperature[15] = 0x5a0000; //90C
  device.plcs.reset.down.voltage[15] = 0x18000; //1.5V
  device.plcs.reset.down.duration[15] = 0x30000; //3s
}

void InitPathLengthControlSystemState(void){
  device.plcs.sequencer.state.enabled = device.plcs.sequencer.settings.enabled;
  device.plcs.sequencer.state.analog = device.plcs.sequencer.settings.analog;
  device.plcs.sequencer.state.logic = device.plcs.sequencer.settings.logic;
  device.plcs.sequencer.state.amplitude = device.plcs.sequencer.settings.amplitude;
  device.plcs.sequencer.state.position[0] = device.plcs.sequencer.settings.position[0]; //starting output sample position
  device.plcs.sequencer.state.position[1] = device.plcs.sequencer.settings.position[1]; //starting reference (delayed) sample position
  device.plcs.sequencer.state.sample[0] = device.plcs.sequencer.settings.sequence[device.plcs.sequencer.state.position[0]];
  device.plcs.sequencer.state.sample[1] = device.plcs.sequencer.settings.sequence[device.plcs.sequencer.state.position[1]];
  device.plcs.sequencer.state.voltage = device.plcs.sequencer.state.amplitude * device.plcs.sequencer.state.sample[0];
  
  device.plcs.feedback.state.input = device.plcs.feedback.settings.input;
  device.plcs.feedback.state.output = device.plcs.feedback.settings.output;
  
  device.plcs.reference.state.sequencer = device.plcs.reference.settings.sequencer;
  device.plcs.reference.state.delta = device.plcs.reference.settings.delta;
  
  device.plcs.detector.state.out = 1;
  
  device.plcs.reset.state.countdown = 0;
}

void DeviceStartPLCS(void){
  //Setup P0.26: Sequencer GPIO output
  LPC_PINCON->PINSEL1  &= ~(3<<20); //P0.26 is GPIO pin (write ( 00 ) in bits 21:20 of PINSEL1)
  LPC_PINCON->PINMODE1 |=  (3<<20); //P0.26 pull-down resistor on (write ( 11 ) in bits 21:20 of PINMODE0)
  LPC_GPIO0->FIODIR    |=  (1<<26); //P0.26 is output (write ( 1 ) in bit 4 of FIODIR)
  LPC_GPIO0->FIOSET    |=  (1<<26); //off
}

/*
    //Move to DevicePLCS regulator
    uint32_t value;
    switch(device.plcs.state.modulation) {
        case 1://малое воздействие
          value = device.SSP.DAC[1] + Gyro.StrayPLC_Pls;
        break;
        
        case 3://малое воздействие
          value = device.SSP.DAC[1] + Gyro.StrayPLC_Mns;
        break;
        
        case 2://большое воздействие
          value = device.SSP.DAC[1] + Gyro.StrayPLC_2Mode;
        break;
        
        default://режим без воздействия
          value = device.SSP.DAC[1];
        break;
    }
    LPC_SSP0->DR = device.SSP.DAC[1];
    */

int32_t plcsInterpolate(int32_t a,int32_t a1,int32_t a2,int32_t b1,int32_t b2) {
  int32_t ma, mb;
  while (1) {
    if (a1 == a) return b1;
    if (a2 == a) return b2;
                
    ma = (a1 + a2) >> 1;
    mb = (b1 + b2) >> 1;
      
    if (a < ma) {
      if (a2 == ma) return mb;
      if (b1 == mb) return mb;
      a2 = ma; b2 = mb;
    } else if (a > ma) {
      if (a1 == ma) return mb;
      if (b2 == mb) return mb;
      a1 = ma; b1 = mb;
    } else return mb;
  }
}

uint32_t plcsFeedback(int32_t voltage) {
  int32_t v1, v2;
  for (uint8_t i = 1; i < device.plcs.feedback.settings.transfer.points; i++) {
    v1 = device.plcs.feedback.settings.transfer.raw[i - 1];
    if (voltage < v1) {
      return device.plcs.feedback.settings.transfer.normalized[i - 1];
    }

    v2 = device.plcs.feedback.settings.transfer.raw[i];
    if (voltage < v2) {
      int32_t r1 = device.plcs.feedback.settings.transfer.normalized[i - 1];
      int32_t r2 = device.plcs.feedback.settings.transfer.normalized[i];
      return plcsInterpolate(voltage, v1, v2, r1, r2);
    }
  }
  return device.plcs.feedback.settings.transfer.normalized[device.plcs.feedback.settings.transfer.points - 1];
}

uint32_t plcsBias(int32_t raw) {
  int32_t r1, r2;
  for (uint8_t i = 1; i < device.plcs.bias.settings.transfer.points; i++) {
    r1 = device.plcs.bias.settings.transfer.raw[i - 1];
    if (raw < r1) {
      return device.plcs.bias.settings.transfer.normalized[i - 1];
    }

    r2 = device.plcs.bias.settings.transfer.raw[i];
    if (raw < r2) {
      int32_t l1 = device.plcs.bias.settings.transfer.normalized[i - 1];
      int32_t l2 = device.plcs.bias.settings.transfer.normalized[i];
      return plcsInterpolate(raw, r1, r2, l1, l2);
    }
  }
  return device.plcs.bias.settings.transfer.normalized[device.plcs.bias.settings.transfer.points - 1];
}

uint32_t plcsCorrection(int32_t error) {
  int32_t e1, e2;
  for (uint8_t i = 1; i < device.plcs.regulator.settings.transfer.points; i++) {
    e1 = device.plcs.regulator.settings.transfer.error[i - 1];
    if (error < e1) {
      return device.plcs.regulator.settings.transfer.correction[i - 1];
    }

    e2 = device.plcs.regulator.settings.transfer.error[i];
    if (error < e2) {
      int32_t c1 = device.plcs.regulator.settings.transfer.correction[i - 1];
      int32_t c2 = device.plcs.regulator.settings.transfer.correction[i];
      return plcsInterpolate(error, e1, e2, c1, c2);
    }
  }
  return device.plcs.regulator.settings.transfer.correction[device.plcs.regulator.settings.transfer.points - 1];
}

uint32_t plcsCode(int32_t voltage) {
  int32_t v1, v2;
  for (uint8_t i = 1; i < device.plcs.output.settings.transfer.points; i++) {
    v1 = device.plcs.output.settings.transfer.voltage[i - 1];
    if (voltage < v1) {
      return device.plcs.output.settings.transfer.code[i - 1];
    }

    v2 = device.plcs.output.settings.transfer.voltage[i];
    if (voltage < v2) {
      int32_t c1 = device.plcs.output.settings.transfer.code[i - 1];
      int32_t c2 = device.plcs.output.settings.transfer.code[i];
      return plcsInterpolate(voltage, v1, v2, c1, c2);
    }
  }
  return device.plcs.output.settings.transfer.code[device.plcs.output.settings.transfer.points - 1];
}

void plcsProcessDelta(void) {
  //Process detector
  int32_t feedback = plcsFeedback(device.plcs.feedback.state.voltage); //-1...+1 in 16.16 format
  int32_t delta = feedback - device.plcs.detector.state.in[0];         //-2...+2 in 16.16 format
  //if (delta > 0) delta = 1;
  //if (delta < 0) delta = -1;
  device.plcs.detector.state.in[0] = feedback;
  device.plcs.detector.state.out = ((delta >> 2) * (device.plcs.detector.state.in[1] >> 1)) >> 14; //-1...+1 in 16.16 format
  
  //Process bias -0.5...+0.5 in 16.16 format
  device.plcs.bias.state.raw = plcsBias(device.plcs.detector.state.out);
  device.plcs.bias.state.sum += device.plcs.bias.state.raw;
  device.plcs.bias.state.counter++;
  if (device.plcs.bias.state.counter == 256) {
    device.plcs.bias.state.average = device.plcs.bias.state.sum >> 8;
    device.plcs.bias.state.sum = 0;
    device.plcs.bias.state.counter = 0;
    
    if (device.plcs.bias.state.average > 0)
      device.plcs.detector.state.in[1] = -1;
    else
      device.plcs.detector.state.in[1] = 1;
  }
}

void plcsProcessSequencer(void) {
  //Process detector
  device.plcs.detector.state.in[0] = plcsFeedback(device.plcs.feedback.state.voltage) >> 1;                  //-1...+1 in 17.15 format
  device.plcs.detector.state.in[1] = device.plcs.sequencer.state.sample[1] >> 1;                             //-1...+1 in 17.15 format
  device.plcs.detector.state.out = device.plcs.detector.state.in[0] * device.plcs.detector.state.in[1] >> 14;//-1...+1 in 16.16 format
  
  //Process bias -0.5...+0.5 in 16.16 format
  device.plcs.bias.state.raw = plcsBias(device.plcs.detector.state.out);
  device.plcs.bias.state.sum += device.plcs.bias.state.raw;
  device.plcs.bias.state.counter++;
  if (device.plcs.bias.state.counter == 64) {
    device.plcs.bias.state.average = device.plcs.bias.state.sum >> 6;
    device.plcs.bias.state.sum = 0;
    device.plcs.bias.state.counter = 0;
  }
  
  //Process output
  if (device.plcs.sequencer.state.enabled) {
    //Set analog output
    if (device.plcs.sequencer.state.analog) {
      if (device.plcs.output.state.enabled) {
        device.plcs.output.state.voltage += device.plcs.sequencer.state.voltage;
      }
    }
  
    //Set logic output
    if (device.plcs.sequencer.state.logic) {
      if (device.plcs.sequencer.state.sample[0] > 0) {
        LPC_GPIO0->FIOCLR |= (1<<26); //1
      } else {
        LPC_GPIO0->FIOSET |= (1<<26); //0
      }
    }
  }
  
  //Process sequencer
  device.plcs.sequencer.state.position[0]++;
  if (device.plcs.sequencer.state.position[0] == 64) device.plcs.sequencer.state.position[0] = 0;
  device.plcs.sequencer.state.position[1]++;
  if (device.plcs.sequencer.state.position[1] == 64) device.plcs.sequencer.state.position[1] = 0;
  device.plcs.sequencer.state.sample[0] = device.plcs.sequencer.settings.sequence[device.plcs.sequencer.state.position[0]];
  device.plcs.sequencer.state.sample[1] = device.plcs.sequencer.settings.sequence[device.plcs.sequencer.state.position[1]];
  device.plcs.sequencer.state.voltage = device.plcs.sequencer.state.amplitude * device.plcs.sequencer.state.sample[0];
}

void plcsRegulate(void) {
  device.plcs.regulator.state.error = device.plcs.regulator.state.reference - device.plcs.bias.state.average;
  device.plcs.regulator.state.correction = plcsCorrection(device.plcs.regulator.state.error);
  
  if (device.plcs.output.state.enabled) {
    if (device.plcs.reset.state.countdown == 0){
      if (device.plcs.regulator.state.enabled) {
        device.plcs.output.state.voltage += device.plcs.regulator.state.correction;
      }
    }
  }
}

void plcsUp(void) {
  int32_t temperature = 0;
  //TODO: set temperature variable from temperature sensor
  
  int32_t t1, t2;
  for (uint8_t i = 1; i < device.plcs.reset.up.points; i++) {
    t1 = device.plcs.reset.up.temperature[i - 1];
    if (temperature < t1) {
      device.plcs.reset.state.voltage = device.plcs.reset.up.voltage[i - 1];
      device.plcs.reset.state.countdown = device.plcs.reset.up.duration[i - 1];
    }

    t2 = device.plcs.reset.up.temperature[i];
    if (temperature < t2) {
      int32_t v1 = device.plcs.reset.up.voltage[i - 1];
      int32_t v2 = device.plcs.reset.up.voltage[i];
      device.plcs.reset.state.voltage = plcsInterpolate(temperature, t1, t2, v1, v2);
      int32_t d1 = device.plcs.reset.up.duration[i - 1];
      int32_t d2 = device.plcs.reset.up.duration[i];
      device.plcs.reset.state.countdown = plcsInterpolate(temperature, t1, t2, d1, d2);
    }
  }
  device.plcs.output.state.voltage = device.plcs.reset.up.voltage[device.plcs.reset.up.points - 1];
  device.plcs.reset.state.countdown = device.plcs.reset.up.duration[device.plcs.reset.up.points - 1];
}

void plcsDown(void) {
  int32_t temperature = 0;
  //TODO: set temperature variable from temperature sensor
  
  int32_t t1, t2;
  for (uint8_t i = 1; i < device.plcs.reset.down.points; i++) {
    t1 = device.plcs.reset.down.temperature[i - 1];
    if (temperature < t1) {
      device.plcs.reset.state.voltage = device.plcs.reset.down.voltage[i - 1];
      device.plcs.reset.state.countdown = device.plcs.reset.down.duration[i - 1];
    }

    t2 = device.plcs.reset.down.temperature[i];
    if (temperature < t2) {
      int32_t v1 = device.plcs.reset.down.voltage[i - 1];
      int32_t v2 = device.plcs.reset.down.voltage[i];
      device.plcs.reset.state.voltage = plcsInterpolate(temperature, t1, t2, v1, v2);
      int32_t d1 = device.plcs.reset.down.duration[i - 1];
      int32_t d2 = device.plcs.reset.down.duration[i];
      device.plcs.reset.state.countdown = plcsInterpolate(temperature, t1, t2, d1, d2);
    }
  }
  device.plcs.reset.state.voltage = device.plcs.reset.down.voltage[device.plcs.reset.down.points - 1];
  device.plcs.reset.state.countdown = device.plcs.reset.down.duration[device.plcs.reset.down.points - 1];
}

void plcsReset(void) {
  if (device.plcs.reset.state.countdown > 0){
    int32_t period = 0x00000005; //5/65536sec = 75 mks
    //TODO: Set processing period from measurement cycle
    
    device.plcs.reset.state.countdown -= period;
    if (device.plcs.reset.state.countdown < 0) device.plcs.reset.state.countdown = 0;
  } else {
    if (device.plcs.output.state.voltage > device.plcs.reset.levels.upper) {
      plcsDown();
    } else if (device.plcs.output.state.voltage < device.plcs.reset.levels.lower) {
      plcsUp();
    };
  }
}

void plcsOutput(void) {
  if (device.plcs.output.state.enabled) {
    device.plcs.output.state.code = plcsCode(device.plcs.output.state.voltage);
    device.controller.SSP.out[1] = device.plcs.output.state.code;
  }
}

//Main PLCS processing function
void plcsProcess(void) {
  //Process reset state output voltage
  if (device.plcs.output.state.enabled) {
    if (device.plcs.reset.state.countdown > 0){
      device.plcs.output.state.voltage = device.plcs.reset.state.voltage;
    }
  }
  //Process input
  if (device.plcs.reference.state.delta) {
    //Process delta state
    if (device.plcs.feedback.state.input) {
      device.plcs.feedback.state.voltage = device.isacs.input.state.average;
    } else if (device.plcs.feedback.state.output) {
      device.plcs.feedback.state.voltage = device.isacs.output.state.voltage;
    }
    
    /*
    device.plcs.feedback.state.sum += device.plcs.feedback.state.voltage;
    if (device.measurement.counter == 31) {
      if (device.plcs.feedback.state.counter == 255) {
        device.plcs.feedback.state.average = device.plcs.feedback.state.sum >> 8;
        device.plcs.feedback.state.sum = 0;
        device.plcs.feedback.state.counter = 0;
        plcsProcessDelta();
      } else {
        device.plcs.feedback.state.counter++;
      }
    }
    */
    plcsProcessDelta();

  } else if (device.plcs.reference.state.sequencer) {
    //Process sequencer state
    if (device.plcs.feedback.state.input) {
      device.plcs.feedback.state.voltage = device.isacs.input.state.voltage;
    } else if (device.plcs.feedback.state.output) {
      device.plcs.feedback.state.voltage = device.isacs.output.state.voltage;
    }
    plcsProcessSequencer();

  }
  //Process regulator
  plcsRegulate();
  //Process reset
  plcsReset();
  //Process output
  plcsOutput();
}