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:
- 1:045edcf091f3
- Parent:
- 0:0010a5abcc31
- Child:
- 3:82665e39f1ea
--- a/serialPortMonitor.h Fri May 09 10:04:36 2014 +0000
+++ b/serialPortMonitor.h Fri May 16 14:18:05 2014 +0000
@@ -101,19 +101,19 @@
break;
//Set yaw
case 'd':
- _rcCommands[0] = value; //Yaw
+ //_rcMappedCommands[0] = value; //Yaw
break;
//Set pitch
case 'e':
- _rcCommands[1] = value; //Pitch
+ //_rcMappedCommands[1] = value; //Pitch
break;
//Set roll
case 'f':
- _rcCommands[2] = value; //Roll
+ //_rcMappedCommands[2] = value; //Roll
break;
//Set thrust
case 'g':
- _rcCommands[3] = value; //Thrust
+ //_rcMappedCommands[3] = value; //Thrust
break;
//Set PID values
case 'h':
@@ -124,96 +124,92 @@
case 'i':
_yawRatePIDControllerI = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'j':
_yawRatePIDControllerD = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'k':
_pitchRatePIDControllerP = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'l':
_pitchRatePIDControllerI = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'm':
_pitchRatePIDControllerD = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'n':
_rollRatePIDControllerP = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'o':
_rollRatePIDControllerI = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'p':
_rollRatePIDControllerD = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'q':
_yawStabPIDControllerP = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'r':
_yawStabPIDControllerI = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 's':
_yawStabPIDControllerD = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 't':
_pitchStabPIDControllerP = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'u':
_pitchStabPIDControllerI = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'v':
_pitchStabPIDControllerD = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'w':
_rollStabPIDControllerP = value;
UpdatePID();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
case 'x':
_rollStabPIDControllerI = value;
UpdatePID();
- //WriteSettingsToConfig();
+ 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);
+ WriteSettingsToConfig();
break;
//Zero pitch and roll
case '1':
ZeroPitchRoll();
- //WriteSettingsToConfig();
+ WriteSettingsToConfig();
break;
default:
break;
@@ -227,147 +223,149 @@
{
_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))
+ if(_rcMappedCommands[3] < 0) //Not flying
{
- _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");
- }
+ _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(_rollStabPIDControllerI);
- if (!_configFile.setValue("_rollStabPIDControllerI", _str))
- {
- _wiredSerial.printf("Failed to write value for _rollStabPIDControllerI\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(_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[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);
}
-
- 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"))
+ else
{
- _wiredSerial.printf("Failure to write settings to configuration file.\n\r");
+ _wiredSerial.printf("Cannot write to config file whilst throttle is above 0\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 );
+ sprintf(_str, "%1.8f", number );
}
//Converts integer to char array
@@ -379,14 +377,12 @@
//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);
+ _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);
}
//Zero gyro, zero yaw and arm
@@ -395,17 +391,6 @@
//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;
}
@@ -414,8 +399,8 @@
void ZeroPitchRoll()
{
//Zero pitch and roll
- int totalPitch = 0;
- int totalRoll = 0;
+ float totalPitch = 0;
+ float totalRoll = 0;
float ypr[3] = {0,0,0}; // Yaw, pitch, roll
for(int i = 0; i < 500; i++)
{
@@ -426,6 +411,6 @@
_zeroValues[1] = totalPitch/500;
_zeroValues[2] = totalRoll/500;
-
- //WriteSettingsToConfig();
+ printf("Pitch %f\r\n", _zeroValues[1]);
+ printf("Roll %f\r\n", _zeroValues[2]);
}