DIY Drone using Blues Swan 3.0 and Notecard

#include<Wire.h>
#include <Notecard.h>
#include <NotecardPseudoSensor.h>
#include <MQ135.h>

#define MPU_6050_WHO_AM_I       0x68
#define MPU_6050_REG_26         0x1A    // Digital low pass filter
#define MPU_6050_10_HZ          0x05    // Set register value to 5 to set it to 10 hz 
#define MPU_6050_REG_27         0x1B    // Digital low pass filter
#define MPU_6050_500_PER_SEC    0x08    // Full scale range 500 degrees per second
#define MPU_6050_REF_67         0x43    // Register for reading the gyro measurements
#define MPU_6050_MEASUREMENT_SZ 0x06    // Number of registers to read for gyro measurements
#define MPU_6050_SCALE_SENS     65.5    // Full range sensitivity
#define MPU_6050_POWER_MGMT     0x6B    // Power Management
#define MPU_6050_POWER_UP       0x00    // Power up
#define MPU_6050_TIME_TO_START  250     // Delay to let 
#define GYRO_CALIB_READINGS_CNT 2000
#define DESIRED_GYRO_FACTOR     0.15

#define WIRE_CLK_FREQ           400000
#define SERIAL_BAUD_RATE        57600

#define RC_CHANNELS_LIMIT       8
#define RC_PPM_PIN              D13
#define RC_CHANNEL_DEFAULT_VAL  1000

#define RC_CHANNEL_IDX_ROLL     1
#define RC_CHANNEL_IDX_PITCH    2
#define RC_CHANNEL_IDX_THROTTLE 3
#define RC_CHANNEL_IDX_YAW      4

// The range of a channel's possible values (microseconds)
#define RC_MIN_CHANNEL_VALUE    1000
#define RC_MAX_CHANNEL_VALUE    2000
#define RC_MID_CHANNEL_VALUE    1500
#define RC_MAX_THROTTLE         1800
#define RC_MIN_THROTTLE         1050

// The maximum error (in either direction) in channel value with which the channel value is still considered valid
#define MAX_CHANNELS  10

// The minimum blanking time (microseconds) after which the frame current is considered to be finished
// Should be bigger than RC_MAX_CHANNEL_VALUE + MAX_CHANNELS
#define BLANK_TIME 2100

// The timeout (microseconds) after which the channels which were not updated are considered invalid
#define FAIL_SAFE_TIMEOUT       500000L

#define MOTOR_1_PIN             D5
#define MOTOR_2_PIN             D11
#define MOTOR_3_PIN             D9
#define MOTOR_4_PIN             D12

#define MOTOR_STOP              1000
#define MOTOR_MAX_SPEED         1999

#define MQ135_PIN               A5

#define PRODUCT_ID              "<PRODUCT_ID>"

#define WIFI_ENABLE             0              // 0 : use LTE, 1 : use WiFi
#define WIFI_SSD                "<SSID>"
#define WIFI_PASSWORD           "<PASSWORD>"

#define LOOP_250_HZ             4000          // run control loop every 4 ms (250 HZ) 
#define SENSING_TIMER           30000         // send data every 30 seconds

using namespace blues;
Notecard notecard;
NotecardPseudoSensor notecardSensor(notecard);

MQ135 gasSensor(MQ135_PIN);

/***********************     DATA TYPES . **********************************/

typedef struct {
  uint16_t roll;
  uint16_t pitch;
  uint16_t yaw;
  uint16_t throttle;
}rc_t;

typedef struct {
  float p;
  float i;
  float d;
}pid_t;

typedef struct {
  float output;
  float lastErr;
  float lastI;
  unsigned long lastTime;
}pid_attr_t;

pid_attr_t pidAttrRoll, pidAttrPitch, pidAttrYaw;

typedef struct {
  float roll;
  float pitch;
  float yaw;
}gyro_t;

typedef struct {
  uint16_t m1;
  uint16_t m2;
  uint16_t m3;
  uint16_t m4;
}motors_t;

/***************************************************************************/

/***********************  GLOBAL DECLARATION *******************************/

// PID constant
pid_t kPid;
// Arrays for keeping track of channel values
volatile unsigned *rawValues = NULL;
// A counter variable for determining which channel is being read next
volatile byte pulseCounter = 0;
// A time variable to remember when the last pulse was read
volatile unsigned long microsAtLastPulse = 0;
// Gyro calibration
gyro_t gyroClibration;
// RC reading
rc_t rc;

