File content as of revision 166:c3c0b8a90d81:

#include "Device.h"
extern Device device;

void InitPathLengthControlSystemWithDefaults(void){
  //device.plcs.amplifier.settings.reference = 0x00020000;
  //device.plcs.amplifier.settings.gain = 10;
}

void InitPathLengthControlSystem(void){
  //Init 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
}

void StartPathLengthControlSystem(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 plcsVoltageToCode(int32_t voltage) {
  int32_t v1, v2;
  for (uint8_t i = 1; i < device.plcs.output.interpolator.settings.points; i++) {
    v1 = device.plcs.output.interpolator.settings.voltage[i - 1];
    if (voltage < v1) {
      return device.plcs.output.interpolator.settings.code[i - 1];
    }

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

/*
void plcsSetOutputVoltage(int32_t voltage) {
  int32_t v1, v2;
  for (uint8_t i = 1; i < device.plcs.output.settings.interpolator.points; i++) {
    v1 = device.plcs.output.settings.interpolator.voltage[i - 1];
    if (voltage < v1) {
      uint32_t code = device.plcs.output.settings.interpolator.code[i - 1];
      device.plcs.output.state.voltage = v1;
      device.controller.SSP.out[1] = code;
      return;
    }

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

void plcsSetOutputCode(int32_t code) {
  int32_t c1, c2;
  for (uint8_t i = 1; i < device.plcs.output.settings.interpolator.points; i++) {
    c1 = device.plcs.output.settings.interpolator.code[i - 1];
    if (code < c1) {
      device.plcs.output.state.voltage = device.plcs.output.settings.interpolator.voltage[i - 1];
      device.controller.SSP.out[1] = c1;
      return;
    }

    c2 = device.plcs.output.settings.interpolator.code[i];
    if (code < c2) {
      int32_t v1 = device.plcs.output.settings.interpolator.voltage[i - 1];
      int32_t v2 = device.plcs.output.settings.interpolator.voltage[i];
      device.plcs.output.state.voltage = plcsInterpolate(code, c1, c2, v1, v2);
      device.controller.SSP.out[1] = code;
      return;
    }
  }
  device.plcs.output.state.voltage = device.plcs.output.settings.interpolator.voltage[device.plcs.output.settings.interpolator.points - 1];
  device.controller.SSP.out[1] = device.plcs.output.settings.interpolator.code[device.plcs.output.settings.interpolator.points - 1];
}
*/

/*
//"Amplifier output voltage" = ("Amplifier reference" - "DAC output voltage") * "Amplifier gain"
void SetPathLengthDriveVoltage(int32_t Vdrive){
  
  //Calculate amplifier output voltage
  int32_t Vamp = Vdrive; //TODO: take control unit (transistor or high voltage amplifier) into account!!!
  //Calculate DAC output voltage
  int32_t Vdac_int32 = device.plcs.amplifier.settings.reference - Vamp / device.plcs.amplifier.settings.gain;
  if (Vdac_int32 < 0) Vdac_int32 = 0;
  uint32_t Vdac_uint32 = (uint32_t)Vdac_int32;
  if (Vdac_uint32 > device.dac.settings.reference) Vdac_uint32 = device.dac.settings.reference;
  //Set DAC output 1 voltage
  DeviceDACSetVoltage(1, Vdac_uint32);
}
*/

void plcsOutput(void) {
  if (device.plcs.output.state.enabled) {
    if (device.plcs.regulator.state.enabled) {
      device.plcs.regulator.state.error = (device.plcs.regulator.state.reference - device.plcs.detector.state.phase) * device.plcs.regulator.state.scale;
      device.plcs.output.state.voltage += device.plcs.regulator.state.error;
    }
    device.controller.SSP.out[1] = plcsVoltageToCode(device.plcs.output.state.voltage);
  }
}

void plcsProcessDelta(void) {
  //Process delta mode
  //TODO
  
  //Output voltage
  plcsOutput();
}

void plcsProcessSequencer(void) {
  //Process sequencer mode
  //TODO
  
  if (device.plcs.sequencer.state.enabled) {
    if (device.plcs.sequencer.state.voltage) {
      device.plcs.output.state.voltage += device.plcs.sequencer.settings.sequence[device.measurement.counter + device.measurement.length * device.plcs.sequencer.state.counter];
    }
  
    //Output io
    if (device.plcs.sequencer.state.io) {
      if (device.plcs.sequencer.settings.sequence[device.measurement.counter + device.measurement.length * device.plcs.sequencer.state.counter] > 0) {
        LPC_GPIO0->FIOSET    |=  (1<<26); //1
      } else {
        LPC_GPIO0->FIOCLR    |=  (1<<26); //0
      }
    }
  }
  //Output voltage
  plcsOutput();
}

void plcsProcess(void) {
  if (device.plcs.detector.state.delta) {

    if (device.plcs.detector.state.input) {
      device.plcs.input.voltage = device.isacs.input.average;
    } else if (device.plcs.detector.state.output) {
      device.plcs.input.voltage = device.isacs.output.voltage;
    }
    
    device.plcs.input.sum += device.plcs.input.voltage;
    if (device.measurement.counter == 31) {
      if (device.plcs.input.counter == 255) {
        device.plcs.input.average = device.plcs.input.sum >> 8;
        device.plcs.input.counter = 0;
        plcsProcessDelta();
      } else {
        device.plcs.input.counter++;
      }
    }

  } else if (device.plcs.detector.state.sequencer) {

    if (device.plcs.detector.state.input) {
      device.plcs.input.voltage = device.isacs.input.voltage;
    } else if (device.plcs.detector.state.output) {
      device.plcs.input.voltage = device.isacs.output.voltage;
    }
    plcsProcessSequencer();

  }
}