File content as of revision 173:7f938afb0447:

#include "Device.h"
extern Device device;

void InitPathLengthControlSystemDefaultSettings(void){
  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
  
  device.plcs.output.settings.sequencer = 1;
}

void InitPathLengthControlSystemState(void){
  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;
  
  device.plcs.output.settings.sequencer = device.plcs.output.state.sequencer;
}

void DeviceStartPLCS(void){
}

/*
    //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.sequencer.sampler.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;
  }
}

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) {
    //Add sequencer output
    if (device.plcs.output.state.sequencer) {
      if (device.sequencer.output.analog.state.enabled) {
        device.plcs.output.state.voltage += device.sequencer.output.analog.state.voltage;
      }
    }
    
    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();
}