/***********************  RC READ SECTION **********************************/

void rcReaderInit() {
    // Setup an array for storing channel values
    rawValues = new unsigned [RC_CHANNELS_LIMIT];
    for (int i = 0; i < RC_CHANNELS_LIMIT; ++i) {
        rawValues[i] = 0;
    }
    // Attach an interrupt to the pin
    pinMode(RC_PPM_PIN, INPUT);
    attachInterrupt(digitalPinToInterrupt(RC_PPM_PIN), handleInterrupt, RISING);
}

void handleInterrupt(void) {
    // Remember the current micros() and calculate the time since the last pulseReceived()
    unsigned long previousMicros = microsAtLastPulse;
    microsAtLastPulse = micros();
    unsigned long time = microsAtLastPulse - previousMicros;

    if (time > BLANK_TIME) {
        // Blank detected: restart from channel 1 
        pulseCounter = 0;
    }
    else {
        // Store times between pulses as channel values
        if (pulseCounter < RC_CHANNELS_LIMIT) {
            rawValues[pulseCounter] = time;
            ++pulseCounter;
        }
    }
}

unsigned rawChannelValue(byte channel) {
    // Check for channel's validity and return the latest raw channel value or 0
    unsigned value = 0;
    if (channel >= 1 && channel <= RC_CHANNELS_LIMIT) {
        noInterrupts();
        value = rawValues[channel-1];
        interrupts();
    }
    return value;
}

unsigned latestValidChannelValue(byte channel, unsigned defaultValue) {
    // Check for channel's validity and return the latest valid channel value or defaultValue.
    unsigned value = defaultValue;
  unsigned long timeout;
  noInterrupts();
  timeout = micros() - microsAtLastPulse;
  interrupts();
    if ((timeout < FAIL_SAFE_TIMEOUT) && (channel >= 1) && (channel <= RC_CHANNELS_LIMIT)) {
        noInterrupts();
        value = rawValues[channel-1];
    interrupts();
    if (value >= RC_MIN_CHANNEL_VALUE - MAX_CHANNELS && value <= RC_MAX_CHANNEL_VALUE + MAX_CHANNELS) {
      value = constrain(value, RC_MIN_CHANNEL_VALUE, RC_MAX_CHANNEL_VALUE);
    }
        else value = defaultValue;
    }
    return value;
}

rc_t getRc() {
  rc_t rc;
  rc.roll     = latestValidChannelValue(RC_CHANNEL_IDX_ROLL, RC_CHANNEL_DEFAULT_VAL);
  rc.pitch    = latestValidChannelValue(RC_CHANNEL_IDX_PITCH, RC_CHANNEL_DEFAULT_VAL);
  rc.yaw      = latestValidChannelValue(RC_CHANNEL_IDX_YAW, RC_CHANNEL_DEFAULT_VAL);
  rc.throttle = latestValidChannelValue(RC_CHANNEL_IDX_THROTTLE, RC_CHANNEL_DEFAULT_VAL);
  rc.throttle = rc.throttle > RC_MAX_THROTTLE ? RC_MAX_THROTTLE : rc.throttle;
  
  return rc;
}

void rcCheck() {
  rc_t rc = getRc();

  while((rc.throttle > RC_MIN_THROTTLE) || (rc.yaw > RC_MIN_THROTTLE) || 
    (rc.roll > RC_MIN_THROTTLE) || (rc.pitch > RC_MIN_THROTTLE)) {
        rc = getRc();
  }

}

void testRc() {
    rc_t rc = getRc();
    Serial.print(" throttle = ");
    Serial.print(rc.throttle);
    Serial.print(" roll = ");
    Serial.print(rc.roll);
    Serial.print(" pitch = ");
    Serial.print(rc.pitch);
    Serial.print(" yaw = ");
    Serial.println(rc.yaw);
    delay(20);
}

/*****************************************************************************/

