123
Fork of LG by
DevicePLCS.c
- Committer:
- Diletant
- Date:
- 2016-05-22
- Revision:
- 166:c3c0b8a90d81
- Parent:
- 156:e68ee0bcdcda
- Child:
- 167:bedc0a9d559a
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(); } }