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
serialPortMonitor.h
- Committer:
- joe4465
- Date:
- 2014-05-09
- Revision:
- 0:0010a5abcc31
- Child:
- 1:045edcf091f3
File content as of revision 0:0010a5abcc31:
#include "mbed.h" #include "rtos.h" #include "hardware.h" //Declarations void CheckCommand(); void ZeroPitchRoll(); void WriteSettingsToConfig(); void ConvertToCharArray(float number); void ConvertToCharArray(int number); void UpdatePID(); void Arm(); //Variables char* _str = new char[1024]; char _serialBuffer[255]; int _serialRxPos = 0; // A thread to monitor the serial ports void SerialPortMonitorThread(void const *args) { while(true) { //Check for serial command if (_wirelessSerial.readable() > 0) { int c = _wirelessSerial.getc(); switch (c) { case 60: // _serialRxPos = 0; break; case 10: // LF case 13: // CR case 62: // > CheckCommand(); break; default: _serialBuffer[_serialRxPos++] = c; if (_serialRxPos > 200) { _serialRxPos = 0; } break; } } Thread::wait(100); } } //Checks for a valid command from the serial port and executes it void CheckCommand() { int length = _serialRxPos; _serialBuffer[_serialRxPos] = 0; _serialRxPos = 0; if (length < 1) { return; } char command = _serialBuffer[0]; double value = 0; if(length > 1) { value = atof((char*)&_serialBuffer[2]); } switch (command) { //Spare case 'a': break; //Arm disarm case 'b': if(_initialised == true && _armed == false) { Arm(); } else { _armed = false; } break; //Set mode case 'c': if(_rate == true) { _rate = false; _stab = true; } else { _rate = true; _stab = false; } break; //Set yaw case 'd': _rcCommands[0] = value; //Yaw break; //Set pitch case 'e': _rcCommands[1] = value; //Pitch break; //Set roll case 'f': _rcCommands[2] = value; //Roll break; //Set thrust case 'g': _rcCommands[3] = value; //Thrust break; //Set PID values case 'h': _yawRatePIDControllerP = value; UpdatePID(); WriteSettingsToConfig(); break; case 'i': _yawRatePIDControllerI = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'j': _yawRatePIDControllerD = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'k': _pitchRatePIDControllerP = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'l': _pitchRatePIDControllerI = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'm': _pitchRatePIDControllerD = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'n': _rollRatePIDControllerP = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'o': _rollRatePIDControllerI = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'p': _rollRatePIDControllerD = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'q': _yawStabPIDControllerP = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'r': _yawStabPIDControllerI = value; UpdatePID(); //WriteSettingsToConfig(); break; case 's': _yawStabPIDControllerD = value; UpdatePID(); //WriteSettingsToConfig(); break; case 't': _pitchStabPIDControllerP = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'u': _pitchStabPIDControllerI = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'v': _pitchStabPIDControllerD = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'w': _rollStabPIDControllerP = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'x': _rollStabPIDControllerI = value; UpdatePID(); //WriteSettingsToConfig(); break; case 'y': _rollStabPIDControllerD = value; UpdatePID(); //WriteSettingsToConfig(); break; //Get Settings case 'z': //_wiredSerial.printf("%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f\n", _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD); break; //Zero pitch and roll case '1': ZeroPitchRoll(); //WriteSettingsToConfig(); break; default: break; } return; } //Saves settings to config file void WriteSettingsToConfig() { _wiredSerial.printf("Writing settings to config file\n\r"); //Pause timer to avoid conflicts //_updateTimer->stop(); //_freeIMU.stop_sampling(); //Write values ConvertToCharArray(_yawRatePIDControllerP); if (!_configFile.setValue("_yawRatePIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _yawRatePIDControllerP\n\r"); } ConvertToCharArray(_yawRatePIDControllerI); if (!_configFile.setValue("_yawRatePIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _yawRatePIDControllerI\n\r"); } ConvertToCharArray(_yawRatePIDControllerD); if (!_configFile.setValue("_yawRatePIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _yawRatePIDControllerD\n\r"); } ConvertToCharArray(_pitchRatePIDControllerP); if (!_configFile.setValue("_pitchRatePIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerP\n\r"); } ConvertToCharArray(_pitchRatePIDControllerI); if (!_configFile.setValue("_pitchRatePIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerI\n\r"); } ConvertToCharArray(_pitchRatePIDControllerD); if (!_configFile.setValue("_pitchRatePIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _pitchRatePIDControllerD\n\r"); } ConvertToCharArray(_rollRatePIDControllerP); if (!_configFile.setValue("_rollRatePIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _rollRatePIDControllerP\n\r"); } ConvertToCharArray(_rollRatePIDControllerI); if (!_configFile.setValue("_rollRatePIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _rollRatePIDControllerI\n\r"); } ConvertToCharArray(_rollRatePIDControllerD); if (!_configFile.setValue("_rollRatePIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _rollRatePIDControllerD\n\r"); } ConvertToCharArray(_yawStabPIDControllerP); if (!_configFile.setValue("_yawStabPIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _yawStabPIDControllerP\n\r"); } ConvertToCharArray(_yawStabPIDControllerI); if (!_configFile.setValue("_yawStabPIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _yawStabPIDControllerI\n\r"); } ConvertToCharArray(_yawStabPIDControllerD); if (!_configFile.setValue("_yawStabPIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _yawStabPIDControllerD\n\r"); } ConvertToCharArray(_pitchStabPIDControllerP); if (!_configFile.setValue("_pitchStabPIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerP\n\r"); } ConvertToCharArray(_pitchStabPIDControllerI); if (!_configFile.setValue("_pitchStabPIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerI\n\r"); } ConvertToCharArray(_pitchStabPIDControllerD); if (!_configFile.setValue("_pitchStabPIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _pitchStabPIDControllerD\n\r"); } ConvertToCharArray(_rollStabPIDControllerP); if (!_configFile.setValue("_rollStabPIDControllerP", _str)) { _wiredSerial.printf("Failed to write value for _rollStabPIDControllerP\n\r"); } ConvertToCharArray(_rollStabPIDControllerI); if (!_configFile.setValue("_rollStabPIDControllerI", _str)) { _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\n\r"); } ConvertToCharArray(_rollStabPIDControllerD); if (!_configFile.setValue("_rollStabPIDControllerD", _str)) { _wiredSerial.printf("Failed to write value for _rollStabPIDControllerD\n\r"); } ConvertToCharArray(_zeroValues[1]); if (!_configFile.setValue("_zeroPitch", _str)) { _wiredSerial.printf("Failed to write value for zero pitch\n\r"); } ConvertToCharArray(_zeroValues[2]); if (!_configFile.setValue("_zeroRoll", _str)) { _wiredSerial.printf("Failed to write value for zero roll\n\r"); } if (!_configFile.write("/local/config.cfg")) { _wiredSerial.printf("Failure to write settings to configuration file.\n\r"); } else _wiredSerial.printf("Successfully wrote settings to configuration file.\n\r"); //Start timer //int updateTime = (1.0 / UPDATE_FREQUENCY) * 1000; //_updateTimer->start(updateTime); //_freeIMU.start_sampling(); } //Converts float to char array void ConvertToCharArray(float number) { sprintf(_str, "%1.6f", number ); } //Converts integer to char array void ConvertToCharArray(int number) { sprintf(_str, "%d", number ); } //Updates PID tunings void UpdatePID() { float updateTime = 1.0 / UPDATE_FREQUENCY; _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime); _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime); _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime); _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime); _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime); _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime); } //Zero gyro, zero yaw and arm void Arm() { //Zero gyro _freeIMU.zeroGyro(); //Zero Yaw int totalYaw = 0; float ypr[3] = {0,0,0}; // Yaw, pitch, roll for(int i = 0; i < 500; i++) { _freeIMU.getYawPitchRoll(ypr); totalYaw += ypr[0]; } _zeroValues[0] = totalYaw/500; //Set armed to true _armed = true; } //Zero pitch and roll void ZeroPitchRoll() { //Zero pitch and roll int totalPitch = 0; int totalRoll = 0; float ypr[3] = {0,0,0}; // Yaw, pitch, roll for(int i = 0; i < 500; i++) { _freeIMU.getYawPitchRoll(ypr); totalPitch += ypr[1]; totalRoll += ypr[2]; } _zeroValues[1] = totalPitch/500; _zeroValues[2] = totalRoll/500; //WriteSettingsToConfig(); }