1
$\begingroup$

I have built a quad copter completely from scratch (electronics, mechanics and software). I am now at the point where all my sensor data looks correct and when I tilt the quad copter the correct motors increase and decrease.

I have been trying to tune the PIDs for a couple of days now, in rate mode it stays level and rotates at roughly the correct degrees per second when I give it a command.

In stability mode a lot of the time it just spins around the axis and when I did get it stable it kept rotating from upright to upside down and then maintaining an upside down flat position. I have come to the conclusion that I am either doing something completely wrong or I have some + - signs mixed around somewhere.

Would anyone who is knowledgeable about quad copter control code be able to take a look at what I have done and how it works as I'm really struggling to work out what needs to change and what I should try next.

My flight control code is posted below, the other relevant classes are hardware.h and main.cpp

#include "mbed.h"
#include "rtos.h"
#include "hardware.h"


//Declarations
float Constrain(const float in, const float min, const float max);
float map(float x, float in_min, float in_max, float out_min, float out_max);
void GetAttitude();
void Task500Hz(void const *n);
void Task10Hz();


//Variables
float _gyroRate[3] ={}; // Yaw, Pitch, Roll
float _ypr[3] = {0,0,0}; // Yaw, pitch, roll
float _yawTarget = 0;
int _notFlying = 0; 
float _altitude = 0;
int _10HzIterator = 0;
float _ratePIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
float _stabPIDControllerOutputs[3] = {0,0,0}; //Yaw, pitch, roll
float _motorPower [4] = {0,0,0,0};




//Timers
RtosTimer *_updateTimer;


// A thread to monitor the serial ports
void FlightControllerThread(void const *args) 
{  
    //Update Timer
    _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0);
    int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000;
    _updateTimer->start(updateTime);

    // Wait here forever
    Thread::wait(osWaitForever);
}


//Constrains value to between min and max
float Constrain(const float in, const float min, const float max)
{
    float out = in;
    out = out > max ? max : out;
    out = out < min ? min : out;
    return out;
}


//Maps input to output
float map(float x, float in_min, float in_max, float out_min, float out_max)
{
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}


//Zeros values
void GetAttitude()
{
    //Take off zero values to account for any angle inbetween the PCB level and ground
    _ypr[1] = _ypr[1] - _zeroValues[1];
    _ypr[2] = _ypr[2] - _zeroValues[2];

    //Swap pitch and roll because IMU is mounted at a right angle to the board
            //Gyro data does need swapping - done within freeIMU class
    float pitch = _ypr[2];
    float roll = _ypr[1];
    _ypr[1] = pitch;
    _ypr[2] = roll;
}


