arducontrol/monitor.c

   1 #include "adc.h"
   2 #include "interface.h"
   3 #include "monitor.h"
   4 #include "output.h"
   5 #include "serial.h"
   6
   7 static uint16_t track_current_samples[16] = { 0 };
   8 static uint8_t track_current_head = 0;
   9 static uint16_t track_current_sum = 0;
  10 static uint16_t track_current_peak = 0;
  11 static uint16_t overcurrent_limit = 9707;
  12 static uint8_t overcurrent_sent = 0;
  13
  14 static uint16_t input_voltage_samples[16] = { 0 };
  15 static uint8_t input_voltage_head = 0;
  16 static uint16_t input_voltage_sum = 0;
  17
  18 static volatile uint8_t adc_state = 0;
  19 static volatile uint16_t adc_value = 0;
  20
  21 static uint16_t current_milliamps(uint16_t);
  22
  23 void monitor_init(void)
  24 {
  25         DDRB |= 0x02;
  26         adc_init();
  27 }
  28
  29 void monitor_check(void)
  30 {
  31         if(!(adc_state&1))
  32         {
  33                 uint16_t value = adc_value;
  34
  35                 if(adc_state==2)
  36                 {
  37                         uint8_t i = track_current_head;
  38                         track_current_sum -= track_current_samples[i];
  39                         track_current_samples[i] = value;
  40                         track_current_sum += value;
  41                         track_current_head = (i+1)&15;
  42
  43                         if(track_current_sum>track_current_peak)
  44                                 track_current_peak = track_current_sum;
  45
  46                         if(track_current_sum>overcurrent_limit)
  47                         {
  48                                 output_set_power(0);
  49                                 PORTB |= 0x02;
  50                                 if(!overcurrent_sent)
  51                                 {
  52                                         overcurrent_sent = 1;
  53                                         interface_send1(OVERCURRENT);
  54                                 }
  55                         }
  56                         else if(overcurrent_sent && output_is_power_on())
  57                         {
  58                                 PORTB &= ~0x02;
  59                                 overcurrent_sent = 0;
  60                         }
  61                 }
  62                 else if(adc_state==4)
  63                 {
  64                         uint8_t i = input_voltage_head;
  65                         input_voltage_sum -= input_voltage_samples[i];
  66                         input_voltage_samples[i] = value;
  67                         input_voltage_sum += value;
  68                         input_voltage_head = (i+1)&15;
  69                 }
  70
  71                 adc_state = (adc_state+1)&3;
  72                 adc_read_async(adc_state>>1);
  73         }
  74 }
  75
  76 uint8_t monitor_command(const uint8_t *cmd_buf, uint8_t cmd_length)
  77 {
  78         if(cmd_buf[0]==READ_TRACK_CURRENT)
  79         {
  80                 if(cmd_length!=1)
  81                         return LENGTH_ERROR;
  82
  83                 uint16_t value = monitor_track_current();
  84                 uint16_t peak = current_milliamps(track_current_peak);
  85                 track_current_peak = 0;
  86
  87                 uint8_t reply[5];
  88                 reply[0] = TRACK_CURRENT;
  89                 reply[1] = value>>8;
  90                 reply[2] = value;
  91                 reply[3] = peak>>8;
  92                 reply[4] = peak;
  93                 interface_send(reply, sizeof(reply));
  94         }
  95         else if(cmd_buf[0]==SET_OVERCURRENT_LIMIT)
  96         {
  97                 if(cmd_length!=3)
  98                         return LENGTH_ERROR;
  99
 100                 uint16_t value = (cmd_buf[1]<<8) | cmd_buf[2];
 101                 if(value>4000)  // Safe maximum value
 102                         return INVALID_VALUE;
 103
 104                 // Convert from milliamps: (512+v/1000*0.185/5*1024)*16
 105                 // multiply by 16384*0.185/5000 = 0.1001101100110b
 106                 uint16_t v_3 = value*3;
 107                 overcurrent_limit = 8192+(value>>1)+(v_3>>5)+(v_3>>8)+(v_3>>12);
 108         }
 109         else if(cmd_buf[0]==READ_INPUT_VOLTAGE)
 110         {
 111                 if(cmd_length!=1)
 112                         return LENGTH_ERROR;
 113
 114                 uint16_t value = monitor_input_voltage();
 115                 uint8_t reply[3];
 116                 reply[0] = INPUT_VOLTAGE;
 117                 reply[1] = value>>8;
 118                 reply[2] = value;
 119                 interface_send(reply, sizeof(reply));
 120         }
 121         else
 122                 return INVALID_COMMAND;
 123
 124         return COMMAND_OK;
 125 }
 126
 127 static uint16_t current_milliamps(uint16_t value)
 128 {
 129         // Convert to milliamps: (v/16*5/1024-2.5)*1000/0.185
 130         if(value<8192)  // Ignore negative current readings
 131                 return 0;
 132         else
 133         {
 134                 value -= 8192;
 135
 136                 // multiply by 5000/0.185/16384 = 1.1010011001001b
 137                 int16_t v_3 = value*3;
 138                 return (v_3>>1)+(value>>3)+(v_3>>7)+(value>>10)+(v_3>>13);
 139         }
 140 }
 141
 142 uint16_t monitor_track_current(void)
 143 {
 144         return current_milliamps(track_current_sum);
 145 }
 146
 147 uint16_t monitor_input_voltage(void)
 148 {
 149         uint16_t value = input_voltage_sum;
 150
 151         // Convert to millivolts: (v/16*5/1024)*1000*11
 152         // multiply by 55000/16384 = 11.0101101101100b
 153         uint16_t v_3 = value*3;
 154         return v_3+(value>>2)+(v_3>>5)+(v_3>>8)+(v_3>>11);
 155 }
 156
 157 static inline void adc_complete(uint16_t value)
 158 {
 159         adc_value = value;
 160         ++adc_state;
 161 }
 162
 163 ADC_SET_CALLBACK(adc_complete)