Autopilot Implementation ■ System Set-up ■ Experiments

 

This page will document the development and implementation of a basic autopilot system utilizing the ArduCopter code base, an ArduPilot Mega 2.5 (APM), and a 3D Robotics Arducopter Quad-C frame. The autopilot developed in this section relies heavily on the work documented in the previous sections. Deriving the system of equations for the quadrotor enabled the development of a quadrotor simulator which simulated the non-linear dynamics of the quadrotor. Further, it was possible to identify the physical parameters of the model using a motion capture system. Using the variables identified in the system identification process, the simulation now closely matched the physical performance of the specific physical device. From these equations, a control system was developed to allow stable operation of the quadrotor given desired position information in the same way a pilot would send commands to the aircraft. This control system was verified and tuned in the simulated environment to ensure stability when implemented on the real system. Finally, in this section, the control system is implemented using the ArcuCopter software and APM hardware, verifying all the modeling, simulation, and control design was done appropriately. The end result is a complete hardware and software system that enables the user to develop, tune, and implement additional control systems in an accurate and repeatable process.

 

Setting Up Your Local IDE

This code relies heavily on the autopilot code and tutorial developed by Dr. Gareth Owenson. However, the code developed in this section utilizes the most recent ArduCopter libraries and therefore some of the syntax functions differ from Dr. Owenson’s version. This code uses ArduCopter 3.2.1 libraries which are the last to support the ArduPilot Mega board. All ArduCopter versions post 3.2.1 are designed for more advanced autopilot boards such as the Pixhawk or even companion computers such as the RasberryPi with the Navio+. The ArduCopter libraries give access to the Hardware Abstraction Layer (HAL). This interface allows us to work with the sensors, motors, and ESCs without having to worry about the low level code and drivers specific to each of these pieces of hardware. The ArduCopter developer wiki goes into further details about these libraries here. This allows us to focus on the higher level aspects of the flight controller and makes the code easier to read and comprehend. It also allows us to compare the implementation of the controller in the MATLAB simulation environment with the ArduPilot implementation.

In order to compile and load the code onto the APM board, you need to download download the ArduPilot version of the Arduino IDE. Also, as mentioned previously, you need to download the ArduCopter libraries. The code below is compatible with the ArduCopter 3.2.1 libraries which can be downloaded here. The libraries should be placed in your sketches folder so they can be found by the Arduino software. Also make sure you select your board type from the Arduino menu. More detailed instructions are available on the ArduPilot developer wiki here.

 

Setting Up Your Quadrotor

This section assumes the user already has assembled their quadrotor, set-up the full ArduCopter software, and successfully flown their quadrotor before. During the set-up process, the ESCs and RC would have been properly calibrated and the APM board properly installed. If not, detailed instructions about installing the APM, attaching the motors and sensors, calibrating the system, and first flight procedures are described here for set-up and here for first time flight. Specific to the set-up of the quadrotor, this autopilot implementation utilizes the “+” orientation for flight while Dr. Owenson utilizes the “X” orientation. This only effects the final motor command calculations derived from the control system computations. The difference between the two orientations is depicted in the image below. Note that this matches the model developed in the System Modeling section.

 

 

Developing the autopilot

This section will walk through the autopilot code. At a high level, the autopilot will read in the commands from the pilot via the RC, measure its current position and orientation, calculate motor commands to respond to the RC inputs, and finally send the motor commands to the motor. Each of these sections will be described independently with the complete code available for review at the end.

Reading the RC Inputs

For this simple autopilot, only a 4 channel RC is needed. These channels correspond to roll, pitch, yaw, and altitude commands that are sent to the autopilot. RCs send commands based on Pulse Width Modulation (PWM). Basically, the length of time the signal is sent corresponds to the value being sent. A value of 0 is sent as 1000 microseconds and a value of 100 is sent as 2000 microseconds. Since there is always error in real systems, the first step to implementing the autopilot is to determine the minimum and maximum values being sent by each of the four channels of the RC.

