#include "sbus.h"

#define THR_CH 2
#define ARM_CH 4
#define ANGLE_CH 6

#define ANGLE_ENABLE 1000
#define ARM_ENABLE 1900
#define ARM_DISABLE 1000


bfs::SbusTx sbus_tx(&Serial1, 6, 7, true, false);
bfs::SbusData data;
int counter = 0;

int drone_to_sbus(int k) {
  return (k - 880) * 8 / 5;
}

void setup() {
    Serial.begin(115200);
    // Serial1.begin(115200, SERIAL_8N1, 6, 7);

    sbus_tx.Begin();

    data.failsafe = false;
    data.ch17 = true;
    data.ch18 = true;
    data.lost_frame = false;

    delay(1000);
    Serial.println("setup finished ESP-C3-Zero");
}


void loop() {
  counter += 1;
  delay(20);

  if (counter > 1500) {
    return;
  }

  data.failsafe = (counter >= 350);

  for (int j = 0; j < 16; j++) {
    data.ch[j] = 992;
  }
  data.ch[THR_CH] = drone_to_sbus(1150);
  data.ch[ARM_CH] = drone_to_sbus(ARM_ENABLE);
  data.ch[ANGLE_CH] = drone_to_sbus(ANGLE_ENABLE);
  sbus_tx.data(data);
  sbus_tx.Write();

  Serial.printf("%d: sent ch0=%d\n", counter, data.ch[0]);

}