X-Git-Url: http://git.tdb.fi/?p=model-railway-devices.git;a=blobdiff_plain;f=arducontrol%2Fmonitor.c;h=9b5154dddfcf1d2209b3253436ac9b0ee2289421;hp=c88a0717e1106dcc9d8e6d3d881e69ee87947ce0;hb=3fb9da57664c39827f687d664343488b0136f4e1;hpb=b47b38e9dd282c5f8fb1ded25ade805e64d0cf1a

diff --git a/arducontrol/monitor.c b/arducontrol/monitor.c
index c88a071..9b5154d 100644
--- a/arducontrol/monitor.c
+++ b/arducontrol/monitor.c
@@ -4,78 +4,113 @@
 #include "output.h"
 #include "serial.h"
 
-uint16_t track_current_samples[16] = { 0 };
-uint8_t track_current_head = 0;
-volatile uint16_t track_current_sum = 0;
-uint16_t overcurrent_limit = 1000<<4;
-uint8_t overcurrent_sent = 0;
+static uint16_t track_current_samples[16] = { 0 };
+static uint8_t track_current_head = 0;
+static volatile uint16_t track_current_sum = 0;
+static uint16_t overcurrent_limit = 9707;
+static uint8_t overcurrent_sent = 0;
 
-uint16_t input_voltage_samples[16] = { 0 };
-uint8_t input_voltage_head = 0;
-volatile uint16_t input_voltage_sum = 0;
+static uint16_t input_voltage_samples[16] = { 0 };
+static uint8_t input_voltage_head = 0;
+static volatile uint16_t input_voltage_sum = 0;
 
-volatile uint8_t adc_state = 0;
+static volatile uint8_t adc_state = 0;
+static volatile uint16_t adc_value = 0;
 
-void monitor_init()
+static uint16_t track_current_milliamps(void);
+static uint16_t input_voltage_millivolts(void);
+
+void monitor_init(void)
 {
+	DDRB |= 0x02;
 	adc_init();
 }
 
-void monitor_check()
+void monitor_check(void)
 {
 	if(!(adc_state&1))
 	{
-		++adc_state;
-		adc_read_async(adc_state>>1);
-	}
+		uint16_t value = adc_value;
 
-	if(track_current_sum>overcurrent_limit)
-	{
-		output_set_power(0);
-		if(!overcurrent_sent)
+		if(adc_state==2)
+		{
+			uint8_t i = track_current_head;
+			track_current_sum -= track_current_samples[i];
+			track_current_samples[i] = value;
+			track_current_sum += value;
+			track_current_head = (i+1)&15;
+
+			if(track_current_sum>overcurrent_limit)
+			{
+				output_set_power(0);
+				PORTB |= 0x02;
+				if(!overcurrent_sent)
+				{
+					overcurrent_sent = 1;
+					interface_send1(OVERCURRENT);
+				}
+			}
+			else if(overcurrent_sent && output_is_power_on())
+			{
+				PORTB &= ~0x02;
+				overcurrent_sent = 0;
+			}
+		}
+		else if(adc_state==4)
 		{
-			overcurrent_sent = 1;
-			serial_write(0xFE);
-			serial_write(OVERCURRENT);
+			uint8_t i = input_voltage_head;
+			input_voltage_sum -= input_voltage_samples[i];
+			input_voltage_samples[i] = value;
+			input_voltage_sum += value;
+			input_voltage_head = (i+1)&15;
 		}
+
+		adc_state = (adc_state+1)&3;
+		adc_read_async(adc_state>>1);
 	}
-	else
-		overcurrent_sent = 0;
 }
 
