main.cpp

00001 //Includes
00002 #include "mbed.h"
00003 #include "rtos.h"
00004 #include "FreeIMU.h"
00005 #include "PID.h"
00006 #include "ConfigFile.h"
00007 #include "hardware.h"
00008 #include "statusLights.h"
00009 #include "serialPortMonitor.h"
00010 #include "flightController.h"
00011 #include "rcCommandMonitor.h"
00012 #include "altitudeMonitor.h"
00013 
00014 //Declarations
00015 void LoadSettingsFromConfig();
00016 void InitialisePID();
00017 void InitialisePWM();
00018 void Setup();
00019 
00020 //Loads settings from the config file - could tidy this a little if I can be assed
00021 void LoadSettingsFromConfig()
00022 {
00023     _wiredSerial.printf("Starting to load settings from config file\n\r");
00024     
00025     //_wiredSerial.printf("Loading settings from config file\n\r");
00026     char value[BUFSIZ];
00027     
00028     //Read a configuration file from a mbed.
00029     if (!_configFile.read("/local/config.cfg"))
00030     {
00031         _wiredSerial.printf("Config file does not exist\n\r");
00032     }
00033     else
00034     {    
00035         //Get values
00036         if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
00037         else
00038         {
00039             _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
00040         }
00041         if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
00042             else
00043         {
00044             _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
00045         }
00046         if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
00047             else
00048         {
00049             _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
00050         }
00051         if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
00052             else
00053         {
00054             _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
00055         }
00056         if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
00057             else
00058         {
00059             _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
00060         }
00061         if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
00062             else
00063         {
00064             _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
00065         }
00066         if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
00067             else
00068         {
00069             _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
00070         }
00071         if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
00072             else
00073         {
00074             _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
00075         }
00076         if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
00077             else
00078         {
00079             _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
00080         }
00081         
00082         if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
00083             else
00084         {
00085             _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
00086         }
00087         if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
00088             else
00089         {
00090             _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
00091         }
00092         if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
00093             else
00094         {
00095             _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
00096         }
00097         if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
00098             else
00099         {
00100             _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
00101         }
00102         if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
00103             else
00104         {
00105             _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
00106         }
00107         if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
00108             else
00109         {
00110             _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
00111         }
00112         if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
00113             else
00114         {
00115             _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
00116         }
00117         if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
00118             else
00119         {
00120             _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
00121         }
00122         if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
00123             else
00124         {
00125             _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
00126         }
00127         if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
00128             else
00129         {
00130             _wiredSerial.printf("Failed to get value for zero pitch\n\r");
00131         }
00132         if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
00133             else
00134         {
00135             _wiredSerial.printf("Failed to get value for zero roll\n\r");
00136         }
00137     }
00138     
00139     _wiredSerial.printf("Finished loading settings from config file\n\r");
00140 }
00141 
00142 //PID initialisation
00143 void InitialisePID()
00144 {
00145     float updateTime = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
00146     
00147     _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
00148     _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
00149     _yawRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
00150     _yawRatePIDController->setMode(AUTO_MODE);
00151     _yawRatePIDController->setSetPoint(0.0);
00152     _yawRatePIDController->setBias(0);
00153     
00154     _pitchRatePIDController = new PID(_pitchRatePIDControllerP, _pitchRatePIDControllerI, _pitchRatePIDControllerD, updateTime);
00155     _pitchRatePIDController->setInputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
00156     _pitchRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
00157     _pitchRatePIDController->setMode(AUTO_MODE);
00158     _pitchRatePIDController->setSetPoint(0.0);
00159     _pitchRatePIDController->setBias(0);
00160     
00161     _rollRatePIDController = new PID(_rollRatePIDControllerP, _rollRatePIDControllerI, _rollRatePIDControllerD, updateTime);
00162     _rollRatePIDController->setInputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
00163     _rollRatePIDController->setOutputLimits(RATE_PID_CONTROLLER_OUTPUT_MIN, RATE_PID_CONTROLLER_OUTPUT_MAX);
00164     _rollRatePIDController->setMode(AUTO_MODE);
00165     _rollRatePIDController->setSetPoint(0.0);
00166     _rollRatePIDController->setBias(0);
00167     
00168     _yawStabPIDController = new PID(_yawStabPIDControllerP, _yawStabPIDControllerI, _yawStabPIDControllerD, updateTime);
00169     _yawStabPIDController->setInputLimits(IMU_YAW_ANGLE_MIN, IMU_YAW_ANGLE_MAX);
00170     _yawStabPIDController->setOutputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
00171     _yawStabPIDController->setMode(AUTO_MODE);
00172     _yawStabPIDController->setSetPoint(0.0);
00173     _yawStabPIDController->setBias(0);
00174     
00175     _pitchStabPIDController = new PID(_pitchStabPIDControllerP, _pitchStabPIDControllerI, _pitchStabPIDControllerD, updateTime);
00176     _pitchStabPIDController->setInputLimits(IMU_PITCH_ANGLE_MIN, IMU_PITCH_ANGLE_MAX);
00177     _pitchStabPIDController->setOutputLimits(IMU_PITCH_RATE_MIN, IMU_PITCH_RATE_MAX);
00178     _pitchStabPIDController->setMode(AUTO_MODE);
00179     _pitchStabPIDController->setSetPoint(0.0);
00180     _pitchStabPIDController->setBias(0);
00181     
00182     _rollStabPIDController = new PID(_rollStabPIDControllerP, _rollStabPIDControllerI, _rollStabPIDControllerD, updateTime);
00183     _rollStabPIDController->setInputLimits(IMU_ROLL_ANGLE_MIN, IMU_ROLL_ANGLE_MAX);
00184     _rollStabPIDController->setOutputLimits(IMU_ROLL_RATE_MIN, IMU_ROLL_RATE_MAX);
00185     _rollStabPIDController->setMode(AUTO_MODE);
00186     _rollStabPIDController->setSetPoint(0.0);
00187     _rollStabPIDController->setBias(0);
00188 }
00189 
00190 //PWM Initialisation
00191 void InitialisePWM()
00192 {
00193     //500Hz
00194     float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
00195     _motor1.period(period);
00196     _motor2.period(period);
00197     _motor3.period(period);
00198     _motor4.period(period);
00199     
00200     //Disable
00201     _motor1 = MOTORS_OFF;
00202     _motor2 = MOTORS_OFF;
00203     _motor2 = MOTORS_OFF;
00204     _motor2 = MOTORS_OFF;
00205 }
00206 
00207 //Setup
00208 void Setup()
00209 { 
00210     //Setup wired serial coms
00211     _wiredSerial.baud(115200);
00212     
00213     printf("\r\n");  
00214     printf("*********************************************************************************\r\n");
00215     printf("Starting Setup\r\n");
00216     printf("*********************************************************************************\r\n");
00217     
00218      //Disable buzzer
00219     _buzzer = 0;
00220     
00221     //Setup wireless serial coms
00222     _wirelessSerial.baud(57600);
00223     
00224     //Read config file
00225     LoadSettingsFromConfig();
00226     
00227     //Set initial RC Ccommands
00228     _rcMappedCommands[0] = 0;
00229     _rcMappedCommands[1] = 0;
00230     _rcMappedCommands[2] = 0;
00231     _rcMappedCommands[3] = 0;
00232     
00233     //Setup RC median filters
00234     _yawMedianFilter = new medianFilter(5);
00235     _pitchMedianFilter = new medianFilter(5);
00236     _rollMedianFilter = new medianFilter(5);
00237     _thrustMedianFilter = new medianFilter(5);
00238 
00239     //Initialise PPM
00240     _ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
00241 
00242     //Initialise IMU
00243     _freeIMU.init(true);
00244     
00245     //Initialise MaxBotix ping sensor
00246     _maxBotixTimer.start();
00247     
00248     //Initialise GPS
00249     _gps.baud(115200);
00250     
00251     //Initialise PID
00252     InitialisePID();
00253     
00254     //Initialise PWM
00255     InitialisePWM();
00256     
00257     //Set initialised flag
00258     _initialised = true;
00259        
00260     // Start threads
00261     _flightControllerThread = new Thread (FlightControllerThread);
00262     _flightControllerThread->set_priority(osPriorityRealtime);
00263     _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
00264     _rcCommandMonitorThread->set_priority(osPriorityHigh);
00265     _altitudeMonitorThread = new Thread (AltitudeMonitorThread);
00266     _altitudeMonitorThread->set_priority(osPriorityHigh);
00267     _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
00268     _serialPortMonitorThread->set_priority(osPriorityLow);
00269     _statusThread = new Thread(StatusThread);
00270     _statusThread->set_priority(osPriorityIdle);
00271     
00272     Thread::wait(1000);
00273     
00274     printf("*********************************************************************************\r\n");
00275     printf("Finished Setup\r\n");
00276     printf("*********************************************************************************\r\n");
00277 }
00278 
00279 int main()
00280 {   
00281     Setup(); 
00282     
00283     Thread::wait(osWaitForever);
00284 }