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
Diff: serialPortMonitor.h
- Revision:
- 9:7b194f83e567
- Parent:
- 7:bc5822aa8878
--- a/serialPortMonitor.h Tue Feb 10 20:58:12 2015 +0000 +++ b/serialPortMonitor.h Sun Feb 22 20:10:12 2015 +0000 @@ -3,15 +3,13 @@ #include "hardware.h" //Declarations -void CheckWirelessSerialCommand(); -//void CheckGPSSerialCommand(); +void CheckSerialCommand(); void UpdatePID(); +void getGPS(); //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) @@ -20,12 +18,6 @@ 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) { @@ -55,13 +47,13 @@ //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]); + _wirelessSerial.printf("<MRCY=%1.2f:MRCP=%1.2f:MRCR=%1.2f:MRCT=%1.2f:RRC1=%1.2f:RRC2=%1.2f:RRC3=%1.2f:RRC4=%1.2f:RRC5=%1.2f:RRC6=%1.2f:RRC7=%1.2f:RRC8=%1.2f>", + _rcMappedCommands[0], _rcMappedCommands[1], _rcMappedCommands[2], _rcMappedCommands[3], _rcCommands[0], _rcCommands[1], _rcCommands[2], _rcCommands[3], _rcCommands[4], _rcCommands[5], _rcCommands[6], _rcCommands[7]); 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>", + _wirelessSerial.printf("<RYPIDP=%1.6f:RYPIDI=%1.6f:RYPIDD=%1.6f:RPPIDP=%1.6f:RPPIDI=%1.6f:RPPIDD=%1.6f:RRPIDP=%1.6f:RRPIDI=%1.6f:RRPIDD=%1.6f:SYPIDP=%1.6f:SYPIDI=%1.6f:SYPIDD=%1.6f:SPPIDP=%1.6f:SPPIDI=%1.6f:SPPIDD=%1.6f:SRPIDP=%1.6f:SRPIDI=%1.6f:SRPIDD=%1.6f>", _yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, _pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, _rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, _yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, _pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, _rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD); break; @@ -75,23 +67,40 @@ case 7: _wirelessSerial.printf("<ZY=%1.6f:ZP=%1.6f:ZR=%1.6f>", _zeroValues[0], _zeroValues[1], _zeroValues[2]); - break; + break; - //Raw RC Commands + //Rate tuning 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]); + //Yaw set point, Yaw actual, Yaw PID output + //Pitch set point, Pitch actual, Pitch PID output + //Roll set point, Roll actual, Roll PID output + _wirelessSerial.printf("<MRCY=%1.2f:RY=%1.2f:RYPID=%1.2f:MRCP=%1.2f:RP=%1.2f:RPPID=%1.2f:MRCR=%1.2f:RR=%1.2f:RRPID=%1.2f>", + _rcMappedCommands[0], _gyroRate[0], _ratePIDControllerOutputs[0], _rcMappedCommands[1], _gyroRate[1], _ratePIDControllerOutputs[1], _rcMappedCommands[2], _gyroRate[2], _ratePIDControllerOutputs[2]); break; + //Stab tuning + case 9: + //Yaw set point, Yaw actual, Yaw PID output + //Pitch set point, Pitch actual, Pitch PID output + //Roll set point, Roll actual, Roll PID output + _wirelessSerial.printf("<MRCY=%1.2f:SY=%1.2f:SYPID=%1.2f:MRCP=%1.2f:SP=%1.2f:SPPID=%1.2f:MRCR=%1.2f:SR=%1.2f:SRPID=%1.2f>", + _rcMappedCommands[0], _ypr[0], _stabPIDControllerOutputs[0], _rcMappedCommands[1], _ypr[1], _stabPIDControllerOutputs[1], _rcMappedCommands[2], _ypr[2], _stabPIDControllerOutputs[2]); + break; + + //Altitude + case 10: + _wirelessSerial.printf("<GAlt=%1.2f:PAlt=%1.2f:BAlt=%1.2f>", + _gpsValues[2], _maxBotixPingAltitude, _barometerAltitude); + default: break; - } + } //Check for wireless serial command - if (_wirelessSerial.readable() > 0) + while (_wirelessSerial.readable() > 0) { int c = _wirelessSerial.getc(); - + switch (c) { case 60: // @@ -101,7 +110,7 @@ case 10: // LF case 13: // CR case 62: // > - CheckWirelessSerialCommand(); + CheckSerialCommand(); break; default: @@ -112,44 +121,25 @@ } break; } - } - + } + //Check for GPS serial command - /*if (_gpsSerial.readable() > 0) + while(_gps.readable() > 0) { - int c = _gpsSerial.getc(); - //printf("%c", c); - _wiredSerial.putc(c); - - switch (c) + int c = _gps.getc(); + if(_tinyGPS.encode(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; + getGPS(); } - } */ + } + Thread::wait(100); } } //Checks for a valid command from the serial port and executes it //<Command=Value> -void CheckWirelessSerialCommand() +void CheckSerialCommand() { int length = _wirelessSerialRxPos; _wirelessSerialBuffer[_wirelessSerialRxPos] = 0; @@ -352,74 +342,22 @@ 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]); - } +void getGPS() +{ + unsigned long fix_age; + _tinyGPS.f_get_position(&_gpsValues[0], &_gpsValues[1], &fix_age); + + _gpsValues[2] = _tinyGPS.f_altitude(); + _gpsValues[3] = _tinyGPS.f_course(); + _gpsValues[4] = _tinyGPS.f_speed_kmph(); - 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; -}*/ + if (fix_age == TinyGPS::GPS_INVALID_AGE) + _gpsConnected = false; + else if (fix_age > 5000) + _gpsConnected = false; + else + _gpsConnected = true; +} //Updates PID tunings void UpdatePID()