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:
- 2015-01-22
- Revision:
- 6:4c207e7b1203
- Parent:
- 5:7b7db24ef6eb
- Child:
- 7:bc5822aa8878
File content as of revision 6:4c207e7b1203:
#include "mbed.h" #include "rtos.h" #include "hardware.h" //Declarations void CheckWirelessSerialCommand(); //void CheckGPSSerialCommand(); void UpdatePID(); //Variables char _wirelessSerialBuffer[255]; int _wirelessSerialRxPos = 0; //char _gpsSerialBuffer[255]; //int _gpsSerialRxPos = 0; // A thread to monitor the serial ports void SerialPortMonitorThread(void const *args) { printf("Serial port monitor thread started\r\n"); while(true) { //Print to windows application //int batt = 0; //_wirelessSerial.printf("<%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%d:%d:%d:%d:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f:%1.2f::1.2f:%d:%1.2f:%1.2f:%1.2f:%1.2f>", //_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*/, _zeroValues[1], _zeroValues[2], _oldZeroValues[1], _oldZeroValues[2]); // 0 1 2 3 4 5 6 7 8 9 10 11 //Check comms mode and print correct data back to PC application switch(_commsMode) { //Motor power case 0: _wirelessSerial.printf("<M1=%1.2f:M2=%1.2f:M3=%1.2f:M4=%1.2f>", _motorPower[0], _motorPower[1], _motorPower[2], _motorPower[3]); break; //PID outputs case 1: _wirelessSerial.printf("<SYPID=%1.2f:SPPID=%1.2f:SRPID=%1.2f:RYPID=%1.2f:RPPID=%1.2f:RRPID=%1.2f>", _stabPIDControllerOutputs[0], _stabPIDControllerOutputs[1], _stabPIDControllerOutputs[2], _ratePIDControllerOutputs[0], _ratePIDControllerOutputs[1], _ratePIDControllerOutputs[2]); break; //IMU outputs case 2: _wirelessSerial.printf("<SY=%1.2f:SP=%1.2f:SR=%1.2f:RY=%1.2f:RP=%1.2f:RR=%1.2f>", _ypr[0], _ypr[1], _ypr[2], _gyroRate[0], _gyroRate[1], _gyroRate[2]); break; //Status case 3: _wirelessSerial.printf("<Batt=%d:Armed=%d:Init=%d:Rate=%d:Stab=%d:Lev=%d>", _batt, _armed, _initialised, _rate, _stab, _levelOffset); break; //Mapped RC commands case 4: _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f>", _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3]); break; //PID Tuning case 5: _wirelessSerial.printf("<RYPIDP=%1.2f:RYPIDI=%1.2f:RYPIDD=%1.2f:RPPIDP=%1.2f:RPPIDI=%1.2f:RPPIDD=%1.2f:RRPIDP=%1.2f:RRPIDI=%1.2f:RRPIDD=%1.2f:SYPIDP=%1.2f:SYPIDI=%1.2f:SYPIDD=%1.2f:SPPIDP=%1.2f:SPPIDI=%1.2f:SPPIDD=%1.2f:SRPIDP=%1.2f:SRPIDI=%1.2f:SRPIDD=%1.2f>", _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD); break; //GPS case 6: _wirelessSerial.printf("<GLat=%1.6f:GLon=%1.6f:GAlt=%1.2f:GAng=%1.2f:GSpd=%1.2f:GInit=%d>", _gpsValues[0], _gpsValues[1], _gpsValues[2], _gpsValues[3], _gpsValues[4], _gpsConnected); break; //Zero mode case 7: _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>", _zeroValues[0], _zeroValues[1], _zeroValues[2]); break; //Raw RC Commands case 8: _wirelessSerial.printf("<RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>", _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]); break; default: break; } //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; } } */ Thread::wait(100); } } //Checks for a valid command from the serial port and executes it //<Command=Value> 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) { //Start level offset mode to teach quad level case 'a': _levelOffset = true; break; //Arm disarm case 'b': if(_initialised == true && _armed == false) { Arm(); } else if(_armed == true) { 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 else _rcMappedCommands[0] = 0; break; //Set pitch case 'e': if(_armed == true) _rcMappedCommands[1] = value; //Pitch else _rcMappedCommands[1] = 0; break; //Set roll case 'f': if(_armed == true) _rcMappedCommands[2] = value; //Roll else _rcMappedCommands[2] = 0; break; //Set thrust case 'g': if(_armed == true) _rcMappedCommands[3] = value; //Thrust else _rcMappedCommands[3] = 0; 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; case 'z': _commsMode = value; break; case '1': _zeroValues[0] = 0; _zeroValues[1] = 0; _zeroValues[2] = 0; WriteSettingsToConfig(); break; default: break; } return; } //Checks for a valid command from the serial port and executes it //<%.2f:%.2f:%.2f:%.2f:%.2f:%d>", latitude, longitude, GnssInfo.altitude.meters(), GnssInfo.course.deg(), GnssInfo.speed.kph(), _connectedFlag); /*void CheckGPSSerialCommand() { int length = _gpsSerialRxPos; _gpsSerialBuffer[_gpsSerialRxPos] = 0; _gpsSerialRxPos = 0; if (length < 1) { return; } printf("command\r\n"); //Convert _gpsSerialBuffer to string //Split on : //Check it split into 6 parts //Convert to cirrect format and assign to vars 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; }*/ //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); }