File content as of revision 173:7f938afb0447:

#include "Device.h"

extern Device device;

void InitISACSOutputDefaultSettings(void){
  device.isacs.output.settings.transfer.points = 2;
  //Control point 1.5 volt - low output limit
  device.isacs.output.settings.transfer.voltage[0] = 0x00017fff;
  device.isacs.output.settings.transfer.code[0] = 800;
  //Control point 12.5 volt - high output limit
  device.isacs.output.settings.transfer.voltage[1] = 0x000c7fff;
  device.isacs.output.settings.transfer.code[1] = 2048;
  device.isacs.output.settings.start.voltage = 0x000c7fff; //+12.5V start
  device.isacs.output.settings.reset.voltage = 0x00077fff; //+7.5V if regulator disabled at reset time
}

void InitISACSOutputState(void){
}

void DeviceStartISACSOutput(void){
}

void InitISACSRegulatorDefaultSettings(void){
  //Startup settings
  device.isacs.regulator.settings.start.enabled = 1;
  device.isacs.regulator.settings.start.reference = 0x00020000; //Reference voltage 2V in 16.16 signed fixed point format
  device.isacs.regulator.settings.start.scale = 1; //Scale factor
  //Regular operation settings
  device.isacs.regulator.settings.regular.enabled = 1;
  device.isacs.regulator.settings.regular.reference = 0x00020000; //Reference voltage 2V in 16.16 signed fixed point format
  device.isacs.regulator.settings.regular.scale = 1; //Scale factor
  //Reset operation settings
  device.isacs.regulator.settings.reset.enabled = 1;
  device.isacs.regulator.settings.reset.reference = 0x00020000; //Reference voltage 2V in 16.16 signed fixed point format
  device.isacs.regulator.settings.reset.scale = 1; //Scale factor
}

void InitISACSRegulatorState(void){
  device.isacs.regulator.state.enabled = device.isacs.regulator.settings.start.enabled;
  device.isacs.regulator.state.reference = device.isacs.regulator.settings.start.reference;
  device.isacs.regulator.state.scale = device.isacs.regulator.settings.start.scale;
}

void DeviceStartISACSRegulator(void){
}

void InitISACSDefaultSettings(void){
  InitISACSPotentiometersDefaultSettings();
  InitISACSRegulatorDefaultSettings();
  InitISACSOutputDefaultSettings();
}

void InitISACSState(void){
  InitISACSPotentiometersState();
  InitISACSRegulatorState();
  InitISACSOutputState();
}

void DeviceStartISACS(void){
  DeviceStartISACSPotentiometers();
  DeviceStartISACSRegulator();
  DeviceStartISACSOutput();
}

/*
//Count leading zeroes
static uint8_t clz(uint32_t x) {
  uint8_t result = 0;
  if (x == 0) return 32;
  while (!(x & 0xF0000000)) { result += 4; x <<= 4; }
  while (!(x & 0x80000000)) { result += 1; x <<= 1; }
  return result;
}
//Count trailing zeroes
static uint8_t ctz(uint32_t x) {
  uint8_t result = 0;
  if (x == 0) return 32;
  while (!(x & 0xF0000000)) { result += 4; x <<= 4; }
  while (!(x & 0x80000000)) { result += 1; x <<= 1; }
  return result;
}
*/

int32_t isacsInterpolate(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;
  }
}

int32_t isacsInput(uint32_t code) {
  int32_t c1, c2;
  for (uint8_t i = 1; i < device.isacs.input.settings.transfer.points; i++) {
    c1 = device.isacs.input.settings.transfer.code[i - 1];
    if (code < c1) {
      return device.isacs.input.settings.transfer.voltage[i - 1];
    }

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

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

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

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

    v2 = device.isacs.output.interpolator.settings.voltage[i];
    if (voltage < v2) {
      //int32_t dv = device.isacs.output.state.voltage - v1;
      //int32_t dvi = v2 - v1;
      //int32_t c1 = device.isacs.output.settings.code[i - 1];
      //int32_t c2 = device.isacs.output.settings.code[i];
      //int32_t dc = c2 - c1;
      
      //uint32_t m = 1;
      //if (device.dac.settings.resolution > 16) {
      //  dv >>= 7; dc >>= 7; dvi >>= 7; m <<= 7;//For 24 bit: (21 - 7) + (24 - 7) - (21 - 7) + 7 = 24
      //} else if (device.dac.settings.resolution > 12) {
      //  dv >>= 3; dc >>= 3; dvi >>= 3; m <<= 3;//For 16 bit: (21 - 3) + (16 - 3) - (21 - 3) + 3 = 16
      //} else {
      //  dv >>= 1; dc >>= 1; dvi >>= 1; m <<= 1;//For 12 bit: (21 - 1) + (12 - 1) - (21 - 1) + 1 = 12
      //}
      //device.isacs.output.state.code = c1 + dv * dc / dvi * m;
      //device.isacs.output.state.voltage = voltage;
      
      int32_t c1 = device.isacs.output.interpolator.settings.code[i - 1];
      int32_t c2 = device.isacs.output.interpolator.settings.code[i];
      uint32_t code = isacsInterpolate(voltage, v1, v2, c1, c2);
      device.isacs.output.voltage = voltage;
      device.controller.SSP.out[0] = code;
      return;
    }
  }
  uint32_t code = device.isacs.output.interpolator.settings.code[device.isacs.output.interpolator.settings.points - 1];
  device.isacs.output.voltage = device.isacs.output.interpolator.settings.voltage[device.isacs.output.interpolator.settings.points - 1];
  device.controller.SSP.out[0] = code;
}

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

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

void isacsProcess(void) {
  device.isacs.input.state.voltage = isacsInput(device.controller.SSP.in[4]);
  device.isacs.input.state.sum += device.isacs.input.state.voltage;
  if (device.measurement.counter == 31) {
    device.isacs.input.state.average = device.isacs.input.state.sum >> 5;
    device.isacs.input.state.sum = 0;
    
    if (device.isacs.regulator.state.enabled) {
      device.isacs.regulator.state.error = device.isacs.regulator.state.reference - device.isacs.input.state.average;
      device.isacs.output.state.voltage += device.isacs.regulator.state.error * device.isacs.regulator.state.scale;
      device.controller.SSP.out[0] = isacsOutput(device.isacs.output.state.voltage);
    }
  }
}