The code below reads in the RC inputs and prints them to the serial console. This can be viewed using the Arduino serial monitor. Once the code is compiled, uploaded, and run, the serial console will display the PWM values received. These should be in the range of 1000 to 2000. Note the minimum and maximum of each channel as these will be needed next.

/* Includes */
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Math.h>
#include <AP_ADC.h>
#include <StorageManager.h>
#include <AP_InertialSensor.h>
#include <AP_Notify.h>
#include <AP_GPS.h>
#include <AP_Baro.h>
#include <Filter.h>
#include <GCS_MAVLink.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_Vehicle.h>
#include <AP_Compass.h>
#include <AP_NavEKF.h>
#include <PID.h>

/* Defines */

/* Variables */
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;  // Hardware abstraction layer

float map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void setup()
{

}

void loop()
{
  // Local variables
  uint16_t channels[8];  // array for raw channel values
  float rcthr, rcyaw, rcpit, rcroll;   // Variables to store rc input

  // Read RC channels and store in channels array
  hal.rcin->read(channels, 8);

  // Copy from channels array to something human readable - array entry 0 = input 1, etc.
  rcthr = channels[2];
  rcyaw = channels[3];
  rcpit = channels[1];
  rcroll = channels[0];

  //Print to serial monitor
  hal.console->printf_P( PSTR("THR %.1f YAW %.1f PIT %.1f ROLL %.1f \n"), rcthr, rcyaw, rcpit, rcroll);

  hal.scheduler->delay(50);  //Wait 50ms

}

AP_HAL_MAIN();    // special macro that replace's one of Arduino's to setup

Once the channel minimum and maximum values are recorded, we will implement a map function to convert the PWM values into desired roll, pitch, yaw, and thrust values. The map function is shown above. This function takes a PWM input value and the range of PWM values for a specific channel and scales the PWM input value to a new range. For the roll, pitch, and yaw channels, the inputs are scaled to a minimum and maximum angle. The thrust is not scaled since it has already been calibrated while flying the quadrotor using the full ArduCopter autopilot code.

The implementation of the map function is shown in the code snippet below. The roll and pitch angles saturate at +/- 45 degrees and the yaw angle saturates at +/- 150 degrees in this example. It is also important to note that the roll and yaw angles should be negative when the RC stick is to the left while the pitch value should be negative when the stick is forward. This corresponds to our model defined in the System Modeling section. I had to change the sign of some of the outputs in order to get this behavior.

rcthr = channels[2];
rcyaw = -map(channels[3], 1124, 1920, -150, 150);
rcpit = -map(channels[1], 1124, 1918, -45, 45);
rcroll = -map(channels[0], 1124, 1918, -45, 45);

After implementing the map function, the roll, pitch, and yaw values should read around zero when the RC sticks are centered. If not, the RC minimum and maximum RC values can be adjusted slightly. Also, the values read when the RC sticks are at their minimum and maximum should match the minimum and maximum angles used in the map function.

 

Reading the IMU and AHRS

The APM has two sensors onboard for determining the quadrotor’s attitude. As described in the Model Verification section, gyroscopes are used to measure angular velocity around each axis and the accelerometers are used to measure translational acceleration. Accelerometers are sensitive to vibrations and have a slower update frequency while gyroscopes have a faster update frequency, are resistant to vibrations, but suffer from drift. However, a sensor fusion algorithm can combine both measurements, minimizing the disadvantages of both sensors, to get a more accurate attitude estimation.

In this autopilot implementation, we will be using the GPS and barometer but not to control the quadrotor’s translational position directly. Instead, we will use the Inertial Measurement Unit (IMU) for direct sensor measurements and the Altitude and Heading Reference System (AHRS) for sensor fusion measurements. An AHRS includes a processing system which provides solved attitude and heading solutions. An IMU just delivers sensor data to an additional device that solves the attitude solution. From the ArduCopter AHRS library, we will use the Direct Cosine Matrix (DCM) to compute the attitude solution. A DCM is an algorithm that is a less processing intensive equivalent of the Kalman Filter, an optimal but computationally complex sensor fusion algorithm. See this for more details concerning the DCM implementation in ArduCopter.

