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