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