The code below initializes the IMU, barometer, and GPS, for use in the AHRS. This is done in the setup() function. In the loop() function, once a measurement is ready for sampling, the gyroscope and AHRS measurements are read and printed to the serial monitor at 10Hz. Moving the quadrotor around will change these values as they are printed out.

/* Includes */
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Math.h>
#include <AP_ADC.h>
#include <StorageManager.h>
#include <AP_InertialSensor.h>
#include <AP_Notify.h>
#include <AP_GPS.h>
#include <AP_Baro.h>
#include <Filter.h>
#include <GCS_MAVLink.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_Vehicle.h>
#include <AP_Compass.h>
#include <AP_NavEKF.h>
#include <PID.h>

/* Defines */

/* Variables */
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;  // Hardware abstraction layer
AP_InertialSensor ins;  // Generic inertial sensor
AP_InertialSensor_MPU6000 imu(ins);  // MPU6000 sensor
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi); // Barometer
AP_GPS gps;  // GPS
AP_AHRS_DCM ahrs(ins, baro, gps); // AHRS DCM implementation

/* Functions */

void setup()
{

  // we need to stop the barometer from holding the SPI bus
  hal.gpio->pinMode(40, HAL_GPIO_OUTPUT);
  hal.gpio->write(40, 1);

  // Initialize MPU6050 sensor
  ins.init(AP_InertialSensor::COLD_START, AP_InertialSensor::RATE_100HZ);

  // Initialize the accelerometer
  ins.init_accel();

  // Initialize the AHRS DCM implementation
  ahrs.init();

}

void loop()
{
  Vector3f gyro;
  float gyroroll, gyropitch, gyroyaw;  // Velocity in deg/s
  float roll, pitch, yaw;  // Angle in degrees
  static uint16_t counter;
  static uint32_t last_print, last_compass;

  uint32_t now = hal.scheduler->micros();
  if (last_print == 0) {
    last_print = now;
    return;
  }
  // Wait for a new measurement
  ins.wait_for_sample();

  // Read the gyroscope (deg/s)
  ins.update();
  gyro = ins.get_gyro();
  gyropitch = ToDeg(gyro.y);
  gyroroll = ToDeg(gyro.x);
  gyroyaw = ToDeg(gyro.z);

  // Read the AHRS to get attitude (deg)
  ahrs.update();
  roll = ToDeg(ahrs.roll);
  pitch = ToDeg(ahrs.pitch);
  yaw = ToDeg(ahrs.yaw);

  // Print to serial monitor at 10Hz
  counter++;
  if (now - last_print >= 100000 /* 100ms : 10hz */) {
    hal.console->printf_P(PSTR("P:%4.1f  R:%4.1f Y:%4.1f rate=%.1f"),pitch,roll,yaw,(1.0e6*counter)/(now-last_print));
    last_print = now;
    counter = 0;
  }

}

AP_HAL_MAIN();    // special macro that replace's one of Arduino's to setup

 

Sending Motor Commands

Commands are sent to the motors via the Electronic Speed Controllers (ESCs) as PWM signals. These also range from 1000 to 2000 microseconds like the RC input signals. The motor commands are sent at 490 Hz. The code below initializes the output channels and sends a specified motor command to each motor. It is recommended to initially test the code with only one motor at a time and with the propeller off. This can be done by either removing the power from 3 of the ESCs or by commenting out 3 of the motor send commands. In the below, the motor command is taken from the thrust value input from the RC. Note that it may be necessary to update the motor position defines to correspond to the correct motors.

/* Includes */
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Math.h>
#include <AP_ADC.h>
#include <StorageManager.h>
#include <AP_InertialSensor.h>
#include <AP_Notify.h>
#include <AP_GPS.h>
#include <AP_Baro.h>
#include <Filter.h>
#include <GCS_MAVLink.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_Vehicle.h>
#include <AP_Compass.h>
#include <AP_NavEKF.h>
#include <PID.h>

/* Defines */