void Task500Hz(void const *n)
{
    _10HzIterator++;
    if(_10HzIterator % 50 == 0)
    {
        Task10Hz();
    }


    //Get IMU data
    _freeIMU.getYawPitchRoll(_ypr);
    _freeIMU.getRate(_gyroRate);
    GetAttitude();

    //Rate mode
    if(_rate == true && _stab == false)
    {
        //Update rate PID process value with gyro rate
        _yawRatePIDController->setProcessValue(_gyroRate[0]);
        _pitchRatePIDController->setProcessValue(_gyroRate[1]);
        _rollRatePIDController->setProcessValue(_gyroRate[2]);

        //Update rate PID set point with desired rate from RC
        _yawRatePIDController->setSetPoint(_rcMappedCommands[0]);
        _pitchRatePIDController->setSetPoint(_rcMappedCommands[1]);
        _rollRatePIDController->setSetPoint(_rcMappedCommands[2]);

        //Compute rate PID outputs
        _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
        _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
        _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
    }
    //Stability mode
    else
    {
        //Update stab PID process value with ypr
        _yawStabPIDController->setProcessValue(_ypr[0]);
        _pitchStabPIDController->setProcessValue(_ypr[1]);
        _rollStabPIDController->setProcessValue(_ypr[2]);

        //Update stab PID set point with desired angle from RC
        _yawStabPIDController->setSetPoint(_yawTarget);
        _pitchStabPIDController->setSetPoint(_rcMappedCommands[1]);
        _rollStabPIDController->setSetPoint(_rcMappedCommands[2]);

        //Compute stab PID outputs
        _stabPIDControllerOutputs[0] = _yawStabPIDController->compute();
        _stabPIDControllerOutputs[1] = _pitchStabPIDController->compute();
        _stabPIDControllerOutputs[2] = _rollStabPIDController->compute();

        //If pilot commanding yaw
        if(abs(_rcMappedCommands[0]) > 0)
        {  
            _stabPIDControllerOutputs[0] = _rcMappedCommands[0];  //Feed to rate PID (overwriting stab PID output)
            _yawTarget = _ypr[0];
        }

        //Update rate PID process value with gyro rate
        _yawRatePIDController->setProcessValue(_gyroRate[0]);
        _pitchRatePIDController->setProcessValue(_gyroRate[1]);
        _rollRatePIDController->setProcessValue(_gyroRate[2]);

        //Update rate PID set point with desired rate from stab PID
        _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs[0]);
        _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs[1]);
        _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs[2]);

        //Compute rate PID outputs
        _ratePIDControllerOutputs[0] = _yawRatePIDController->compute();
        _ratePIDControllerOutputs[1] = _pitchRatePIDController->compute();
        _ratePIDControllerOutputs[2] = _rollRatePIDController->compute();
    }

    //Testing
    _ratePIDControllerOutputs[0] = 0; // yaw
    //_ratePIDControllerOutputs[1] = 0; // pitch
    _ratePIDControllerOutputs[2] = 0; // roll


    //Calculate motor power if flying
    if(_rcMappedCommands[3] > 0.1 && _armed == true)
    {
        //Constrain motor power to 1, this means at max throttle there is no overhead for stabilising
        _motorPower[0] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
        _motorPower[1] = Constrain((_rcMappedCommands[3] + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);
        _motorPower[2] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]), 0.0, 1.0);
        _motorPower[3] = Constrain((_rcMappedCommands[3] - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]), 0.0, 1.0);


        //Map 0-1 value to actual pwm pulsewidth 1060 - 1860
        _motorPower[0] = map(_motorPower[0], 0.0, 1.0, MOTORS_MIN, 1500); //Reduced to 1500 to limit power for testing
        _motorPower[1] = map(_motorPower[1], 0.0, 1.0, MOTORS_MIN, 1500);
        _motorPower[2] = map(_motorPower[2], 0.0, 1.0, MOTORS_MIN, 1500);
        _motorPower[3] = map(_motorPower[3], 0.0, 1.0, MOTORS_MIN, 1500);
    }


    //Not flying
    else if(_armed == true)
    {
        _yawTarget = _ypr[0];

        //Set motors to armed state
        _motorPower[0] = MOTORS_ARMED;
        _motorPower[1] = MOTORS_ARMED;
        _motorPower[2] = MOTORS_ARMED;
        _motorPower[3] = MOTORS_ARMED;

        _notFlying ++;
        if(_notFlying > 500) //Not flying for 1 second
        {
            //Reset iteratior
            _notFlying = 0;

            //Reset I
            _yawRatePIDController->reset();
            _pitchRatePIDController->reset();
            _rollRatePIDController->reset();
            _yawStabPIDController->reset();
            _pitchStabPIDController->reset();
            _rollStabPIDController->reset();
        }
    } 
    else
    {
        //Disable Motors
        _motorPower[0] = MOTORS_OFF;
        _motorPower[1] = MOTORS_OFF;
        _motorPower[2] = MOTORS_OFF;
        _motorPower[3] = MOTORS_OFF;
    }

    //Set motor power
    _motor1.pulsewidth_us(_motorPower[0]);
    _motor2.pulsewidth_us(_motorPower[1]);
    _motor3.pulsewidth_us(_motorPower[2]);
    _motor4.pulsewidth_us(_motorPower[3]);
}


//Print data
void Task10Hz()
{
    int batt = 0;
    _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f>",
    _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD);    
}

The whole program can be seen on my mBed page at http://mbed.org/users/joe4465/code/QuadMK6/

If you need any more info or something explaining let me know.

If anyone can point me in the right direction or has any idea of what I should try next it would be very much appreciated.

Thanks Joe

Improve this question
$\endgroup$
1
  • $\begingroup$ It's nice weekend of you to include your code, but it's too long for anyone to be expected to read it... And the question is too general, so don't know where to start... Be more specific and you might have better chances of getting an answer $\endgroup$
    – dm76
    Commented May 18, 2014 at 21:17

1 Answer 1

Reset to default
0
$\begingroup$

If you've built this from scratch, the best person to troubleshoot your code is yourself.

Look at the basic pieces of the code: the sensor, the PIDs, and the motor control. Write some functions to help you test each of those pieces individually, so that you can determine whether the error you're seeing is in just one component, or a system-level error based on some incorrect code in all 3 components.

Once you know the extent of the error, you'll be able to either recognize the problem and fix it, or to ask a more focused question here.

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.