-uint8_t monitor_command()
+uint8_t monitor_command(const uint8_t *cmd_buf, uint8_t cmd_length)
 {
 	if(cmd_buf[0]==READ_TRACK_CURRENT)
 	{
+		uint8_t reply[3];
+
 		if(cmd_length!=1)
 			return LENGTH_ERROR;
 
-		serial_write(0xFC);
-		serial_write(TRACK_CURRENT);
-		uint16_t value = track_current_sum>>4;
-		serial_write(value>>8);
-		serial_write(value);
+		uint16_t value = track_current_milliamps();
+		reply[0] = TRACK_CURRENT;
+		reply[1] = value>>8;
+		reply[2] = value;
+		interface_send(reply, sizeof(reply));
 	}
 	else if(cmd_buf[0]==SET_OVERCURRENT_LIMIT)
 	{
 		if(cmd_length!=3)
 			return LENGTH_ERROR;
 
-		if(cmd_buf[1]&0xF0)
+		uint16_t value = (cmd_buf[1]<<8) | cmd_buf[2];
+		if(value>4000)  // Safe maximum value
 			return INVALID_VALUE;
 
-		overcurrent_limit = (cmd_buf[1]<<12) | (cmd_buf[2]<<4);
+		// Convert from milliamps: (512+v/1000*0.185/5*1024)*16
+		// multiply by 16384*0.185/5000 = 0.1001101100110b
+		uint16_t v_3 = value*3;
+		overcurrent_limit = 8192+(value>>1)+(v_3>>5)+(v_3>>8)+(v_3>>12);
 	}
 	else if(cmd_buf[0]==READ_INPUT_VOLTAGE)
 	{
+		uint8_t reply[3];
+
 		if(cmd_length!=1)
 			return LENGTH_ERROR;
 
-		serial_write(0xFC);
-		serial_write(INPUT_VOLTAGE);
-		uint16_t value = (input_voltage_sum>>3)*5;
-		serial_write(value>>8);
-		serial_write(value);
+		uint16_t value = input_voltage_millivolts();
+		reply[0] = INPUT_VOLTAGE;
+		reply[1] = value>>8;
+		reply[2] = value;
+		interface_send(reply, sizeof(reply));
 	}
 	else
 		return INVALID_COMMAND;
@@ -83,39 +118,36 @@ uint8_t monitor_command()
 	return COMMAND_OK;
 }
 
-static inline void adc_complete(uint16_t value)
+static uint16_t track_current_milliamps(void)
 {
-	if(adc_state==1)
-	{
-		// Convert to milliamps: (v*5/1024-2.5)*1000/0.185
-		if(value<512)  // Ignore negative current readings
-			value = 0;
-		else if(value>663)  // Limit range so averaging won't overflow
-			value = 4000;
-		else
-			value = (value-512)*132/5;
-
-		uint8_t i = track_current_head;
-		track_current_sum -= track_current_samples[i];
-		track_current_samples[i] = value;
-		track_current_sum += value;
-		track_current_head = (i+1)&15;
-	}
-	else if(adc_state==3)
+	uint16_t value = track_current_sum;
+
+	// Convert to milliamps: (v/16*5/1024-2.5)*1000/0.185
+	if(value<8192)  // Ignore negative current readings
+		return 0;
+	else
 	{
-		// Convert to centivolts: (v*5/1024)*100*11
-		if(value>744) // Limit range so averaging won't overflow
-			value = 4000;
-		else
-			value = value*43/8;
-
-		uint8_t i = input_voltage_head;
-		input_voltage_sum -= input_voltage_samples[i];
-		input_voltage_samples[i] = value;
-		input_voltage_sum += value;
-		input_voltage_head = (i+1)&15;
+		value -= 8192;
+
+		// multiply by 5000/0.185/16384 = 1.1010011001001b
+		int16_t v_3 = value*3;
+		return (v_3>>1)+(value>>3)+(v_3>>7)+(value>>10)+(v_3>>13);
 	}
+}
 
+static uint16_t input_voltage_millivolts(void)
+{
+	uint16_t value = input_voltage_sum;
+
+	// Convert to millivolts: (v/16*5/1024)*1000*11
+	// multiply by 55000/16384 = 11.0101101101100b
+	uint16_t v_3 = value*3;
+	return v_3+(value>>2)+(v_3>>5)+(v_3>>8)+(v_3>>11);
+}
+
+static inline void adc_complete(uint16_t value)
+{
+	adc_value = value;
 	++adc_state;
 }