// Motor Definitions
#define MOTOR_F   2    // Front
#define MOTOR_R   0    // Right
#define MOTOR_B   3    // Back
#define MOTOR_L   1    // Left

/* Variables */
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;  // Hardware abstraction layer

void setup()
{

  //ESC Frequency to 490 Hz
  hal.rcout->set_freq(0xF, 490);

  // Enable output channels
  for (uint8_t i=0; i<8; i++) {
    hal.rcout->enable_ch(i);
  }

}

void loop()
{
  uint16_t channels[8];  // array for raw channel values
  float rcthr, rcyaw, rcpit, rcroll;   // Variables to store rc input

  // Read RC channels and store in channels array
  hal.rcin->read(channels, 8);

  // Copy from channels array to something human readable - array entry 0 = input 1, etc.
  rcthr = channels[2];
  rcyaw = -map(channels[3], 1124, 1920, -150, 150);
  rcpit = -map(channels[1], 1124, 1918, -45, 45);
  rcroll = -map(channels[0], 1124, 1918, -45, 45);

  // mix pid outputs and send to the motors.
  hal.rcout->write(MOTOR_F, rcthr);
  // hal.rcout->write(MOTOR_R, rcthr);
  // hal.rcout->write(MOTOR_B, rcthr);
  // hal.rcout->write(MOTOR_L, rcthr);

}

AP_HAL_MAIN();    // special macro that replace's one of Arduino's to setup

 

Implementing The Control System

The control system takes the RC inputs, the current attitude measurements from the IMU and AHRS, and computes control system outputs. This section of code implements the nested control system described in the Controller Design section. Since we are not sending the quadrotor desired position, we won’t be using the translational position controller. Instead, the RC inputs will be used to compute desired roll and pitch angles, yaw rate, and thrust. The are inputs into the attitude controller to compute desired roll and pitch rates. The rate controller computes the moment control inputs which will be mixed with the thrust input to compute the final motor commands.

/* Includes */
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Progmem.h>
#include <AP_HAL.h>
#include <AP_HAL_AVR.h>
#include <AP_Math.h>
#include <AP_ADC.h>
#include <StorageManager.h>
#include <AP_InertialSensor.h>
#include <AP_Notify.h>
#include <AP_GPS.h>
#include <AP_Baro.h>
#include <Filter.h>
#include <GCS_MAVLink.h>
#include <AP_AHRS.h>
#include <AP_Airspeed.h>
#include <AP_Vehicle.h>
#include <AP_Compass.h>
#include <AP_NavEKF.h>
#include <PID.h>

/* Defines */

// Motor Definitions
#define MOTOR_F   2    // Front
#define MOTOR_R   0    // Right
#define MOTOR_B   3    // Back
#define MOTOR_L   1    // Left

// PID array
PID pids[6];
#define PID_PITCH_STAB 0
#define PID_ROLL_STAB 1
#define PID_YAW_STAB 2
#define PID_PITCH_RATE 3
#define PID_ROLL_RATE 4
#define PID_YAW_RATE 5

#define wrap_180(x) (x < -180 ? x+360 : (x > 180 ? x - 360: x))

/* Variables */
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;  // Hardware abstraction layer

AP_InertialSensor ins;
AP_InertialSensor_MPU6000 imu(ins);
AP_Baro_MS5611 baro(&AP_Baro_MS5611::spi);
AP_GPS gps;
AP_Compass_HMC5843 compass;
AP_AHRS_DCM ahrs(ins, baro, gps);

