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-09-18
- Revision:
- 3:82665e39f1ea
- Parent:
- 1:045edcf091f3
- Child:
- 5:7b7db24ef6eb
File content as of revision 3:82665e39f1ea:
#include "mbed.h" #include "rtos.h" #include "hardware.h" //Declarations void SerialPortMonitorTask(void const *n); void CheckWirelessSerialCommand(); //void CheckGPSSerialCommand(); void WriteSettingsToConfig(); void ConvertToCharArray(float number); void ConvertToCharArray(int number); void UpdatePID(); //Variables char* _str = new char[1024]; char _wirelessSerialBuffer[255]; int _wirelessSerialRxPos = 0; char _gpsSerialBuffer[255]; int _gpsSerialRxPos = 0; //Timers RtosTimer *_serialPortMonitorUpdateTimer; // A thread to monitor the serial ports void SerialPortMonitorThread(void const *args) { //Update Timer _serialPortMonitorUpdateTimer = new RtosTimer(SerialPortMonitorTask, osTimerPeriodic, (void *)0); int updateTime = (1.0 / SERIAL_MONITOR_FREQUENCY) * 1000; _serialPortMonitorUpdateTimer->start(updateTime); // Wait here forever Thread::wait(osWaitForever); } void SerialPortMonitorTask(void const *n) { int batt = 0; _wirelessSerial.printf("<%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%1.6f:%d:%d:%d:%d:%1.6f:%1.6f:%1.6f:%1.2f:%1.2f:%1.2f:%1.2f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%1.8f:%d>", _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3], _ypr[0], _ypr[1], _ypr[2], batt, _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2], _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _armed, _initialised, _rate, _stab, _gyroRate[0], _gyroRate[1], _gyroRate[2], _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, 0/*_gpsValues[0]*/, 0/* _gpsValues[1]*/, 0/*_gpsValues[2]*/, 0/*_gpsValues[3]*/, 0/*_gpsValues[4]*/, 0/*_gpsConnected*/); //Check for wireless serial command if (_wirelessSerial.readable() > 0) { int c = _wirelessSerial.getc(); switch (c) { case 60: // _wirelessSerialRxPos = 0; break; case 10: // LF case 13: // CR case 62: // > CheckWirelessSerialCommand(); break; default: _wirelessSerialBuffer[_wirelessSerialRxPos++] = c; if (_wirelessSerialRxPos > 200) { _wirelessSerialRxPos = 0; } break; } } //Check for GPS serial command /*if (_gpsSerial.readable() > 0) { int c = _gpsSerial.getc(); //printf("%c", c); _wiredSerial.putc(c); switch (c) { case 60: // _gpsSerialRxPos = 0; break; case 10: // LF case 13: // CR case 62: // > CheckGPSSerialCommand(); break; default: _gpsSerialBuffer[_gpsSerialRxPos++] = c; if (_gpsSerialRxPos > 200) { _gpsSerialRxPos = 0; printf("oh no \r\n"); } break; } } */ } //Checks for a valid command from the serial port and executes it void CheckWirelessSerialCommand() { int length = _wirelessSerialRxPos; _wirelessSerialBuffer[_wirelessSerialRxPos] = 0; _wirelessSerialRxPos = 0; if (length < 1) { return; } char command = _wirelessSerialBuffer[0]; double value = 0; if(length > 1) { value = atof((char*)&_wirelessSerialBuffer[2]); } switch (command) { //Spare case 'a': break; //Arm disarm case 'b': if(_initialised == true && _armed == false) { Arm(); } else { Disarm(); } break; //Set mode case 'c': if(_rate == true) { _rate = false; _stab = true; } else { _rate = true; _stab = false; } break; //Set yaw case 'd': if(_armed == true) _rcMappedCommands[0] = value; //Yaw break; //Set pitch case 'e': if(_armed == true) _rcMappedCommands[1] = value; //Pitch break; //Set roll case 'f': if(_armed == true) _rcMappedCommands[2] = value; //Roll break; //Set thrust case 'g': if(_armed == true) _rcMappedCommands[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; //Zero pitch and roll case '1': ZeroPitchRoll(); WriteSettingsToConfig(); break; default: break; } return; } //Checks for a valid command from the serial port and executes it /*void CheckGPSSerialCommand() { int length = _gpsSerialRxPos; _gpsSerialBuffer[_gpsSerialRxPos] = 0; _gpsSerialRxPos = 0; if (length < 1) { return; } printf("command\r\n"); char command = _gpsSerialBuffer[0]; double value = 0; if(length > 1) { value = atof((char*)&_gpsSerialBuffer[2]); } switch (command) { //Latitude case 'a': _gpsValues[0] = value; _gpsConnected = true; break; //Longitude case 'b': _gpsValues[1] = value; _gpsConnected = true; break; //Altitude case 'c': _gpsValues[2] = value; _gpsConnected = true; break; //Course case 'd': _gpsValues[3] = value; _gpsConnected = true; break; //Speed case 'e': _gpsValues[4] = value; _gpsConnected = true; break; //Not Connected case 'f': _gpsConnected = false; break; default: break; } return; }*/ //Saves settings to config file void WriteSettingsToConfig() { _wiredSerial.printf("Writing settings to config file\n\r"); if(_armed == false) //Not flying { _freeIMU.sample(false); //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"); _freeIMU.sample(true); } else { _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\n\r"); } } //Converts float to char array void ConvertToCharArray(float number) { sprintf(_str, "%1.8f", number ); } //Converts integer to char array void ConvertToCharArray(int number) { sprintf(_str, "%d", number ); } //Updates PID tunings void UpdatePID() { _yawRatePIDController->setTunings(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD); _pitchRatePIDController->setTunings(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD); _rollRatePIDController->setTunings(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD); _yawStabPIDController->setTunings(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD); _pitchStabPIDController->setTunings(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD); _rollStabPIDController->setTunings(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD); }