Quadcopter altitude hold test code version 1

Expectation: Auto takeoff and hover/hold at 30 cm height from ground.

Hardware: Arduino UNO R3, HC-SR04 Sonar module, KK2.0 Flight Control Board, a Quadcopter.

Description:  Apply PID function with altitude signal comes from HC-SR04 and adjust the output throttle signal to takeoff smoothly, and hold at target altitude.  Other flight control channel (Aileron, Elevator, Rudder) are directly connected to RC transmitter.

Issue: Vertical oscillation with about 10 cm up and down. Need to perform a careful PID tuning.

// Arduino Code

#include  
#include 
#include 

#define DEBUG_SERIAL_OUTPUT false

// Pin definition
#define ALT_HOLD_ENABLE 5
#define THROTTLE_IN 4
#define THROTTLE_OUT 11

#define SONAR_TRIGGER_PIN  12
#define SONAR_ECHO_PIN     13
// Pin definition end

#define SONAR_MAX_DISTANCE 500 // Max range in cm

NewPing sonar(SONAR_TRIGGER_PIN, SONAR_ECHO_PIN, SONAR_MAX_DISTANCE); // Setup sonar port and max range

double ch3_in, ch6_in;
double pid_out;
double ultrasonic_result;
double target_altitude = 0;

Servo ch3_pwm;

// PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
PID myPID(&ultrasonic_result, &pid_out, &target_altitude, 3, 5, 1, DIRECT);

void setup() 
{

  pinMode(THROTTLE_IN, INPUT);
  pinMode(ALT_HOLD_ENABLE, INPUT);

  myPID.SetOutputLimits(1200, 1800);
  myPID.SetMode(AUTOMATIC);

  Serial.begin(9600);

  ch3_pwm.attach(THROTTLE_OUT, 1000, 2000);
  ch3_pwm.writeMicroseconds(1000);

}

void loop() 
{
  ch6_in = pulseIn(ALT_HOLD_ENABLE, HIGH, 25000);

  if( ch6_in > 1500 )
  {
    // Altitude Hold Disable, throttle directly controlled by RC

    target_altitude = 0;
    // Update incomming ch3 to output
    ch3_in = pulseIn(THROTTLE_IN, HIGH, 25000);
    ch3_pwm.writeMicroseconds(ch3_in);

    SerialOutput("DIRECT\t");
    SerialOutput(ch3_in);
  }
  else
  {
    // Altitude Hold Enable, throttle control by PID

    target_altitude = 30; // cm to Us
    
    ultrasonic_result = (double) sonar.ping() / US_ROUNDTRIP_CM;

    SerialOutput("Sonar\t");
    SerialOutput(ultrasonic_result);
    SerialOutput(" CM\t");
    
    SerialOutput("Target\t");
    SerialOutput(target_altitude);
    SerialOutput(" CM\t");

    if( target_altitude == 0 )
    {
      // Initialize the Altitude hold function, use current altitude as hold target
      target_altitude = ultrasonic_result;
    }

    if( ultrasonic_result > 0 )
    {
      // A filter here might be needed to smooth the signal
      myPID.Compute();
      ch3_pwm.writeMicroseconds( pid_out );

      SerialOutput("PID\t");
      SerialOutput(pid_out);
    }
  }

  SerialOutput("\n");
}

// Serial output control
void SerialOutput(String str)
{
#if DEBUG_SERIAL_OUTPUT
  Serial.print(str);
#endif
}
void SerialOutput(double str)
{
#if DEBUG_SERIAL_OUTPUT
  Serial.print(str);
#endif
}

}

2 thoughts on “Quadcopter altitude hold test code version 1

  1. Ken says:

    Did you use a servo control board or just the PWM capability on the Adruino? The reason I asked is I tried to use one of those sonar sensors with the onboard PWM but found the libraries for each used the same timer and interfered with each other and I got random results.

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.