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@0:0010a5abcc31, 2014-05-09 (annotated)
- Committer:
- joe4465
- Date:
- Fri May 09 10:04:36 2014 +0000
- Revision:
- 0:0010a5abcc31
- Child:
- 1:045edcf091f3
Added get rate function that returns the gyroscope rate - yaw, pitch, roll
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:0010a5abcc31 | 11 | |
joe4465 | 0:0010a5abcc31 | 12 | //Declarations |
joe4465 | 0:0010a5abcc31 | 13 | void LoadSettingsFromConfig(); |
joe4465 | 0:0010a5abcc31 | 14 | void InitialisePID(); |
joe4465 | 0:0010a5abcc31 | 15 | void InitialisePWM(); |
joe4465 | 0:0010a5abcc31 | 16 | void Setup(); |
joe4465 | 0:0010a5abcc31 | 17 | |
joe4465 | 0:0010a5abcc31 | 18 | //Loads settings from the config file |
joe4465 | 0:0010a5abcc31 | 19 | void LoadSettingsFromConfig() |
joe4465 | 0:0010a5abcc31 | 20 | { |
joe4465 | 0:0010a5abcc31 | 21 | _wiredSerial.printf("Loading settings from config file\n\r"); |
joe4465 | 0:0010a5abcc31 | 22 | |
joe4465 | 0:0010a5abcc31 | 23 | //_wiredSerial.printf("Loading settings from config file\n\r"); |
joe4465 | 0:0010a5abcc31 | 24 | char value[BUFSIZ]; |
joe4465 | 0:0010a5abcc31 | 25 | |
joe4465 | 0:0010a5abcc31 | 26 | //Read a configuration file from a mbed. |
joe4465 | 0:0010a5abcc31 | 27 | if (!_configFile.read("/local/config.cfg")) |
joe4465 | 0:0010a5abcc31 | 28 | { |
joe4465 | 0:0010a5abcc31 | 29 | _wiredSerial.printf("Config file does not exist\n\r"); |
joe4465 | 0:0010a5abcc31 | 30 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 31 | } |
joe4465 | 0:0010a5abcc31 | 32 | |
joe4465 | 0:0010a5abcc31 | 33 | //Get values |
joe4465 | 0:0010a5abcc31 | 34 | if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value); |
joe4465 | 0:0010a5abcc31 | 35 | else |
joe4465 | 0:0010a5abcc31 | 36 | { |
joe4465 | 0:0010a5abcc31 | 37 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 38 | _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r"); |
joe4465 | 0:0010a5abcc31 | 39 | } |
joe4465 | 0:0010a5abcc31 | 40 | if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value); |
joe4465 | 0:0010a5abcc31 | 41 | else |
joe4465 | 0:0010a5abcc31 | 42 | { |
joe4465 | 0:0010a5abcc31 | 43 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 44 | _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r"); |
joe4465 | 0:0010a5abcc31 | 45 | } |
joe4465 | 0:0010a5abcc31 | 46 | if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value); |
joe4465 | 0:0010a5abcc31 | 47 | else |
joe4465 | 0:0010a5abcc31 | 48 | { |
joe4465 | 0:0010a5abcc31 | 49 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 50 | _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r"); |
joe4465 | 0:0010a5abcc31 | 51 | } |
joe4465 | 0:0010a5abcc31 | 52 | if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value); |
joe4465 | 0:0010a5abcc31 | 53 | else |
joe4465 | 0:0010a5abcc31 | 54 | { |
joe4465 | 0:0010a5abcc31 | 55 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 56 | _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r"); |
joe4465 | 0:0010a5abcc31 | 57 | } |
joe4465 | 0:0010a5abcc31 | 58 | if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value); |
joe4465 | 0:0010a5abcc31 | 59 | else |
joe4465 | 0:0010a5abcc31 | 60 | { |
joe4465 | 0:0010a5abcc31 | 61 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 62 | _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r"); |
joe4465 | 0:0010a5abcc31 | 63 | } |
joe4465 | 0:0010a5abcc31 | 64 | if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value); |
joe4465 | 0:0010a5abcc31 | 65 | else |
joe4465 | 0:0010a5abcc31 | 66 | { |
joe4465 | 0:0010a5abcc31 | 67 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 68 | _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r"); |
joe4465 | 0:0010a5abcc31 | 69 | } |
joe4465 | 0:0010a5abcc31 | 70 | if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value); |
joe4465 | 0:0010a5abcc31 | 71 | else |
joe4465 | 0:0010a5abcc31 | 72 | { |
joe4465 | 0:0010a5abcc31 | 73 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 74 | _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r"); |
joe4465 | 0:0010a5abcc31 | 75 | } |
joe4465 | 0:0010a5abcc31 | 76 | if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value); |
joe4465 | 0:0010a5abcc31 | 77 | else |
joe4465 | 0:0010a5abcc31 | 78 | { |
joe4465 | 0:0010a5abcc31 | 79 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 80 | _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r"); |
joe4465 | 0:0010a5abcc31 | 81 | } |
joe4465 | 0:0010a5abcc31 | 82 | if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value); |
joe4465 | 0:0010a5abcc31 | 83 | else |
joe4465 | 0:0010a5abcc31 | 84 | { |
joe4465 | 0:0010a5abcc31 | 85 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 86 | _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r"); |
joe4465 | 0:0010a5abcc31 | 87 | } |
joe4465 | 0:0010a5abcc31 | 88 | |
joe4465 | 0:0010a5abcc31 | 89 | if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value); |
joe4465 | 0:0010a5abcc31 | 90 | else |
joe4465 | 0:0010a5abcc31 | 91 | { |
joe4465 | 0:0010a5abcc31 | 92 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 93 | _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r"); |
joe4465 | 0:0010a5abcc31 | 94 | } |
joe4465 | 0:0010a5abcc31 | 95 | if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value); |
joe4465 | 0:0010a5abcc31 | 96 | else |
joe4465 | 0:0010a5abcc31 | 97 | { |
joe4465 | 0:0010a5abcc31 | 98 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 99 | _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r"); |
joe4465 | 0:0010a5abcc31 | 100 | } |
joe4465 | 0:0010a5abcc31 | 101 | if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value); |
joe4465 | 0:0010a5abcc31 | 102 | else |
joe4465 | 0:0010a5abcc31 | 103 | { |
joe4465 | 0:0010a5abcc31 | 104 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 105 | _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r"); |
joe4465 | 0:0010a5abcc31 | 106 | } |
joe4465 | 0:0010a5abcc31 | 107 | if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value); |
joe4465 | 0:0010a5abcc31 | 108 | else |
joe4465 | 0:0010a5abcc31 | 109 | { |
joe4465 | 0:0010a5abcc31 | 110 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 111 | _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r"); |
joe4465 | 0:0010a5abcc31 | 112 | } |
joe4465 | 0:0010a5abcc31 | 113 | if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value); |
joe4465 | 0:0010a5abcc31 | 114 | else |
joe4465 | 0:0010a5abcc31 | 115 | { |
joe4465 | 0:0010a5abcc31 | 116 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 117 | _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r"); |
joe4465 | 0:0010a5abcc31 | 118 | } |
joe4465 | 0:0010a5abcc31 | 119 | if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value); |
joe4465 | 0:0010a5abcc31 | 120 | else |
joe4465 | 0:0010a5abcc31 | 121 | { |
joe4465 | 0:0010a5abcc31 | 122 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 123 | _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r"); |
joe4465 | 0:0010a5abcc31 | 124 | } |
joe4465 | 0:0010a5abcc31 | 125 | if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value); |
joe4465 | 0:0010a5abcc31 | 126 | else |
joe4465 | 0:0010a5abcc31 | 127 | { |
joe4465 | 0:0010a5abcc31 | 128 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 129 | _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r"); |
joe4465 | 0:0010a5abcc31 | 130 | } |
joe4465 | 0:0010a5abcc31 | 131 | if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value); |
joe4465 | 0:0010a5abcc31 | 132 | else |
joe4465 | 0:0010a5abcc31 | 133 | { |
joe4465 | 0:0010a5abcc31 | 134 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 135 | _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r"); |
joe4465 | 0:0010a5abcc31 | 136 | } |
joe4465 | 0:0010a5abcc31 | 137 | if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value); |
joe4465 | 0:0010a5abcc31 | 138 | else |
joe4465 | 0:0010a5abcc31 | 139 | { |
joe4465 | 0:0010a5abcc31 | 140 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 141 | _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r"); |
joe4465 | 0:0010a5abcc31 | 142 | } |
joe4465 | 0:0010a5abcc31 | 143 | if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value); |
joe4465 | 0:0010a5abcc31 | 144 | else |
joe4465 | 0:0010a5abcc31 | 145 | { |
joe4465 | 0:0010a5abcc31 | 146 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 147 | _wiredSerial.printf("Failed to get value for zero pitch\n\r"); |
joe4465 | 0:0010a5abcc31 | 148 | } |
joe4465 | 0:0010a5abcc31 | 149 | if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value); |
joe4465 | 0:0010a5abcc31 | 150 | else |
joe4465 | 0:0010a5abcc31 | 151 | { |
joe4465 | 0:0010a5abcc31 | 152 | _initialised = false; |
joe4465 | 0:0010a5abcc31 | 153 | _wiredSerial.printf("Failed to get value for zero roll\n\r"); |
joe4465 | 0:0010a5abcc31 | 154 | } |
joe4465 | 0:0010a5abcc31 | 155 | |
joe4465 | 0:0010a5abcc31 | 156 | if(_initialised == true) |
joe4465 | 0:0010a5abcc31 | 157 | { |
joe4465 | 0:0010a5abcc31 | 158 | _wiredSerial.printf("Finished loading settings from config file\n\r"); |
joe4465 | 0:0010a5abcc31 | 159 | } |
joe4465 | 0:0010a5abcc31 | 160 | else |
joe4465 | 0:0010a5abcc31 | 161 | { |
joe4465 | 0:0010a5abcc31 | 162 | _wiredSerial.printf("Failed to load settings from config file\n\r"); |
joe4465 | 0:0010a5abcc31 | 163 | } |
joe4465 | 0:0010a5abcc31 | 164 | } |
joe4465 | 0:0010a5abcc31 | 165 | |
joe4465 | 0:0010a5abcc31 | 166 | //PID initialisation |
joe4465 | 0:0010a5abcc31 | 167 | void InitialisePID() |
joe4465 | 0:0010a5abcc31 | 168 | { |
joe4465 | 0:0010a5abcc31 | 169 | float updateTime = 1.0 / UPDATE_FREQUENCY; |
joe4465 | 0:0010a5abcc31 | 170 | |
joe4465 | 0:0010a5abcc31 | 171 | _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime); |
joe4465 | 0:0010a5abcc31 | 172 | _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX); |
joe4465 | 0:0010a5abcc31 | 173 | _yawRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX); |
joe4465 | 0:0010a5abcc31 | 174 | _yawRatePIDController->setMode(AUTO_MODE); |
joe4465 | 0:0010a5abcc31 | 175 | _yawRatePIDController->setSetPoint(0.0); |
joe4465 | 0:0010a5abcc31 | 176 | _yawRatePIDController->setBias(0); |
joe4465 | 0:0010a5abcc31 | 177 | |
joe4465 | 0:0010a5abcc31 | 178 | _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime); |
joe4465 | 0:0010a5abcc31 | 179 | _pitchRatePIDController->setInputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX); |
joe4465 | 0:0010a5abcc31 | 180 | _pitchRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX); |
joe4465 | 0:0010a5abcc31 | 181 | _pitchRatePIDController->setMode(AUTO_MODE); |
joe4465 | 0:0010a5abcc31 | 182 | _pitchRatePIDController->setSetPoint(0.0); |
joe4465 | 0:0010a5abcc31 | 183 | _pitchRatePIDController->setBias(0); |
joe4465 | 0:0010a5abcc31 | 184 | |
joe4465 | 0:0010a5abcc31 | 185 | _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime); |
joe4465 | 0:0010a5abcc31 | 186 | _rollRatePIDController->setInputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX); |
joe4465 | 0:0010a5abcc31 | 187 | _rollRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX); |
joe4465 | 0:0010a5abcc31 | 188 | _rollRatePIDController->setMode(AUTO_MODE); |
joe4465 | 0:0010a5abcc31 | 189 | _rollRatePIDController->setSetPoint(0.0); |
joe4465 | 0:0010a5abcc31 | 190 | _rollRatePIDController->setBias(0); |
joe4465 | 0:0010a5abcc31 | 191 | |
joe4465 | 0:0010a5abcc31 | 192 | _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime); |
joe4465 | 0:0010a5abcc31 | 193 | _yawStabPIDController->setInputLimits(IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX); |
joe4465 | 0:0010a5abcc31 | 194 | _yawStabPIDController->setOutputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX); |
joe4465 | 0:0010a5abcc31 | 195 | _yawStabPIDController->setMode(AUTO_MODE); |
joe4465 | 0:0010a5abcc31 | 196 | _yawStabPIDController->setSetPoint(0.0); |
joe4465 | 0:0010a5abcc31 | 197 | _yawStabPIDController->setBias(0); |
joe4465 | 0:0010a5abcc31 | 198 | |
joe4465 | 0:0010a5abcc31 | 199 | _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime); |
joe4465 | 0:0010a5abcc31 | 200 | _pitchStabPIDController->setInputLimits(IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX); |
joe4465 | 0:0010a5abcc31 | 201 | _pitchStabPIDController->setOutputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX); |
joe4465 | 0:0010a5abcc31 | 202 | _pitchStabPIDController->setMode(AUTO_MODE); |
joe4465 | 0:0010a5abcc31 | 203 | _pitchStabPIDController->setSetPoint(0.0); |
joe4465 | 0:0010a5abcc31 | 204 | _pitchStabPIDController->setBias(0); |
joe4465 | 0:0010a5abcc31 | 205 | |
joe4465 | 0:0010a5abcc31 | 206 | _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime); |
joe4465 | 0:0010a5abcc31 | 207 | _rollStabPIDController->setInputLimits(IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX); |
joe4465 | 0:0010a5abcc31 | 208 | _rollStabPIDController->setOutputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX); |
joe4465 | 0:0010a5abcc31 | 209 | _rollStabPIDController->setMode(AUTO_MODE); |
joe4465 | 0:0010a5abcc31 | 210 | _rollStabPIDController->setSetPoint(0.0); |
joe4465 | 0:0010a5abcc31 | 211 | _rollStabPIDController->setBias(0); |
joe4465 | 0:0010a5abcc31 | 212 | } |
joe4465 | 0:0010a5abcc31 | 213 | |
joe4465 | 0:0010a5abcc31 | 214 | //PWM Initialisation |
joe4465 | 0:0010a5abcc31 | 215 | void InitialisePWM() |
joe4465 | 0:0010a5abcc31 | 216 | { |
joe4465 | 0:0010a5abcc31 | 217 | // |
joe4465 | 0:0010a5abcc31 | 218 | float period = 1.0 / MOTOR_PWM_FREQUENCY; |
joe4465 | 0:0010a5abcc31 | 219 | _motor1.period(period); |
joe4465 | 0:0010a5abcc31 | 220 | _motor2.period(period); |
joe4465 | 0:0010a5abcc31 | 221 | _motor3.period(period); |
joe4465 | 0:0010a5abcc31 | 222 | _motor4.period(period); |
joe4465 | 0:0010a5abcc31 | 223 | |
joe4465 | 0:0010a5abcc31 | 224 | //Disable |
joe4465 | 0:0010a5abcc31 | 225 | _motor1 = MOTORS_OFF; |
joe4465 | 0:0010a5abcc31 | 226 | _motor2 = MOTORS_OFF; |
joe4465 | 0:0010a5abcc31 | 227 | _motor2 = MOTORS_OFF; |
joe4465 | 0:0010a5abcc31 | 228 | _motor2 = MOTORS_OFF; |
joe4465 | 0:0010a5abcc31 | 229 | } |
joe4465 | 0:0010a5abcc31 | 230 | |
joe4465 | 0:0010a5abcc31 | 231 | //Setup |
joe4465 | 0:0010a5abcc31 | 232 | void Setup() |
joe4465 | 0:0010a5abcc31 | 233 | { |
joe4465 | 0:0010a5abcc31 | 234 | //Setup wired serial coms |
joe4465 | 0:0010a5abcc31 | 235 | _wiredSerial.baud(115200); |
joe4465 | 0:0010a5abcc31 | 236 | |
joe4465 | 0:0010a5abcc31 | 237 | //Setup wireless serial coms |
joe4465 | 0:0010a5abcc31 | 238 | _wirelessSerial.baud(57600); |
joe4465 | 0:0010a5abcc31 | 239 | |
joe4465 | 0:0010a5abcc31 | 240 | //Read config file |
joe4465 | 0:0010a5abcc31 | 241 | LoadSettingsFromConfig(); |
joe4465 | 0:0010a5abcc31 | 242 | |
joe4465 | 0:0010a5abcc31 | 243 | //Set initial RC Ccommands |
joe4465 | 0:0010a5abcc31 | 244 | _rcCommands[0] = 0; |
joe4465 | 0:0010a5abcc31 | 245 | _rcCommands[1] = 0; |
joe4465 | 0:0010a5abcc31 | 246 | _rcCommands[2] = 0; |
joe4465 | 0:0010a5abcc31 | 247 | _rcCommands[3] = 0; |
joe4465 | 0:0010a5abcc31 | 248 | |
joe4465 | 0:0010a5abcc31 | 249 | //Initialise IMU |
joe4465 | 0:0010a5abcc31 | 250 | _freeIMU.init(true); |
joe4465 | 0:0010a5abcc31 | 251 | |
joe4465 | 0:0010a5abcc31 | 252 | //Initialise PID |
joe4465 | 0:0010a5abcc31 | 253 | InitialisePID(); |
joe4465 | 0:0010a5abcc31 | 254 | |
joe4465 | 0:0010a5abcc31 | 255 | //Initialise PWM |
joe4465 | 0:0010a5abcc31 | 256 | InitialisePWM(); |
joe4465 | 0:0010a5abcc31 | 257 | |
joe4465 | 0:0010a5abcc31 | 258 | //Start new line |
joe4465 | 0:0010a5abcc31 | 259 | _wiredSerial.printf("\n\r"); |
joe4465 | 0:0010a5abcc31 | 260 | |
joe4465 | 0:0010a5abcc31 | 261 | // Start threads |
joe4465 | 0:0010a5abcc31 | 262 | if(_initialised == true) |
joe4465 | 0:0010a5abcc31 | 263 | { |
joe4465 | 0:0010a5abcc31 | 264 | _serialPortMonitorThread = new Thread (SerialPortMonitorThread); |
joe4465 | 0:0010a5abcc31 | 265 | _serialPortMonitorThread->set_priority(osPriorityLow); |
joe4465 | 0:0010a5abcc31 | 266 | _statusThread = new Thread(StatusThread); |
joe4465 | 0:0010a5abcc31 | 267 | _statusThread->set_priority(osPriorityIdle); |
joe4465 | 0:0010a5abcc31 | 268 | _flightControllerThread = new Thread (FlightControllerThread); |
joe4465 | 0:0010a5abcc31 | 269 | _flightControllerThread->set_priority(osPriorityRealtime); |
joe4465 | 0:0010a5abcc31 | 270 | } |
joe4465 | 0:0010a5abcc31 | 271 | } |
joe4465 | 0:0010a5abcc31 | 272 | |
joe4465 | 0:0010a5abcc31 | 273 | //MAIN LOOP////////////////////////////////////////////////////////////////////////// |
joe4465 | 0:0010a5abcc31 | 274 | int main() |
joe4465 | 0:0010a5abcc31 | 275 | { |
joe4465 | 0:0010a5abcc31 | 276 | Setup(); |
joe4465 | 0:0010a5abcc31 | 277 | |
joe4465 | 0:0010a5abcc31 | 278 | // Wait here forever |
joe4465 | 0:0010a5abcc31 | 279 | Thread::wait(osWaitForever); |
joe4465 | 0:0010a5abcc31 | 280 | } |