arducontrol/monitor.c

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