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 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 = 1000<<4;
  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
  19 void monitor_init()
  20 {
  21         adc_init();
  22 }
  23
  24 void monitor_check()
  25 {
  26         if(!(adc_state&1))
  27         {
  28                 ++adc_state;
  29                 adc_read_async(adc_state>>1);
  30         }
  31
  32         if(track_current_sum>overcurrent_limit)
  33         {
  34                 output_set_power(0);
  35                 if(!overcurrent_sent)
  36                 {
  37                         overcurrent_sent = 1;
  38                         serial_write(0xFE);
  39                         serial_write(OVERCURRENT);
  40                 }
  41         }
  42         else
  43                 overcurrent_sent = 0;
  44 }
  45
  46 uint8_t monitor_command()
  47 {
  48         if(cmd_buf[0]==READ_TRACK_CURRENT)
  49         {
  50                 if(cmd_length!=1)
  51                         return LENGTH_ERROR;
  52
  53                 serial_write(0xFC);
  54                 serial_write(TRACK_CURRENT);
  55                 uint16_t value = track_current_sum>>4;
  56                 serial_write(value>>8);
  57                 serial_write(value);
  58         }
  59         else if(cmd_buf[0]==SET_OVERCURRENT_LIMIT)
  60         {
  61                 if(cmd_length!=3)
  62                         return LENGTH_ERROR;
  63
  64                 if(cmd_buf[1]&0xF0)
  65                         return INVALID_VALUE;
  66
  67                 overcurrent_limit = (cmd_buf[1]<<12) | (cmd_buf[2]<<4);
  68         }
  69         else if(cmd_buf[0]==READ_INPUT_VOLTAGE)
  70         {
  71                 if(cmd_length!=1)
  72                         return LENGTH_ERROR;
  73
  74                 serial_write(0xFC);
  75                 serial_write(INPUT_VOLTAGE);
  76                 uint16_t value = (input_voltage_sum>>3)*5;
  77                 serial_write(value>>8);
  78                 serial_write(value);
  79         }
  80         else
  81                 return INVALID_COMMAND;
  82
  83         return COMMAND_OK;
  84 }
  85
  86 static inline void adc_complete(uint16_t value)
  87 {
  88         if(adc_state==1)
  89         {
  90                 // Convert to milliamps: (v*5/1024-2.5)*1000/0.185
  91                 if(value<512)  // Ignore negative current readings
  92                         value = 0;
  93                 else if(value>663)  // Limit range so averaging won't overflow
  94                         value = 4000;
  95                 else
  96                         value = (value-512)*132/5;
  97
  98                 uint8_t i = track_current_head;
  99                 track_current_sum -= track_current_samples[i];
 100                 track_current_samples[i] = value;
 101                 track_current_sum += value;
 102                 track_current_head = (i+1)&15;
 103         }
 104         else if(adc_state==3)
 105         {
 106                 // Convert to centivolts: (v*5/1024)*100*11
 107                 if(value>744) // Limit range so averaging won't overflow
 108                         value = 4000;
 109                 else
 110                         value = value*43/8;
 111
 112                 uint8_t i = input_voltage_head;
 113                 input_voltage_sum -= input_voltage_samples[i];
 114                 input_voltage_samples[i] = value;
 115                 input_voltage_sum += value;
 116                 input_voltage_head = (i+1)&15;
 117         }
 118
 119         ++adc_state;
 120 }
 121
 122 ADC_SET_CALLBACK(adc_complete)