Joseph Roberts / Mbed 2 deprecated QuadCopter

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

main.cpp@1:045edcf091f3, 2014-05-16 (annotated)

Committer:
joe4465
Date:
Fri May 16 14:18:05 2014 +0000
Revision:
1:045edcf091f3
Parent:
0:0010a5abcc31
Child:
3:82665e39f1ea
first commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
joe4465 0:0010a5abcc31 1 //Includes
joe4465 0:0010a5abcc31 2 #include "mbed.h"
joe4465 0:0010a5abcc31 3 #include "rtos.h"
joe4465 0:0010a5abcc31 4 #include "FreeIMU.h"
joe4465 0:0010a5abcc31 5 #include "PID.h"
joe4465 0:0010a5abcc31 6 #include "ConfigFile.h"
joe4465 0:0010a5abcc31 7 #include "hardware.h"
joe4465 0:0010a5abcc31 8 #include "statusLights.h"
joe4465 0:0010a5abcc31 9 #include "serialPortMonitor.h"
joe4465 0:0010a5abcc31 10 #include "flightController.h"
joe4465 1:045edcf091f3 11 #include "MAF.h"
joe4465 0:0010a5abcc31 12
joe4465 0:0010a5abcc31 13 //Declarations
joe4465 0:0010a5abcc31 14 void LoadSettingsFromConfig();
joe4465 0:0010a5abcc31 15 void InitialisePID();
joe4465 0:0010a5abcc31 16 void InitialisePWM();
joe4465 0:0010a5abcc31 17 void Setup();
joe4465 1:045edcf091f3 18 void SignalRise();
joe4465 1:045edcf091f3 19
joe4465 1:045edcf091f3 20 //VERY MESSY NNED TO TIDY
joe4465 1:045edcf091f3 21
joe4465 1:045edcf091f3 22 //PPM STUFF
joe4465 1:045edcf091f3 23 Timer _PPMTimer;
joe4465 1:045edcf091f3 24 const int _numberOfPPMChannels=8; // This is the number of channels in your PPM signal
joe4465 1:045edcf091f3 25 unsigned char _currentPPMChannel=0; //This will point to the current channel in PPM frame.
joe4465 1:045edcf091f3 26 int _PPMChannelValues[_numberOfPPMChannels]={0,0,0,0,0,0}; //This where the channels value is stored until frame is complete.
joe4465 1:045edcf091f3 27 unsigned int _PPMChannelTimes[_numberOfPPMChannels]={0,0,0,0,0,0}; //This where the channel pulse time value is stored until frame is complete.
joe4465 1:045edcf091f3 28 float C0s[_numberOfPPMChannels]; // Zeroth order coefficient for converting times to channels
joe4465 1:045edcf091f3 29 float C1s[_numberOfPPMChannels]; // First order coefficient for converting times to channels
joe4465 1:045edcf091f3 30 int _frameCount=0;
joe4465 1:045edcf091f3 31 int i;
joe4465 1:045edcf091f3 32 int _timeElapsedBetweenPPMInterrupts =0; //Keeps track of time between PPM interrupts
joe4465 1:045edcf091f3 33 int _miniumumSyncTime = 6000; // Minimum time of the sync pulse (us)
joe4465 1:045edcf091f3 34 int _shortPulseTime = 800; // If the pulse time for a channel is this short, something is wrong (us)
joe4465 1:045edcf091f3 35 int _pulseMin = 1000; // The minimum pulse time for a channel should be this long (us)
joe4465 1:045edcf091f3 36 int _pulseMax = 2000; // The maximum pulse time for a channel should be this long (us)
joe4465 1:045edcf091f3 37 int _channelOutputMin = RCMIN; // Desired minimum value for outputs
joe4465 1:045edcf091f3 38 int _channelOutputMax = RCMAX; // Desired maximum value for outputs
joe4465 1:045edcf091f3 39 const int JCCount=4; //Number of joystick channels
joe4465 1:045edcf091f3 40 unsigned char JoystickChannels[JCCount]={0,1,2,3}; // List of joystick channels
joe4465 1:045edcf091f3 41 char dum1,dum2;
joe4465 1:045edcf091f3 42
joe4465 1:045edcf091f3 43 //RC command filters
joe4465 1:045edcf091f3 44 MAF _thrustFilter;
joe4465 1:045edcf091f3 45 MAF _yawFilter;
joe4465 1:045edcf091f3 46 MAF _pitchRateFilter;
joe4465 1:045edcf091f3 47 MAF _rollRateFilter;
joe4465 1:045edcf091f3 48 MAF _pitchStabFilter;
joe4465 1:045edcf091f3 49 MAF _rollStabFilter;
joe4465 1:045edcf091f3 50
joe4465 1:045edcf091f3 51 //Here were all the work decoding the PPM signal takes place
joe4465 1:045edcf091f3 52 void SignalRise()
joe4465 1:045edcf091f3 53 {
joe4465 1:045edcf091f3 54 _timeElapsedBetweenPPMInterrupts = _PPMTimer.read_us(); // Since we are measuring from signal rise to signal rise, note that _timeElapsedBetweenPPMInterrupts includes the fixed separator time as well
joe4465 1:045edcf091f3 55 if (_timeElapsedBetweenPPMInterrupts < _shortPulseTime)
joe4465 1:045edcf091f3 56 {
joe4465 1:045edcf091f3 57 return; //Channel timing too short; ignore, it's a glitch. Don't move to the next channel
joe4465 1:045edcf091f3 58 }
joe4465 1:045edcf091f3 59 __disable_irq();
joe4465 1:045edcf091f3 60 //_PPMSignal.rise(NULL);
joe4465 1:045edcf091f3 61 _PPMTimer.reset();
joe4465 1:045edcf091f3 62 if ((_timeElapsedBetweenPPMInterrupts > _miniumumSyncTime ) && (_currentPPMChannel != 0))
joe4465 1:045edcf091f3 63 {
joe4465 1:045edcf091f3 64 //somehow before reaching the end of PPM frame you read "New" frame signal???
joe4465 1:045edcf091f3 65 //Ok, it happens. Just ignore this frame and start a new one
joe4465 1:045edcf091f3 66 _currentPPMChannel=0;
joe4465 1:045edcf091f3 67 }
joe4465 1:045edcf091f3 68 if ((_timeElapsedBetweenPPMInterrupts > _miniumumSyncTime ) && (_currentPPMChannel == 0))
joe4465 1:045edcf091f3 69 {
joe4465 1:045edcf091f3 70 // This is good. You've received "New" frame signal as expected
joe4465 1:045edcf091f3 71 __enable_irq();
joe4465 1:045edcf091f3 72 //_PPMSignal.rise(&SignalRise);
joe4465 1:045edcf091f3 73 return;
joe4465 1:045edcf091f3 74 }
joe4465 1:045edcf091f3 75
joe4465 1:045edcf091f3 76 // Process current channel. This is a correct channel in a correct frame so far
joe4465 1:045edcf091f3 77 _PPMChannelValues[_currentPPMChannel]= C0s[_currentPPMChannel] + C1s[_currentPPMChannel]*_timeElapsedBetweenPPMInterrupts; // Normalize reading (Min: 900us Max: 1900 us). This is my readings, yours can be different
joe4465 1:045edcf091f3 78 _PPMChannelTimes[_currentPPMChannel] = _timeElapsedBetweenPPMInterrupts;
joe4465 1:045edcf091f3 79 _currentPPMChannel++;
joe4465 1:045edcf091f3 80
joe4465 1:045edcf091f3 81 if (_currentPPMChannel==_numberOfPPMChannels )
joe4465 1:045edcf091f3 82 {
joe4465 1:045edcf091f3 83 // great!, you've a complete correct frame. Set CanUpdate and start a new frame
joe4465 1:045edcf091f3 84 _frameCount++;
joe4465 1:045edcf091f3 85 _currentPPMChannel=0;
joe4465 1:045edcf091f3 86
joe4465 1:045edcf091f3 87 //Aux Channels
joe4465 1:045edcf091f3 88 float channel5 = map(_PPMChannelValues[4], RCMIN, RCMAX, 0, 1);
joe4465 1:045edcf091f3 89 float channel6 = map(_PPMChannelValues[5], RCMIN, RCMAX, 0, 1);
joe4465 1:045edcf091f3 90 float channel7 = map(_PPMChannelValues[6], RCMIN, RCMAX, 0, 1);
joe4465 1:045edcf091f3 91 float channel8 = map(_PPMChannelValues[7], RCMIN, RCMAX, 0, 1);
joe4465 1:045edcf091f3 92
joe4465 1:045edcf091f3 93 //Arm
joe4465 1:045edcf091f3 94 if(channel5 > 0.4)
joe4465 1:045edcf091f3 95 {
joe4465 1:045edcf091f3 96 //Zero gyro
joe4465 1:045edcf091f3 97 _freeIMU.zeroGyro();
joe4465 1:045edcf091f3 98 _armed = true;
joe4465 1:045edcf091f3 99 }
joe4465 1:045edcf091f3 100 else _armed = false;
joe4465 1:045edcf091f3 101
joe4465 1:045edcf091f3 102 //Mode
joe4465 1:045edcf091f3 103 if(channel6 > 0.4)
joe4465 1:045edcf091f3 104 {
joe4465 1:045edcf091f3 105 _rate = true;
joe4465 1:045edcf091f3 106 _stab = false;
joe4465 1:045edcf091f3 107 }
joe4465 1:045edcf091f3 108 else
joe4465 1:045edcf091f3 109 {
joe4465 1:045edcf091f3 110 _rate = false;
joe4465 1:045edcf091f3 111 _stab = true;
joe4465 1:045edcf091f3 112 }
joe4465 1:045edcf091f3 113
joe4465 1:045edcf091f3 114 //Filtering
joe4465 1:045edcf091f3 115 float thrust = _thrustFilter.update(map(_PPMChannelValues[2], RCMIN, RCMAX, RC_THRUST_MIN, RC_THRUST_MAX));
joe4465 1:045edcf091f3 116 float yaw = _yawFilter.update(map(_PPMChannelValues[3], RCMIN, RCMAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
joe4465 1:045edcf091f3 117 float rollRate = _rollRateFilter.update(map(_PPMChannelValues[0], RCMIN, RCMAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
joe4465 1:045edcf091f3 118 float pitchRate = _pitchRateFilter.update(map(_PPMChannelValues[1], RCMIN, RCMAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX));
joe4465 1:045edcf091f3 119 float rollStab = _rollStabFilter.update(map(_PPMChannelValues[0], RCMIN, RCMAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
joe4465 1:045edcf091f3 120 float pitchStab = _pitchStabFilter.update(map(_PPMChannelValues[1], RCMIN, RCMAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX));
joe4465 1:045edcf091f3 121
joe4465 1:045edcf091f3 122 //Set rc commands
joe4465 1:045edcf091f3 123 //Rate mode
joe4465 1:045edcf091f3 124 if(_rate == true && _stab == false)
joe4465 1:045edcf091f3 125 {
joe4465 1:045edcf091f3 126 _rcMappedCommands[0] = yaw;
joe4465 1:045edcf091f3 127 _rcMappedCommands[1] = pitchRate;
joe4465 1:045edcf091f3 128 _rcMappedCommands[2] = rollRate;
joe4465 1:045edcf091f3 129 _rcMappedCommands[3] = thrust;
joe4465 1:045edcf091f3 130 }
joe4465 1:045edcf091f3 131 else // Stab mode
joe4465 1:045edcf091f3 132 {
joe4465 1:045edcf091f3 133 _rcMappedCommands[0] = yaw;
joe4465 1:045edcf091f3 134 _rcMappedCommands[1] = pitchStab;
joe4465 1:045edcf091f3 135 _rcMappedCommands[2] = rollStab;
joe4465 1:045edcf091f3 136 _rcMappedCommands[3] = thrust;
joe4465 1:045edcf091f3 137 }
joe4465 1:045edcf091f3 138 }
joe4465 1:045edcf091f3 139 __enable_irq();
joe4465 1:045edcf091f3 140 }
joe4465 0:0010a5abcc31 141
joe4465 0:0010a5abcc31 142 //Loads settings from the config file
joe4465 0:0010a5abcc31 143 void LoadSettingsFromConfig()
joe4465 0:0010a5abcc31 144 {
joe4465 0:0010a5abcc31 145 _wiredSerial.printf("Loading settings from config file\n\r");
joe4465 0:0010a5abcc31 146
joe4465 0:0010a5abcc31 147 //_wiredSerial.printf("Loading settings from config file\n\r");
joe4465 0:0010a5abcc31 148 char value[BUFSIZ];
joe4465 0:0010a5abcc31 149
joe4465 0:0010a5abcc31 150 //Read a configuration file from a mbed.
joe4465 0:0010a5abcc31 151 if (!_configFile.read("/local/config.cfg"))
joe4465 0:0010a5abcc31 152 {
joe4465 0:0010a5abcc31 153 _wiredSerial.printf("Config file does not exist\n\r");
joe4465 0:0010a5abcc31 154 _initialised = false;
joe4465 0:0010a5abcc31 155 }
joe4465 0:0010a5abcc31 156
joe4465 0:0010a5abcc31 157 //Get values
joe4465 0:0010a5abcc31 158 if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
joe4465 0:0010a5abcc31 159 else
joe4465 0:0010a5abcc31 160 {
joe4465 0:0010a5abcc31 161 _initialised = false;
joe4465 0:0010a5abcc31 162 _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
joe4465 0:0010a5abcc31 163 }
joe4465 0:0010a5abcc31 164 if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
joe4465 0:0010a5abcc31 165 else
joe4465 0:0010a5abcc31 166 {
joe4465 0:0010a5abcc31 167 _initialised = false;
joe4465 0:0010a5abcc31 168 _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
joe4465 0:0010a5abcc31 169 }
joe4465 0:0010a5abcc31 170 if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
joe4465 0:0010a5abcc31 171 else
joe4465 0:0010a5abcc31 172 {
joe4465 0:0010a5abcc31 173 _initialised = false;
joe4465 0:0010a5abcc31 174 _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
joe4465 0:0010a5abcc31 175 }
joe4465 0:0010a5abcc31 176 if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
joe4465 0:0010a5abcc31 177 else
joe4465 0:0010a5abcc31 178 {
joe4465 0:0010a5abcc31 179 _initialised = false;
joe4465 0:0010a5abcc31 180 _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
joe4465 0:0010a5abcc31 181 }
joe4465 0:0010a5abcc31 182 if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
joe4465 0:0010a5abcc31 183 else
joe4465 0:0010a5abcc31 184 {
joe4465 0:0010a5abcc31 185 _initialised = false;
joe4465 0:0010a5abcc31 186 _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
joe4465 0:0010a5abcc31 187 }
joe4465 0:0010a5abcc31 188 if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
joe4465 0:0010a5abcc31 189 else
joe4465 0:0010a5abcc31 190 {
joe4465 0:0010a5abcc31 191 _initialised = false;
joe4465 0:0010a5abcc31 192 _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
joe4465 0:0010a5abcc31 193 }
joe4465 0:0010a5abcc31 194 if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
joe4465 0:0010a5abcc31 195 else
joe4465 0:0010a5abcc31 196 {
joe4465 0:0010a5abcc31 197 _initialised = false;
joe4465 0:0010a5abcc31 198 _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
joe4465 0:0010a5abcc31 199 }
joe4465 0:0010a5abcc31 200 if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
joe4465 0:0010a5abcc31 201 else
joe4465 0:0010a5abcc31 202 {
joe4465 0:0010a5abcc31 203 _initialised = false;
joe4465 0:0010a5abcc31 204 _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
joe4465 0:0010a5abcc31 205 }
joe4465 0:0010a5abcc31 206 if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
joe4465 0:0010a5abcc31 207 else
joe4465 0:0010a5abcc31 208 {
joe4465 0:0010a5abcc31 209 _initialised = false;
joe4465 0:0010a5abcc31 210 _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
joe4465 0:0010a5abcc31 211 }
joe4465 0:0010a5abcc31 212
joe4465 0:0010a5abcc31 213 if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
joe4465 0:0010a5abcc31 214 else
joe4465 0:0010a5abcc31 215 {
joe4465 0:0010a5abcc31 216 _initialised = false;
joe4465 0:0010a5abcc31 217 _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
joe4465 0:0010a5abcc31 218 }
joe4465 0:0010a5abcc31 219 if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
joe4465 0:0010a5abcc31 220 else
joe4465 0:0010a5abcc31 221 {
joe4465 0:0010a5abcc31 222 _initialised = false;
joe4465 0:0010a5abcc31 223 _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
joe4465 0:0010a5abcc31 224 }
joe4465 0:0010a5abcc31 225 if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
joe4465 0:0010a5abcc31 226 else
joe4465 0:0010a5abcc31 227 {
joe4465 0:0010a5abcc31 228 _initialised = false;
joe4465 0:0010a5abcc31 229 _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
joe4465 0:0010a5abcc31 230 }
joe4465 0:0010a5abcc31 231 if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
joe4465 0:0010a5abcc31 232 else
joe4465 0:0010a5abcc31 233 {
joe4465 0:0010a5abcc31 234 _initialised = false;
joe4465 0:0010a5abcc31 235 _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
joe4465 0:0010a5abcc31 236 }
joe4465 0:0010a5abcc31 237 if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
joe4465 0:0010a5abcc31 238 else
joe4465 0:0010a5abcc31 239 {
joe4465 0:0010a5abcc31 240 _initialised = false;
joe4465 0:0010a5abcc31 241 _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
joe4465 0:0010a5abcc31 242 }
joe4465 0:0010a5abcc31 243 if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
joe4465 0:0010a5abcc31 244 else
joe4465 0:0010a5abcc31 245 {
joe4465 0:0010a5abcc31 246 _initialised = false;
joe4465 0:0010a5abcc31 247 _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
joe4465 0:0010a5abcc31 248 }
joe4465 0:0010a5abcc31 249 if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
joe4465 0:0010a5abcc31 250 else
joe4465 0:0010a5abcc31 251 {
joe4465 0:0010a5abcc31 252 _initialised = false;
joe4465 0:0010a5abcc31 253 _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
joe4465 0:0010a5abcc31 254 }
joe4465 0:0010a5abcc31 255 if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
joe4465 0:0010a5abcc31 256 else
joe4465 0:0010a5abcc31 257 {
joe4465 0:0010a5abcc31 258 _initialised = false;
joe4465 0:0010a5abcc31 259 _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
joe4465 0:0010a5abcc31 260 }
joe4465 0:0010a5abcc31 261 if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
joe4465 0:0010a5abcc31 262 else
joe4465 0:0010a5abcc31 263 {
joe4465 0:0010a5abcc31 264 _initialised = false;
joe4465 0:0010a5abcc31 265 _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
joe4465 0:0010a5abcc31 266 }
joe4465 0:0010a5abcc31 267 if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
joe4465 0:0010a5abcc31 268 else
joe4465 0:0010a5abcc31 269 {
joe4465 0:0010a5abcc31 270 _initialised = false;
joe4465 0:0010a5abcc31 271 _wiredSerial.printf("Failed to get value for zero pitch\n\r");
joe4465 0:0010a5abcc31 272 }
joe4465 0:0010a5abcc31 273 if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
joe4465 0:0010a5abcc31 274 else
joe4465 0:0010a5abcc31 275 {
joe4465 0:0010a5abcc31 276 _initialised = false;
joe4465 0:0010a5abcc31 277 _wiredSerial.printf("Failed to get value for zero roll\n\r");
joe4465 0:0010a5abcc31 278 }
joe4465 0:0010a5abcc31 279
joe4465 0:0010a5abcc31 280 if(_initialised == true)
joe4465 0:0010a5abcc31 281 {
joe4465 0:0010a5abcc31 282 _wiredSerial.printf("Finished loading settings from config file\n\r");
joe4465 0:0010a5abcc31 283 }
joe4465 0:0010a5abcc31 284 else
joe4465 0:0010a5abcc31 285 {
joe4465 0:0010a5abcc31 286 _wiredSerial.printf("Failed to load settings from config file\n\r");
joe4465 0:0010a5abcc31 287 }
joe4465 0:0010a5abcc31 288 }
joe4465 0:0010a5abcc31 289
joe4465 0:0010a5abcc31 290 //PID initialisation
joe4465 0:0010a5abcc31 291 void InitialisePID()
joe4465 0:0010a5abcc31 292 {
joe4465 0:0010a5abcc31 293 float updateTime = 1.0 / UPDATE_FREQUENCY;
joe4465 0:0010a5abcc31 294
joe4465 0:0010a5abcc31 295 _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 296 _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
joe4465 0:0010a5abcc31 297 _yawRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
joe4465 0:0010a5abcc31 298 _yawRatePIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 299 _yawRatePIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 300 _yawRatePIDController->setBias(0);
joe4465 0:0010a5abcc31 301
joe4465 0:0010a5abcc31 302 _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 303 _pitchRatePIDController->setInputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
joe4465 0:0010a5abcc31 304 _pitchRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
joe4465 0:0010a5abcc31 305 _pitchRatePIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 306 _pitchRatePIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 307 _pitchRatePIDController->setBias(0);
joe4465 0:0010a5abcc31 308
joe4465 0:0010a5abcc31 309 _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime);
joe4465 0:0010a5abcc31 310 _rollRatePIDController->setInputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
joe4465 0:0010a5abcc31 311 _rollRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
joe4465 0:0010a5abcc31 312 _rollRatePIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 313 _rollRatePIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 314 _rollRatePIDController->setBias(0);
joe4465 0:0010a5abcc31 315
joe4465 0:0010a5abcc31 316 _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 317 _yawStabPIDController->setInputLimits(IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX);
joe4465 0:0010a5abcc31 318 _yawStabPIDController->setOutputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
joe4465 0:0010a5abcc31 319 _yawStabPIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 320 _yawStabPIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 321 _yawStabPIDController->setBias(0);
joe4465 0:0010a5abcc31 322
joe4465 0:0010a5abcc31 323 _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 324 _pitchStabPIDController->setInputLimits(IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX);
joe4465 0:0010a5abcc31 325 _pitchStabPIDController->setOutputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
joe4465 0:0010a5abcc31 326 _pitchStabPIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 327 _pitchStabPIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 328 _pitchStabPIDController->setBias(0);
joe4465 0:0010a5abcc31 329
joe4465 0:0010a5abcc31 330 _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime);
joe4465 0:0010a5abcc31 331 _rollStabPIDController->setInputLimits(IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX);
joe4465 0:0010a5abcc31 332 _rollStabPIDController->setOutputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
joe4465 0:0010a5abcc31 333 _rollStabPIDController->setMode(AUTO_MODE);
joe4465 0:0010a5abcc31 334 _rollStabPIDController->setSetPoint(0.0);
joe4465 0:0010a5abcc31 335 _rollStabPIDController->setBias(0);
joe4465 0:0010a5abcc31 336 }
joe4465 0:0010a5abcc31 337
joe4465 0:0010a5abcc31 338 //PWM Initialisation
joe4465 0:0010a5abcc31 339 void InitialisePWM()
joe4465 0:0010a5abcc31 340 {
joe4465 1:045edcf091f3 341 //500Hz
joe4465 0:0010a5abcc31 342 float period = 1.0 / MOTOR_PWM_FREQUENCY;
joe4465 0:0010a5abcc31 343 _motor1.period(period);
joe4465 0:0010a5abcc31 344 _motor2.period(period);
joe4465 0:0010a5abcc31 345 _motor3.period(period);
joe4465 0:0010a5abcc31 346 _motor4.period(period);
joe4465 0:0010a5abcc31 347
joe4465 0:0010a5abcc31 348 //Disable
joe4465 0:0010a5abcc31 349 _motor1 = MOTORS_OFF;
joe4465 0:0010a5abcc31 350 _motor2 = MOTORS_OFF;
joe4465 0:0010a5abcc31 351 _motor2 = MOTORS_OFF;
joe4465 0:0010a5abcc31 352 _motor2 = MOTORS_OFF;
joe4465 0:0010a5abcc31 353 }
joe4465 0:0010a5abcc31 354
joe4465 0:0010a5abcc31 355 //Setup
joe4465 0:0010a5abcc31 356 void Setup()
joe4465 1:045edcf091f3 357 {
joe4465 0:0010a5abcc31 358 //Setup wired serial coms
joe4465 0:0010a5abcc31 359 _wiredSerial.baud(115200);
joe4465 0:0010a5abcc31 360
joe4465 0:0010a5abcc31 361 //Setup wireless serial coms
joe4465 0:0010a5abcc31 362 _wirelessSerial.baud(57600);
joe4465 0:0010a5abcc31 363
joe4465 0:0010a5abcc31 364 //Read config file
joe4465 0:0010a5abcc31 365 LoadSettingsFromConfig();
joe4465 0:0010a5abcc31 366
joe4465 0:0010a5abcc31 367 //Set initial RC Ccommands
joe4465 1:045edcf091f3 368 _rcMappedCommands[0] = 0;
joe4465 1:045edcf091f3 369 _rcMappedCommands[1] = 0;
joe4465 1:045edcf091f3 370 _rcMappedCommands[2] = 0;
joe4465 1:045edcf091f3 371 _rcMappedCommands[3] = 0;
joe4465 0:0010a5abcc31 372
joe4465 0:0010a5abcc31 373 //Initialise IMU
joe4465 0:0010a5abcc31 374 _freeIMU.init(true);
joe4465 0:0010a5abcc31 375
joe4465 0:0010a5abcc31 376 //Initialise PID
joe4465 0:0010a5abcc31 377 InitialisePID();
joe4465 0:0010a5abcc31 378
joe4465 0:0010a5abcc31 379 //Initialise PWM
joe4465 0:0010a5abcc31 380 InitialisePWM();
joe4465 0:0010a5abcc31 381
joe4465 1:045edcf091f3 382 //Initalise PPM
joe4465 1:045edcf091f3 383 _PPMSignal.mode (PullUp);
joe4465 1:045edcf091f3 384 _PPMSignal.rise(&SignalRise); //Attach SignalRise routine to handle _PPMSignal rise
joe4465 1:045edcf091f3 385 _PPMTimer.start();
joe4465 1:045edcf091f3 386
joe4465 1:045edcf091f3 387 //Initialize all channels to produce "sane" outputs (depending on your definition of sanity ;-).
joe4465 1:045edcf091f3 388 C1s[0] = 1.0*(_channelOutputMax-_channelOutputMin)/(_pulseMax-_pulseMin);
joe4465 1:045edcf091f3 389 C0s[0] = 1.0*_channelOutputMin - _pulseMin*C1s[0];
joe4465 1:045edcf091f3 390 for (i=1; i<_numberOfPPMChannels; i++)
joe4465 1:045edcf091f3 391 {
joe4465 1:045edcf091f3 392 C1s[i]=C1s[0];
joe4465 1:045edcf091f3 393 C0s[i]=C0s[0];
joe4465 1:045edcf091f3 394 }
joe4465 1:045edcf091f3 395
joe4465 1:045edcf091f3 396 // Initialize joystick channels using saved calibration information
joe4465 1:045edcf091f3 397 FILE *fpi = fopen("/local/coefs.txt", "r"); // Open coefficient file on the local file system for reading
joe4465 1:045edcf091f3 398 for(i=0; i<JCCount; i++)
joe4465 1:045edcf091f3 399 {
joe4465 1:045edcf091f3 400 fscanf(fpi, "%f%c%f%c",&C0s[JoystickChannels[i]],&dum1,&C1s[JoystickChannels[i]],&dum2);
joe4465 1:045edcf091f3 401 }
joe4465 1:045edcf091f3 402 fclose(fpi);
joe4465 1:045edcf091f3 403 _PPMTimer.reset();
joe4465 1:045edcf091f3 404
joe4465 0:0010a5abcc31 405 //Start new line
joe4465 0:0010a5abcc31 406 _wiredSerial.printf("\n\r");
joe4465 1:045edcf091f3 407 _wiredSerial.printf("Finished initialising\n\r");
joe4465 0:0010a5abcc31 408
joe4465 0:0010a5abcc31 409 // Start threads
joe4465 0:0010a5abcc31 410 if(_initialised == true)
joe4465 0:0010a5abcc31 411 {
joe4465 0:0010a5abcc31 412 _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
joe4465 0:0010a5abcc31 413 _serialPortMonitorThread->set_priority(osPriorityLow);
joe4465 0:0010a5abcc31 414 _statusThread = new Thread(StatusThread);
joe4465 0:0010a5abcc31 415 _statusThread->set_priority(osPriorityIdle);
joe4465 0:0010a5abcc31 416 _flightControllerThread = new Thread (FlightControllerThread);
joe4465 0:0010a5abcc31 417 _flightControllerThread->set_priority(osPriorityRealtime);
joe4465 0:0010a5abcc31 418 }
joe4465 0:0010a5abcc31 419 }
joe4465 0:0010a5abcc31 420
joe4465 0:0010a5abcc31 421 //MAIN LOOP//////////////////////////////////////////////////////////////////////////
joe4465 0:0010a5abcc31 422 int main()
joe4465 0:0010a5abcc31 423 {
joe4465 0:0010a5abcc31 424 Setup();
joe4465 0:0010a5abcc31 425
joe4465 0:0010a5abcc31 426 // Wait here forever
joe4465 0:0010a5abcc31 427 Thread::wait(osWaitForever);
joe4465 0:0010a5abcc31 428 }