// solarRegulator sketch
// for Mike Payne's solar powered plane project
// by ceptimus. New Year's Day 2018.  Modified to work without pot on A2: 2018-05-29
// ideally runs on a 3.3V Pro Mini 8MHz but now (2018-07-30) also supports 16MHz operation (compile time operation, see F_CPU test)
// 2018-07-30 Modified to use INTERNAL voltage reference (1.1V) to provide better stability when working near 3.3V input and allow same code to run on 3.3V or 5V Arduinos
// throttle channel input from receiver on A1
// throttle output to ESC on pin 10
// if using an ESC that does not have a BEC, the Pro Mini's regulator can be used as a BEC, but is only rated up to 0.5A, so very low power servos must be used.
// safer to use an external BEC, with a higher current capacity, so that a briefly-overloading servo is less likely to reset the Arduino

// solar cell input voltage into A3 via potential divider
// V.solar ----/\/\/\/\----+----/\/\/\/\---- GND
//                R1       |       R2
//                        A3
// Recommended values:
//                     R1 = 68K
//                     R2 = 10K
// Gives theoretical input range of 1.1 * (68 + 10) / 10 = 8.58V  good for up to 12 solar cells.  Adjust R1 and/or R2 if more cells used
// 8.58V range also allows 2-cell LiPo battery to be used for tests (fully charged 2-cell is 8.4V)

#define R1 68000.0
#define R2 10000.0

// The sketch attempts to control the ESC so as to keep the solar cell voltage above OPERATING_VOLTAGE.  Here are some good defaults based on the Arduino type but you can set your own limit if you wish
// (For example, some ESCs receivers or servos may not be happy working at 3.3V even if the Arduino is happy to do so)
#if F_CPU > 8000000
  #define OPERATING_VOLTAGE 5.0
#else
  #define OPERATING_VOLTAGE 3.3
#endif

#define SMOOTHED_ADC_COUNT_TO_VOLTS (1.1 * (R1 + R2) / R2 / 32736.0)

// ramp up rate for increasing throttle output to ESC.  Lower numbers give a faster rate.  1 is the lowest valid value.  2 is twice as slow, 3 three times as slow and so on.  A value of 0 gives an effective value of 256.  2 is a good starting point
#define RAMP_UP_RATE 2

// ramp down rate for decreasing throttle output to ESC when available voltage is too low.  Higher numbers give a faster rate.  100.0 is a good starting point
#define RAMP_DOWN_RATE 100.0

const float mptRatio = 0.85;  // maximum power transfer ratio.  This is multiplied by the highest (minimum load) voltage seen to give throttleBackVoltage.  85% is said to be optimum.

uint16_t filteredA3; // remove noise from analog inputs by simple 1st order low pass filters ("leaky integrators")


volatile uint16_t rxThrottlePulseWidth = 0; // this will be updated by the interrupt routine when a signal is received from the receiver. 0 is a flag for 'not received yet'
int minRxThrottlePulseWidth = 2200;
int maxRxThrottlePulseWidth = 800; // these will keep track of the actual (within acceptable range) minimum and maximum values seen arriving from the receiver
int maxEscPulseWidth = 0; // this value is updated by the program so as to always keep the solarCellVoltage at or above the selected minimum.  0 is a flag for 'not calculated yet'
float throttleBackVoltage = OPERATING_VOLTAGE; // minimum voltage (to allow Arduino and Rx to always be powered)  Adapts to fraction of highest (minimum load) voltage seen via A3.  85% is reckoned to give max power transfer
float maximumVoltageSeen = OPERATING_VOLTAGE / mptRatio;

void setup() {
  analogReference(INTERNAL); // use the Arduino's built-in analog reference voltage of 1.1V - so the measuring range of an analog input will now be 0 to 1.1V
  Serial.begin(115200);
  pinMode(A1, INPUT); // throttle input from receiver
  digitalWrite(10, LOW);
  pinMode(10, OUTPUT); // throttle output to ESC
  TCCR1A = 0x00; // intialize timer counter 1 ready to run in normal mode where it counts up and can generate an interrupt when it wraps from 0xFFFF to 0x0000 counts
  TCCR1C = 0x00;

  // enable pinChange interrupt on A1
  PCMSK1 = 0x02; // mask bits of PORTC (A0 - A7) so that only A1 generate pinchange interrupts
  PCICR |= 0x02; // enable pinchange interrupts for PORTC

  filteredA3 = 0;  // start A3 filter on minimum so that any start-up noise doesn't cause maximumVoltageSeen to be set artificially high
}

