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
main.cpp
- Committer:
- joe4465
- Date:
- 2015-01-22
- Revision:
- 6:4c207e7b1203
- Parent:
- 3:82665e39f1ea
- Child:
- 7:bc5822aa8878
File content as of revision 6:4c207e7b1203:
//Includes #include "mbed.h" #include "rtos.h" #include "FreeIMU.h" #include "PID.h" #include "ConfigFile.h" #include "hardware.h" #include "statusLights.h" #include "serialPortMonitor.h" #include "flightController.h" #include "rcCommandMonitor.h" //Declarations void LoadSettingsFromConfig(); void InitialisePID(); void InitialisePWM(); void Setup(); //Loads settings from the config file - could tidy this a little if I can be assed void LoadSettingsFromConfig() { _wiredSerial.printf("Starting to load settings from config file\n\r"); //_wiredSerial.printf("Loading settings from config file\n\r"); char value[BUFSIZ]; //Read a configuration file from a mbed. if (!_configFile.read("/local/config.cfg")) { _wiredSerial.printf("Config file does not exist\n\r"); } else { //Get values if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value); else { _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r"); } if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value); else { _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r"); } if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value); else { _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r"); } if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value); else { _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r"); } if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value); else { _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r"); } if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value); else { _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r"); } if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value); else { _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r"); } if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value); else { _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r"); } if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value); else { _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r"); } if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value); else { _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r"); } if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value); else { _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r"); } if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value); else { _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r"); } if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value); else { _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r"); } if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value); else { _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r"); } if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value); else { _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r"); } if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value); else { _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r"); } if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value); else { _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r"); } if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value); else { _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r"); } if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value); else { _wiredSerial.printf("Failed to get value for zero pitch\n\r"); } if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value); else { _wiredSerial.printf("Failed to get value for zero roll\n\r"); } } _wiredSerial.printf("Finished loading settings from config file\n\r"); } //PID initialisation void InitialisePID() { float updateTime = 1.0 / FLIGHT_CONTROLLER_FREQUENCY; _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime); _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX); _yawRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX); _yawRatePIDController->setMode(AUTO_MODE); _yawRatePIDController->setSetPoint(0.0); _yawRatePIDController->setBias(0); _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime); _pitchRatePIDController->setInputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX); _pitchRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX); _pitchRatePIDController->setMode(AUTO_MODE); _pitchRatePIDController->setSetPoint(0.0); _pitchRatePIDController->setBias(0); _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime); _rollRatePIDController->setInputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX); _rollRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX); _rollRatePIDController->setMode(AUTO_MODE); _rollRatePIDController->setSetPoint(0.0); _rollRatePIDController->setBias(0); _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime); _yawStabPIDController->setInputLimits(IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX); _yawStabPIDController->setOutputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX); _yawStabPIDController->setMode(AUTO_MODE); _yawStabPIDController->setSetPoint(0.0); _yawStabPIDController->setBias(0); _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime); _pitchStabPIDController->setInputLimits(IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX); _pitchStabPIDController->setOutputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX); _pitchStabPIDController->setMode(AUTO_MODE); _pitchStabPIDController->setSetPoint(0.0); _pitchStabPIDController->setBias(0); _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime); _rollStabPIDController->setInputLimits(IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX); _rollStabPIDController->setOutputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX); _rollStabPIDController->setMode(AUTO_MODE); _rollStabPIDController->setSetPoint(0.0); _rollStabPIDController->setBias(0); } //PWM Initialisation void InitialisePWM() { //500Hz float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY; _motor1.period(period); _motor2.period(period); _motor3.period(period); _motor4.period(period); //Disable _motor1 = MOTORS_OFF; _motor2 = MOTORS_OFF; _motor2 = MOTORS_OFF; _motor2 = MOTORS_OFF; } //Setup void Setup() { //Setup wired serial coms _wiredSerial.baud(115200); printf("\r\n"); printf("*********************************************************************************\r\n"); printf("Starting Setup\r\n"); printf("*********************************************************************************\r\n"); //Setup wireless serial coms _wirelessSerial.baud(57600); printf("Setup wireless serial\r\n"); //Setup GPS serial comms //_gpsSerial.baud(115200); //printf("Setup GPS serial\r\n"); //Read config file LoadSettingsFromConfig(); //Set initial RC Ccommands _rcMappedCommands[0] = 0; _rcMappedCommands[1] = 0; _rcMappedCommands[2] = 0; _rcMappedCommands[3] = 0; printf("Setup initial RC commands\r\n"); //Setup RC median filters _yawMedianFilter = new medianFilter(10); _pitchMedianFilter = new medianFilter(10); _rollMedianFilter = new medianFilter(10); _thrustMedianFilter = new medianFilter(10); _channel7MedianFilter = new medianFilter(10); _channel8MedianFilter = new medianFilter(10); printf("Setup RC median filters\r\n"); //Initialise PPM _ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL); printf("Setup PPM\r\n"); //Initialise IMU wait(1); _freeIMU.init(true); printf("Setup IMU\r\n"); //Initialise PID InitialisePID(); printf("Setup PID\r\n"); //Initialise PWM InitialisePWM(); printf("Setup PWM\r\n"); //Set initialised flag _initialised = true; printf("*********************************************************************************\r\n"); printf("Finished Setup\r\n"); printf("*********************************************************************************\r\n"); // Start threads _flightControllerThread = new Thread (FlightControllerThread); _flightControllerThread->set_priority(osPriorityRealtime); _rcCommandMonitorThread = new Thread (RcCommandMonitorThread); _rcCommandMonitorThread->set_priority(osPriorityHigh); _serialPortMonitorThread = new Thread (SerialPortMonitorThread); _serialPortMonitorThread->set_priority(osPriorityLow); _statusThread = new Thread(StatusThread); _statusThread->set_priority(osPriorityIdle); } int main() { Setup(); // Wait here forever Thread::wait(osWaitForever); }