Hackster is hosting Hackster Holidays, Ep. 1: Welcome & Giveaway Drawing. Watch now!Tune in to Hackster Holidays, Ep. 1 now!
Abhishek Chavan
Published © MIT

DIY Drone using Blues Swan 3.0 and Notecard

Craft your own Air-Quality & Weather Monitoring Drone: DIY tech meets environmental awareness. Build, sense, explore, and contribute!

IntermediateFull instructions provided6 hours2,275
DIY Drone using Blues Swan 3.0 and Notecard

Things used in this project

Hardware components

Blues Starter Kit for EMEA
×1
MT2204 - 2300 KV Brushless DC Motor
×4
12A ESC
×4
QAV 250 Drone Frame
×1
11.1v 1500 mAh LiPo Battery
×1
5045 CW Propellers
×2
5045 CCW Propeller
×2
6 DOF Sensor - MPU6050
DFRobot 6 DOF Sensor - MPU6050
×1
Flysky i6X Radio
×1
MQ135 Gas and Air quality sensor
×1
Power Distribution Board For Quadcopter
×1
Jumper wires (generic)
Jumper wires (generic)
×1

Software apps and online services

Arduino IDE
Arduino IDE
Blues Notehub.io
Blues Notehub.io
Datacake
Datacake

Hand tools and fabrication machines

Soldering iron (generic)
Soldering iron (generic)
Solder Wire, Lead Free
Solder Wire, Lead Free

Story

Read more

Code

Code to run the drone and send data to notehub.io

C/C++
Use Arduino IDE to modify and upload the code to your Blues Swan
#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();
}

Credits

Abhishek Chavan

Abhishek Chavan

2 projects • 2 followers

Comments