void loop() {
  static uint8_t rampUpRate = RAMP_UP_RATE;

  // update analog input filtered value - this is bit-shifted 5 bits so have range 0 - 1023 * 2^5 = 0 - 32736
  filteredA3 -= filteredA3 >> 5;
  filteredA3 += analogRead(A3);

  float solarCellVoltage = filteredA3 * SMOOTHED_ADC_COUNT_TO_VOLTS; // measured solar cell voltage
  if ((millis() < 10000UL) && (solarCellVoltage > maximumVoltageSeen)) { // ignore surges in voltage, such as can be caused by sudden throttling back, after first 10 seconds of operation
    maximumVoltageSeen = solarCellVoltage;
    throttleBackVoltage = mptRatio * maximumVoltageSeen;
  }

  // when within-acceptable-range pulse widths arrive from the receiver, keep track of the minimum and maximum values seen so far
  uint16_t rpw = rxThrottlePulseWidth;
  if (rpw <= 2200 && rpw > maxRxThrottlePulseWidth) {
    maxRxThrottlePulseWidth = rpw;
  }
  if (rpw >= 800 && rpw < minRxThrottlePulseWidth) {
    minRxThrottlePulseWidth = rpw;
  }

  if (maxRxThrottlePulseWidth >= minRxThrottlePulseWidth) { // don't alter maxEscPulseWidth until we've begun receiving throttle input from the receiver
    uint16_t mpw = maxEscPulseWidth;
    if (solarCellVoltage <= throttleBackVoltage) { // need to reduce maxEscPulseWidth so as to reduce power draw on solar cells
      mpw -= (uint16_t)((throttleBackVoltage - solarCellVoltage) * RAMP_DOWN_RATE); // ramp power down pretty quickly (this is a potential emergency!) but proportional to the severity of the under voltage condition
    } else if (rpw > maxEscPulseWidth) { // extra power is available and user is requesting more power than maxEscPulseWidth currently permits
      if (!--rampUpRate) {
        ++mpw; // ramp up the ESC signal fairly slowly
        rampUpRate = RAMP_UP_RATE;
      }
    } else { // user is requesting less power than maxEscPulseWidth
      mpw = rpw; // set maxEscPulseWidth to current throttle position.  This is necessary to prevent fast ramping up of power next time the user increases the throttle - lighting conditions may have changed and a fast ramp up may trip out the ESC
    }
    if (mpw < minRxThrottlePulseWidth) {
      mpw = minRxThrottlePulseWidth;
    } else if (mpw > maxRxThrottlePulseWidth) {
      mpw = maxRxThrottlePulseWidth;
    }
    if (mpw != maxEscPulseWidth) {
      noInterrupts(); // altering a 16-bit value used by the pinchange/timer interrupts so disable interrupts...
      maxEscPulseWidth = mpw; // ...while updating the value to prevent possible glitches when the value changes from, say, 1791 to 1792 (0x05FF to 0x0600)
      interrupts();
    }
  }

  // probably best to leave these debugging print commands in place, even if you're not using them, as they affect the loop time
  Serial.print(solarCellVoltage);
  Serial.print("V\t");
  Serial.print(maximumVoltageSeen);
  Serial.print("V\t");
  Serial.print(throttleBackVoltage);
  Serial.print("V\t");
  Serial.print(rxThrottlePulseWidth);
  Serial.print("us\t");
  Serial.print(maxEscPulseWidth);
  Serial.print("us\n");
}

ISR(PCINT1_vect) { // pinchange occured on A1
  static uint16_t pulseRisingEdgeTime;
  if (PINC & 0x02) { // positive edge of PWM signal from receiver
    PORTB |= 0x04; // echo positive edge out to servo on pin 10
    if (maxEscPulseWidth) { // if main loop has seen valid receiver signals and calculated a maximum ESC pulse width
#if F_CPU > 8000000
      pulseRisingEdgeTime = ~(maxEscPulseWidth << 1); // when running with a 16MHz crystal, convert from microseconds to half-microseconds to suit counter speed
#else
      pulseRisingEdgeTime = ~maxEscPulseWidth; // using the bitwise NOT of the target count as a starting point causes the timer/counter wrap interrupt to occur after target counter clocks
#endif
    } else {
      pulseRisingEdgeTime = 0x0001;
    }

    TCNT1 = pulseRisingEdgeTime;
    TIMSK1 = 0x01; // enable timer interrupt
    TCCR1B = 0x02; // start timer running at 1MHz (8MHz crystal) or 2MHz (16MHz crystal) - it will generate an interrupt after maxEscPulseWidth microsecond
  } else { // negative edge of PWM signal from receiver
    PORTB &= ~0x04; // echo negative edge out to ESC on pin 10 (unless the timer limiting the pulse width to maxEscPulseWidth occurred first)
    uint16_t pulseFallingEdgeTime = TCNT1;
    TCCR1B = 0x00; // stop timer1 now until we need it again
#if F_CPU > 8000000
    rxThrottlePulseWidth =  (pulseFallingEdgeTime - pulseRisingEdgeTime) >> 1; // when running with a 16MHz crystal, divide counter by 2 to get microseconds
#else
    rxThrottlePulseWidth = pulseFallingEdgeTime - pulseRisingEdgeTime; // no need to worry about a TCNT1 wrap occurring during the pulse - with 16-bit unsigned integers the answer still comes out correctly
#endif
  }
}

ISR(TIMER1_OVF_vect) { // timer1 has triggered after maxEscPulseWidth microseconds
  TIMSK1 = 0; // prevent further interrupts from timer/counter 1 till reenabled
  PORTB &= ~0x04; // send negative edge out to ESC on pin 10 (unless the falling edge from the receiver signal occurred first)
}