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-10
- Revision:
- 7:bc5822aa8878
- Parent:
- 6:4c207e7b1203
- Child:
- 9:7b194f83e567
File content as of revision 7:bc5822aa8878:
#include "mbed.h" #include "rtos.h" #include "hardware.h" //Declarations void GetAttitude(); void FlightControllerTask(void const *n); void NotFlying(); //Variables float _yawTarget = 0; int _notFlying = 0; float _altitude = 0; double _basePower = 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() { //Use the 2 spare channels to alter the offset if(_levelOffset == true) { float pitchOffset = /*_channel7MedianFilter->process(*/Map(_rcCommands[6], RC_OUT_MIN, RC_OUT_MAX, -5, 5);//); float rollOffset = /*_channel8MedianFilter->process(*/Map(_rcCommands[7], RC_OUT_MIN, RC_OUT_MAX, -5, 5);//); _zeroValues[1] = _oldZeroValues[1] + pitchOffset; _zeroValues[2] = _oldZeroValues[2] + rollOffset; } else { _oldZeroValues[1] = _zeroValues[1]; _oldZeroValues[2] = _zeroValues[2]; } //_zeroValues[1] = _rcCommands[6]; //_zeroValues[2] = _rcCommands[7]; //Get raw data from IMU _freeIMU.getYawPitchRoll(_ypr); _freeIMU.getRate(_gyroRate); //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 angle because IMU is mounted at a right angle to the board float yaw = _ypr[0]; float pitch = _ypr[2]; float roll = - _ypr[1]; //Needs to be negative _ypr[0] = yaw; _ypr[1] = pitch; _ypr[2] = roll; //Swap pitch, roll and yaw? rate because IMU is mounted at a right angle to the board 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(); }