Quadcopter software. Flying - Contact me for more details or a copy of the PC ground station code
Dependencies: ConfigFile PID PPM MODSERIAL mbed-rtos mbed MaxbotixDriver TinyGPS filter
flightController.h
- Committer:
- joe4465
- Date:
- 2015-02-22
- Revision:
- 9:7b194f83e567
- Parent:
- 7:bc5822aa8878
File content as of revision 9:7b194f83e567:
#include "mbed.h" #include "rtos.h" #include "hardware.h" //Declarations void GetAttitude(); void FlightControllerTask(void const *n); void NotFlying(); //Variables int _notFlying = 0; //Timers RtosTimer *_flightControllerUpdateTimer; // A thread to control flight void FlightControllerThread(void const *args) { printf("Flight controller thread started\r\n"); //Update Timer _flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0); int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; _flightControllerUpdateTimer->start(updateTime); // Wait here forever Thread::wait(osWaitForever); } void FlightControllerTask(void const *n) { //Get IMU data 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(); //Set stability PID outputs to 0 _stabPIDControllerOutputs[0] = 0; _stabPIDControllerOutputs[1] = 0; _stabPIDControllerOutputs[2] = 0; } //Stability mode else if(_rate == false && _stab == true) { //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 //_stabPIDControllerOutputs[0] = 0; // yaw //_stabPIDControllerOutputs[1] = 0; // pitch //_stabPIDControllerOutputs[2] = 0; // roll //Calculate motor power if flying //RC Mapped thottle is between 0 and 1 //Add 0.2 to try to avoid false starts if(_rcMappedCommands[3] > (RC_THRUST_MIN + 0.2) && _armed == true) { //Calculate base power to apply from throttle - returns 1060 at min, 1860 at max float basePower = MOTORS_MIN + (_rcMappedCommands[3] * 800); //Map motor power - each PID returns -100 <-> 100 _motorPower[0] = basePower + _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]; _motorPower[1] = basePower + _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]; _motorPower[2] = basePower - _ratePIDControllerOutputs[1] - _ratePIDControllerOutputs[2] + _ratePIDControllerOutputs[0]; _motorPower[3] = basePower - _ratePIDControllerOutputs[1] + _ratePIDControllerOutputs[2] - _ratePIDControllerOutputs[0]; //Check motor power is within limits - if not add/remove constant to all motors to keep motor ratio the same float motorFix = 0; float motorMin = _motorPower[0]; float motorMax = _motorPower[0]; for(int i=1; i<4; i++) { if(_motorPower[i] < motorMin) motorMin = _motorPower[i]; if(_motorPower[i] > motorMax) motorMax = _motorPower[i]; } //Check if min or max is outside of the limits if(motorMin < MOTORS_MIN) motorFix = MOTORS_MIN - motorMin; else if(motorMax > MOTORS_MAX) motorFix = MOTORS_MAX - motorMax; //Add/remove constant if neccessary for(int i=0; i<4; i++) { _motorPower[i] = _motorPower[i] + motorFix; } } //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 { NotFlying(); } } else { //Disable Motors _motorPower[0] = MOTORS_OFF; _motorPower[1] = MOTORS_OFF; _motorPower[2] = MOTORS_OFF; _motorPower[3] = MOTORS_OFF; _notFlying ++; if(_notFlying > 500) //Not flying for 1 second { NotFlying(); } } //Set motor power _motor1.pulsewidth_us(_motorPower[0]); _motor2.pulsewidth_us(_motorPower[1]); _motor3.pulsewidth_us(_motorPower[2]); _motor4.pulsewidth_us(_motorPower[3]); } void GetAttitude() { //Get raw data from IMU _freeIMU.getYawPitchRoll(_ypr); _freeIMU.getRate(_gyroRate); //Take off zero values to account for any angle inbetween level and ground //_ypr[1] = _ypr[1] - _zeroValues[1]; //_ypr[2] = _ypr[2] - _zeroValues[2]; //Swap pitch and roll angle because IMU is mounted at a right angle to the board float pitch = _ypr[2]; float roll = -_ypr[1]; _ypr[1] = pitch; _ypr[2] = roll; _ypr[0] = _ypr[0]; //Swap pitch, roll and yaw rate because IMU is mounted at a right angle to the board float yaw = _gyroRate[2]; pitch = _gyroRate[0]; roll = _gyroRate[1]; _gyroRate[0] = yaw; _gyroRate[1] = pitch; _gyroRate[2] = roll; } void NotFlying() { //Reset iteratior _notFlying = 0; //Zero gyro _freeIMU.zeroGyro(); //Reset I _yawRatePIDController->reset(); _pitchRatePIDController->reset(); _rollRatePIDController->reset(); _yawStabPIDController->reset(); _pitchStabPIDController->reset(); _rollStabPIDController->reset(); }