/***********************  GYRO READ SECTION **********************************/
gyro_t getRawGyro() {
  gyro_t rawGyro;
  
  // Set Low pass filter
  Wire.beginTransmission(MPU_6050_WHO_AM_I);  // Start I2C communication with the gyro
  Wire.write(MPU_6050_REG_26);
  Wire.write(MPU_6050_10_HZ);
  Wire.endTransmission();

  // Set scale range
  Wire.beginTransmission(MPU_6050_WHO_AM_I);  // Start I2C communication with the gyro
  Wire.write(MPU_6050_REG_27);
  Wire.write(MPU_6050_500_PER_SEC);
  Wire.endTransmission();
  
  Wire.beginTransmission(MPU_6050_WHO_AM_I);  // Start I2C communication with the gyro
  Wire.write(MPU_6050_REF_67);
  Wire.endTransmission();

  Wire.requestFrom(MPU_6050_WHO_AM_I, MPU_6050_MEASUREMENT_SZ);

  int16_t gyroY = Wire.read() << 8 | Wire.read();
  int16_t gyroX = Wire.read() << 8 | Wire.read();
  int16_t gyroZ = Wire.read() << 8 | Wire.read();

  rawGyro.roll -= (float) gyroX / MPU_6050_SCALE_SENS;
  rawGyro.pitch = (float) gyroY / MPU_6050_SCALE_SENS;
  rawGyro.yaw = (float) gyroZ / MPU_6050_SCALE_SENS;

  return rawGyro;
}

gyro_t getGyro() {
  gyro_t gyro = getRawGyro();

  gyro.roll -= gyroClibration.roll;
  gyro.pitch -= gyroClibration.pitch;
  gyro.yaw -= gyroClibration.yaw;

  return gyro;
}

void calibrateGyro() {
  for(int i = 0; i < GYRO_CALIB_READINGS_CNT; i++) {
    gyro_t rawGyro = getRawGyro();
    gyroClibration.roll += rawGyro.roll;
    gyroClibration.pitch += rawGyro.pitch;
    gyroClibration.yaw += rawGyro.yaw;
    delay(1);
  }

  gyroClibration.roll /= GYRO_CALIB_READINGS_CNT;
  gyroClibration.pitch /= GYRO_CALIB_READINGS_CNT;
  gyroClibration.yaw /= GYRO_CALIB_READINGS_CNT;
}

void initGyro() {
  Wire.setClock(WIRE_CLK_FREQ);
  Wire.begin();
  delay(MPU_6050_TIME_TO_START);
  Wire.beginTransmission(MPU_6050_WHO_AM_I); 
  Wire.write(MPU_6050_POWER_MGMT);
  Wire.write(MPU_6050_POWER_UP);
  Wire.endTransmission();
}

void testGyro(gyro_t gyro){
  Serial.print("Roll rate = ");
  Serial.print(gyro.roll);
  Serial.print(" Pitch rate = ");
  Serial.print(gyro.pitch);
  Serial.print(" Yaw rate = ");
  Serial.println(gyro.yaw);  
}

/*****************************************************************************/
/***********************  MOTOR CONTROL SECTION ******************************/

void updateMotorSpeeds(motors_t motors) {
  analogWrite(MOTOR_1_PIN, motors.m1);
  analogWrite(MOTOR_2_PIN, motors.m2);
  analogWrite(MOTOR_3_PIN, motors.m3);
  analogWrite(MOTOR_4_PIN, motors.m4);
}


void runMotors(uint16_t throttle, float pitch, float roll, float yaw) {
  motors_t motors;
  if(throttle > RC_MIN_THROTTLE) {
    motors.m1 = 1.024 * (throttle - roll - pitch - yaw);
    motors.m1 = motors.m1 > MOTOR_MAX_SPEED ? MOTOR_MAX_SPEED : motors.m1;
    motors.m2 = 1.024 * (throttle - roll + pitch + yaw);
    motors.m2 = motors.m2 > MOTOR_MAX_SPEED ? MOTOR_MAX_SPEED : motors.m2;
    motors.m3 = 1.024 * (throttle + roll + pitch - yaw);
    motors.m3 = motors.m3 > MOTOR_MAX_SPEED ? MOTOR_MAX_SPEED : motors.m3;
    motors.m4 = 1.024 * (throttle + roll - pitch + yaw);
    motors.m4 = motors.m4 > MOTOR_MAX_SPEED ? MOTOR_MAX_SPEED : motors.m4;
  } else {
    motors.m1 = MOTOR_STOP;
    motors.m2 = MOTOR_STOP;
    motors.m3 = MOTOR_STOP;
    motors.m4 = MOTOR_STOP;
  }

  updateMotorSpeeds(motors);
}