/* Functions */
float map(long x, long in_min, long in_max, long out_min, long out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void setup()
{

  //ESC Frequency to 490 Hz
  hal.rcout->set_freq(0xF, 490);

  // Enable output channels
  for (uint8_t i=0; i<8; i++) {
    hal.rcout->enable_ch(i);
  }

  // we need to stop the barometer from holding the SPI bus
  hal.gpio->pinMode(40, HAL_GPIO_OUTPUT);
  hal.gpio->write(40, 1);

  // Initialize MPU6050 sensor
  ins.init(AP_InertialSensor::COLD_START, AP_InertialSensor::RATE_100HZ);

  ins.init_accel();

  ahrs.init();

  gps.init(NULL);

  // PID values
  pids[PID_PITCH_STAB].kP(4.5);
  pids[PID_PITCH_STAB].kI(0);
  pids[PID_PITCH_STAB].kD(0);
  pids[PID_PITCH_STAB].imax(8);

  pids[PID_ROLL_STAB].kP(4.5);
  pids[PID_ROLL_STAB].kI(0);
  pids[PID_ROLL_STAB].kD(0);
  pids[PID_ROLL_STAB].imax(8);

  pids[PID_YAW_STAB].kP(10);
  pids[PID_YAW_STAB].kI(0);
  pids[PID_YAW_STAB].kD(0);
  pids[PID_YAW_STAB].imax(8);

  pids[PID_PITCH_RATE].kP(0.7);
  pids[PID_PITCH_RATE].kI(1);
  pids[PID_PITCH_RATE].kD(0);
  pids[PID_PITCH_RATE].imax(50);

  pids[PID_ROLL_RATE].kP(0.7);
  pids[PID_ROLL_RATE].kI(1);
  pids[PID_ROLL_RATE].kD(0);
  pids[PID_ROLL_RATE].imax(50);

  pids[PID_YAW_RATE].kP(2.7);
  pids[PID_YAW_RATE].kI(1);
  pids[PID_YAW_RATE].kD(0);
  pids[PID_YAW_RATE].imax(50);

}

void loop()
{
  uint16_t channels[8];  // array for raw channel values
  float rcthr, rcyaw, rcpit, rcroll;   // Variables to store rc input
  Vector3f gyro;
  float gyroroll, gyropitch, gyroyaw;
  float roll, pitch, yaw;
  static uint16_t counter;
  static uint32_t last_print, last_compass;
  static float yaw_target = 0;
  float pitch_stab_output;
  float roll_stab_output;
  float yaw_stab_output;
  float pitch_output;
  float roll_output;
  float yaw_output;

  uint32_t now = hal.scheduler->micros();
  if (last_print == 0) {
    last_print = now;
    return;
  }

  // Read RC channels and store in channels array
  hal.rcin->read(channels, 8);

  // Copy from channels array to something human readable - array entry 0 = input 1, etc.
  rcthr = channels[2];
  rcyaw = -map(channels[3], 1124, 1920, -150, 150);
  rcpit = -map(channels[1], 1124, 1918, -45, 45);
  rcroll = -map(channels[0], 1124, 1918, -45, 45);

  ins.wait_for_sample();

  ins.update();
  gyro = ins.get_gyro();
  gyropitch = ToDeg(gyro.y);
  gyroroll = ToDeg(gyro.x);
  gyroyaw = ToDeg(gyro.z);

  ahrs.update();
  roll = ToDeg(ahrs.roll);
  pitch = ToDeg(ahrs.pitch);
  yaw = ToDeg(ahrs.yaw);

  counter++;

  // Attitude PIDS
  pitch_stab_output = constrain_float(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
  roll_stab_output = constrain_float(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
  yaw_stab_output = constrain_float(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);

  // Yaw input is rate control, check if pilot is changing yaw rate
  if(abs(rcyaw ) > 5) {
    yaw_stab_output = rcyaw;
    yaw_target = yaw;   // remember this yaw for when pilot stops
  }

  // Rate PIDS
  pitch_output =  constrain_float(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyropitch, 1), - 500, 500);
  roll_output =  constrain_float(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroroll, 1), -500, 500);
  yaw_output =  -constrain_float(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroyaw, 1), -500, 500);

  if (now - last_print >= 100000 /* 100ms : 10hz */) {
    hal.console->printf_P(PSTR("F %.2f R %.2f B %.2f L %.2f"),pitch_stab_output, -roll_stab_output, -pitch_stab_output, roll_stab_output);
    last_print = now;
    counter = 0;
  }

}

AP_HAL_MAIN();    // special macro that replace's one of Arduino's to setup

