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 volatile uint16_t track_current_sum = 0;
  10 static uint16_t overcurrent_limit = 8796;
  11 static uint8_t overcurrent_sent = 0;
  12
  13 static uint16_t input_voltage_samples[16] = { 0 };
  14 static uint8_t input_voltage_head = 0;
  15 static volatile uint16_t input_voltage_sum = 0;
  16
  17 static volatile uint8_t adc_state = 0;
  18 static volatile uint16_t adc_value = 0;
  19
  20 static uint16_t track_current_milliamps(void);
  21 static uint16_t input_voltage_millivolts(void);
  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>overcurrent_limit)
  44                         {
  45                                 output_set_power(0);
  46                                 PORTB |= 0x02;
  47                                 if(!overcurrent_sent)
  48                                 {
  49                                         overcurrent_sent = 1;
  50                                         interface_send1(OVERCURRENT);
  51                                 }
  52                         }
  53                         else if(overcurrent_sent && output_is_power_on())
  54                         {
  55                                 PORTB &= ~0x02;
  56                                 overcurrent_sent = 0;
  57                         }
  58                 }
  59                 else if(adc_state==4)
  60                 {
  61                         uint8_t i = input_voltage_head;
  62                         input_voltage_sum -= input_voltage_samples[i];
  63                         input_voltage_samples[i] = value;
  64                         input_voltage_sum += value;
  65                         input_voltage_head = (i+1)&15;
  66                 }
  67
  68                 adc_state = (adc_state+1)&3;
  69                 adc_read_async(adc_state>>1);
  70         }
  71 }
  72
  73 uint8_t monitor_command(const uint8_t *cmd_buf, uint8_t cmd_length)
  74 {
  75         if(cmd_buf[0]==READ_TRACK_CURRENT)
  76         {
  77                 uint8_t reply[3];
  78
  79                 if(cmd_length!=1)
  80                         return LENGTH_ERROR;
  81
  82                 uint16_t value = track_current_milliamps();
  83                 reply[0] = TRACK_CURRENT;
  84                 reply[1] = value>>8;
  85                 reply[2] = value;
  86                 interface_send(reply, sizeof(reply));
  87         }
  88         else if(cmd_buf[0]==SET_OVERCURRENT_LIMIT)
  89         {
  90                 if(cmd_length!=3)
  91                         return LENGTH_ERROR;
  92
  93                 uint16_t value = (cmd_buf[1]<<8) | cmd_buf[2];
  94                 if(value>4000)  // Safe maximum value
  95                         return INVALID_VALUE;
  96
  97                 // Convert from milliamps: (512+v/1000*0.185/5*1024)*16
  98                 // multiply by 16384*0.185/5000 = 0.1001101100110b
  99                 uint16_t v_3 = value*3;
 100                 overcurrent_limit = 8192+(value>>1)+(v_3>>5)+(v_3>>8)+(v_3>>12);
 101         }
 102         else if(cmd_buf[0]==READ_INPUT_VOLTAGE)
 103         {
 104                 uint8_t reply[3];
 105
 106                 if(cmd_length!=1)
 107                         return LENGTH_ERROR;
 108
 109                 uint16_t value = input_voltage_millivolts();
 110                 reply[0] = INPUT_VOLTAGE;
 111                 reply[1] = value>>8;
 112                 reply[2] = value;
 113                 interface_send(reply, sizeof(reply));
 114         }
 115         else
 116                 return INVALID_COMMAND;
 117
 118         return COMMAND_OK;
 119 }
 120
 121 static uint16_t track_current_milliamps(void)
 122 {
 123         uint16_t value = track_current_sum;
 124
 125         // Convert to milliamps: (v/16*5/1024-2.5)*1000/0.185
 126         if(value<8192)  // Ignore negative current readings
 127                 return 0;
 128         else
 129         {
 130                 value -= 8192;
 131
 132                 // multiply by 5000/0.185/16384 = 1.1010011001001b
 133                 int16_t v_3 = value*3;
 134                 return (v_3>>1)+(value>>3)+(v_3>>7)+(value>>10)+(v_3>>13);
 135         }
 136 }
 137
 138 static uint16_t input_voltage_millivolts(void)
 139 {
 140         uint16_t value = input_voltage_sum;
 141
 142         // Convert to millivolts: (v/16*5/1024)*1000*11
 143         // multiply by 55000/16384 = 11.0101101101100b
 144         uint16_t v_3 = value*3;
 145         return v_3+(value>>2)+(v_3>>5)+(v_3>>8)+(v_3>>11);
 146 }
 147
 148 static inline void adc_complete(uint16_t value)
 149 {
 150         adc_value = value;
 151         ++adc_state;
 152 }
 153
 154 ADC_SET_CALLBACK(adc_complete)