/*****************************************************************************/
/****************************  PID SECTION ***********************************/
void computePid(float err, pid_t k, pid_attr_t &attr) {
  float pTerm = k.p * err;
  
  float iTerm = attr.lastI + k.i * (err + attr.lastErr) * 0.004/2;
  iTerm = iTerm > 400 ? 400 : iTerm;
  iTerm = iTerm < -400 ? -400 : iTerm;
  
  float dTerm = k.d * (err - attr.lastErr) /  0.004;
  float output = pTerm + iTerm + dTerm;
  
  if (output>400) output = 400;
  else if (output <-400) output = -400;
  
  attr.output = output;
  attr.lastErr = err;
  attr.lastI = iTerm;
  attr.lastTime = millis();
}

void runPid(rc_t rc) {
  gyro_t desired, err;  
 
  desired.roll  = DESIRED_GYRO_FACTOR * (rc.roll - RC_MID_CHANNEL_VALUE);
  desired.pitch = DESIRED_GYRO_FACTOR * (rc.pitch - RC_MID_CHANNEL_VALUE);
  desired.yaw   = DESIRED_GYRO_FACTOR * (rc.yaw - RC_MID_CHANNEL_VALUE);
  
  gyro_t actual = getGyro();
  err.roll = desired.roll - actual.roll;
  computePid(err.roll, kPid, pidAttrRoll);
  err.pitch = desired.pitch - actual.pitch;
  computePid(err.pitch, kPid, pidAttrPitch);
  err.yaw = desired.yaw - actual.yaw;
  computePid(err.yaw, kPid, pidAttrYaw);

  runMotors(rc.throttle, pidAttrPitch.output, pidAttrRoll.output, pidAttrYaw.output);
}
/*****************************************************************************/
/*************************  NOTEHUB SECTION **********************************/
void initConnectivity() {
  J *req;
  #if WIFI_ENABLE
  req = notecard.newRequest("card.wifi");
  JAddStringToObject(req, "ssid", WIFI_SSD);
  JAddStringToObject(req, "password", WIFI_PASSWORD);
  #else
  req = notecard.newRequest("card.wireless");
  #endif
  notecard.sendRequest(req);
}

void initNotehub() {
  J *req = notecard.newRequest("hub.set");
  JAddStringToObject(req, "product", PRODUCT_ID);
  JAddStringToObject(req, "mode", "continuous");
  notecard.sendRequest(req);
}

void initLocation() {
  J *req = notecard.newRequest("card.location.mode");
  JAddStringToObject(req, "mode", "continuous");
  notecard.sendRequest(req);
}

void sendSensorsData() {
  float temperature = notecardSensor.temp();
  float humidity = notecardSensor.humidity();
  float airQuality = gasSensor.getCorrectedPPM(temperature, humidity); 
  J *location = notecard.requestAndResponse(notecard.newRequest("card.location"));
  J *req = notecard.newRequest("note.add");
  JAddStringToObject(req, "file", "drone.qo");
  J *body = JCreateObject();
  JAddNumberToObject(body, "temp", temperature);
  JAddNumberToObject(body, "humidity", humidity);
  JAddNumberToObject(body, "airquality", airQuality);
  JAddItemToObject(body, "location", location);
  JAddItemToObject(req, "body", body);
  notecard.sendRequest(req);
  notecard.deleteResponse(location);
}

/*****************************************************************************/

void setup() {
  kPid.p = 0.5;
  kPid.i = 1;
  kPid.d = 0.05;
  
  pinMode(MOTOR_1_PIN, OUTPUT);
  pinMode(MOTOR_2_PIN, OUTPUT);
  pinMode(MOTOR_3_PIN, OUTPUT);
  pinMode(MOTOR_4_PIN, OUTPUT);
  
  Serial.begin(SERIAL_BAUD_RATE);

  initGyro();
  calibrateGyro();
  
  analogReadResolution(8);
  analogWriteResolution(12);
  analogWriteFrequency(250);
  
  rcReaderInit();
  delay(50);

  notecard.begin();
  initConnectivity();
  initNotehub();
  initLocation();
  
  rcCheck();
  
  pinMode(LED_BUILTIN, OUTPUT);
  digitalWrite(LED_BUILTIN, HIGH);
}

unsigned long loopTimer = micros();
unsigned long sensingTimer = millis();

void loop() {
  rc_t rc = getRc();
  
  if(millis() - sensingTimer > SENSING_TIMER) {
    sendSensorsData();
    sensingTimer = millis();
  }
  
  runPid(rc);
  
  while(micros() - loopTimer < LOOP_250_HZ);
  loopTimer = micros();
}