Joseph Roberts
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:
- 6:4c207e7b1203
- Parent:
- 5:7b7db24ef6eb
- Child:
- 7:bc5822aa8878
--- a/serialPortMonitor.h Mon Sep 22 10:16:31 2014 +0000
+++ b/serialPortMonitor.h Thu Jan 22 18:03:22 2015 +0000
@@ -3,99 +3,148 @@
#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;
+//char _gpsSerialBuffer[255];
+//int _gpsSerialRxPos = 0;
// 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);
+{
+ printf("Serial port monitor thread started\r\n");
- // Wait here forever
- Thread::wait(osWaitForever);
-}
-
-void SerialPortMonitorTask(void const *n)
-{
- //Print to windows application
- 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*/);
+ 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 for wireless serial command
- if (_wirelessSerial.readable() > 0)
- {
- int c = _wirelessSerial.getc();
-
- switch (c)
+ //Check comms mode and print correct data back to PC application
+ switch(_commsMode)
{
- case 60: //
- _wirelessSerialRxPos = 0;
+ //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;
- case 10: // LF
- case 13: // CR
- case 62: // >
- CheckWirelessSerialCommand();
+ //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:
- _wirelessSerialBuffer[_wirelessSerialRxPos++] = c;
- if (_wirelessSerialRxPos > 200)
- {
- _wirelessSerialRxPos = 0;
- }
- break;
+ break;
}
- }
-
- //Check for GPS serial command
- /*if (_gpsSerial.readable() > 0)
- {
- int c = _gpsSerial.getc();
- //printf("%c", c);
- _wiredSerial.putc(c);
- switch (c)
+ //Check for wireless serial command
+ if (_wirelessSerial.readable() > 0)
{
- case 60: // <
- _gpsSerialRxPos = 0;
- break;
+ int c = _wirelessSerial.getc();
- case 10: // LF
- case 13: // CR
- case 62: // >
- CheckGPSSerialCommand();
- break;
+ switch (c)
+ {
+ case 60: //
+ _wirelessSerialRxPos = 0;
+ break;
- default:
- _gpsSerialBuffer[_gpsSerialRxPos++] = c;
- if (_gpsSerialRxPos > 200)
- {
+ 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;
- printf("oh no \r\n");
- }
- break;
- }
- } */
+ 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
@@ -120,20 +169,23 @@
switch (command)
{
- //Spare
+ //Start level offset mode to teach quad level
case 'a':
+ _levelOffset = true;
break;
+
//Arm disarm
case 'b':
if(_initialised == true && _armed == false)
{
Arm();
}
- else
+ else if(_armed == true)
{
Disarm();
}
break;
+
//Set mode
case 'c':
if(_rate == true)
@@ -147,118 +199,151 @@
_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;
- //Zero pitch and roll
+
+ case 'z':
+ _commsMode = value;
+ break;
+
case '1':
- ZeroPitchRoll();
+ _zeroValues[0] = 0;
+ _zeroValues[1] = 0;
+ _zeroValues[2] = 0;
WriteSettingsToConfig();
break;
+
default:
break;
}
@@ -335,162 +420,6 @@
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()
{