123
Fork of LG by
DevicePLCS.c
- Committer:
- Diletant
- Date:
- 2016-06-19
- Revision:
- 173:7f938afb0447
- Parent:
- 167:bedc0a9d559a
- Child:
- 177:672ef279c8e0
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(); }