If(_notFlying > 500) //Not flying for 1 second _motorPower = map(_motorPower, 0.0, 1.0, MOTORS_MIN, 1500) //Reduced to 1500 to limit power for testing Map 0-1 value to actual pwm pulsewidth 1060 - 1860 _motorPower = Constrain((_rcMappedCommands - _ratePIDControllerOutputs - _ratePIDControllerOutputs - _ratePIDControllerOutputs), 0.0, 1.0) _motorPower = Constrain((_rcMappedCommands - _ratePIDControllerOutputs + _ratePIDControllerOutputs + _ratePIDControllerOutputs), 0.0, 1.0) _motorPower = Constrain((_rcMappedCommands + _ratePIDControllerOutputs + _ratePIDControllerOutputs - _ratePIDControllerOutputs), 0.0, 1.0) _motorPower = Constrain((_rcMappedCommands + _ratePIDControllerOutputs - _ratePIDControllerOutputs + _ratePIDControllerOutputs), 0.0, 1.0) Constrain motor power to 1, this means at max throttle there is no overhead for stabilising If(_rcMappedCommands > 0.1 & _armed = true) _rollRatePIDController->setSetPoint(_stabPIDControllerOutputs) _pitchRatePIDController->setSetPoint(_stabPIDControllerOutputs) _yawRatePIDController->setSetPoint(_stabPIDControllerOutputs) Update rate PID set point with desired rate from stab PID _yawRatePIDController->setProcessValue(_gyroRate) Update rate PID process value with gyro rate _stabPIDControllerOutputs = _rcMappedCommands //Feed to rate PID (overwriting stab PID output) _stabPIDControllerOutputs = _rollStabPIDController->compute() _stabPIDControllerOutputs = _pitchStabPIDController->compute() _stabPIDControllerOutputs = _yawStabPIDController->compute() _rollStabPIDController->setSetPoint(_rcMappedCommands) _pitchStabPIDController->setSetPoint(_rcMappedCommands) _yawStabPIDController->setSetPoint(_yawTarget) Update stab PID set point with desired angle from RC _rollStabPIDController->setProcessValue(_ypr) _pitchStabPIDController->setProcessValue(_ypr) _yawStabPIDController->setProcessValue(_ypr) _ratePIDControllerOutputs = _rollRatePIDController->compute() _ratePIDControllerOutputs = _pitchRatePIDController->compute() _ratePIDControllerOutputs = _yawRatePIDController->compute() _rollRatePIDController->setSetPoint(_rcMappedCommands) _pitchRatePIDController->setSetPoint(_rcMappedCommands) _yawRatePIDController->setSetPoint(_rcMappedCommands) Update rate PID set point with desired rate from RC _rollRatePIDController->setProcessValue(_gyroRate) _pitchRatePIDController->setProcessValue(_gyroRate) Constrains value to between min and maxįloat Constrain(const float in, const float min, const float max) Int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000 _updateTimer = new RtosTimer(Task500Hz, osTimerPeriodic, (void *)0) Void FlightControllerThread(void const *args) My flight control code is posted below, the other relevant classes are hardware.h and main.cpp #include "mbed.h"įloat Constrain(const float in, const float min, const float max) įloat map(float x, float in_min, float in_max, float out_min, float out_max) įloat _gyroRate = //Yaw, pitch, roll 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. I have come to the conclusion that I am either doing something completely wrong or I have some + - signs mixed around somewhere. 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 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. 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 built a quad copter completely from scratch (electronics, mechanics and software).
0 Comments
Leave a Reply. |
AuthorWrite something about yourself. No need to be fancy, just an overview. ArchivesCategories |