This section starts by defining an array to store the PID values for each of the PID controllers. Next, a wrap macro is defined to keep the yaw value between +/- 180 degrees. In the setup() function, the PID values are defined. These can be updated to tune the performance of the autopilot. The attitude controllers are initially only proportional controllers. The rate controllers are set to PI controllers but derivative terms can be added to reduce oscillations if necessary. In the loop() function, the PIDs outputs are constrained with the constrain_float() function to prevent sending any abnormal values to the motors which may result in unstable behavior. Also, when the yaw stick is moved, the RC input yaw value is used as the desired yaw rate. This prevent the quadrotor from returning to its home orientation when the yaw stick is centered. With this implementation, the quadrotor will maintain its current heading when the yaw stick is returned to center. Lastly, the first print statement only prints the roll and pitch portions of the control output assuming the yaw input is left untouched. This print statement can be used to test the control system. For example, moving the roll stick to the left should result in a positive value for the right motor and a negative value for the left motor. Moving the pitch stick forward should result in a positive value for the rear motor and a negative value for the front motor. Uncommenting out the yaw print statement will help debug the yaw controller. Moving the yaw stick left should result in positive values for the front and back motors and negative values for the left and right motors.

Putting it all together

This section details the remaining code required to finalize the autopilot. Primarily, these modifications determine when to send commands to the motors and how to combine the control inputs specific to each motor. Only a portion of the code is reproduced below since the majority of the code remains the same as the code in the “Implementing the Control System” section above. A complete, final version of the autopilot code is available for viewing and downloading here.

  if(rcthr > 1120 + 100){

    // Attitude PIDS
    pitch_stab_output = constrain_float(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
    roll_stab_output = constrain_float(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
    yaw_stab_output = constrain_float(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);

    // Yaw input is rate control, check if pilot is changing yaw rate
    if(abs(rcyaw ) > 5) {
      yaw_stab_output = rcyaw;
      yaw_target = yaw;   // remember this yaw for when pilot stops
    }

    // Rate PIDS
    pitch_output =  constrain_float(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyropitch, 1), - 500, 500);
    roll_output =  constrain_float(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroroll, 1), -500, 500);
    yaw_output =  -constrain_float(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroyaw, 1), -500, 500);

    // mix pid outputs and send to the motors.
    hal.rcout->write(MOTOR_F, rcthr + pitch_output + yaw_output);
    hal.rcout->write(MOTOR_R, rcthr - roll_output  - yaw_output);
    hal.rcout->write(MOTOR_B, rcthr - pitch_output + yaw_output);
    hal.rcout->write(MOTOR_L, rcthr + roll_output  - yaw_output);
  }
  else {
    // motors off
    hal.rcout->write(MOTOR_F, 1000);
    hal.rcout->write(MOTOR_R, 1000);
    hal.rcout->write(MOTOR_B, 1000);
    hal.rcout->write(MOTOR_L, 1000);

    yaw_target = yaw;

    for(int i=0; i<6; i++) // reset PID integrals whilst on the ground
      pids[i].reset_I();

  }

 

First, the control commands are only activated if the pilot raises the throttle slightly above the minimum value. If the throttle remains at a minimum, the motors are kept in the off position, and the quadrotor remains on the ground. This safety precaution is similar to arming the quadrotor found on other quadrotor autopilots. Additionally, the desired yaw angle is updated to the current yaw reading in case the quadrotor is rotated while it is on the ground. Lastly, the PID integrator values are reset as an anti-windup precaution. In the code, this is implemented with a simple if statement comparing the current throttle value to a given minimum value to determine if the control code should be run or not.

Second, the control inputs are combined to calculate individual motor commands. As mentioned previously, this mixing is based off of an “+” flight configuration. These equations would need to be modified if a “X” orientation is used. The mixing equations are similar to the motor control equations detailed in the Control Systems Design section and reproduced below for reference and comparison.

 

A video describing the entire autopilot development process including video of the quadrotor flying with the final autopilot software is available below.

[embedyt] http://www.youtube.com/watch?v=X1rk2wLTY-Q%5B/embedyt%5D

 

[simple-social-share]