Diff: main.cpp

Revision:

6:4c207e7b1203

Parent:

3:82665e39f1ea

Child:

7:bc5822aa8878

diff -r 7b7db24ef6eb -r 4c207e7b1203 main.cpp
--- a/main.cpp	Mon Sep 22 10:16:31 2014 +0000
+++ b/main.cpp	Thu Jan 22 18:03:22 2015 +0000
@@ -16,10 +16,10 @@
 void InitialisePWM();
 void Setup();
 
-//Loads settings from the config file
+//Loads settings from the config file - could tidy this a little if I can be assed
 void LoadSettingsFromConfig()
 {
-    _wiredSerial.printf("Loading settings from config file\n\r");
+    _wiredSerial.printf("Starting to load settings from config file\n\r");
     
     //_wiredSerial.printf("Loading settings from config file\n\r");
     char value[BUFSIZ];
@@ -28,140 +28,114 @@
     if (!_configFile.read("/local/config.cfg"))
     {
         _wiredSerial.printf("Config file does not exist\n\r");
-        _initialised = false;
     }
-    
-    //Get values
-    if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
     else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
-    }
-    if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
-    }
-    if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
-    }
-    if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
+    {    
+        //Get values
+        if (_configFile.getValue("_yawRatePIDControllerP", &value[0], sizeof(value))) _yawRatePIDControllerP = atof(value);
         else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
-    }
-    if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
-    }
-    if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
-    }
-    if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
-    }
-    if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
-    }
-    if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
+        {
+            _wiredSerial.printf("Failed to get value for _yawRatePIDControllerP\n\r");
+        }
+        if (_configFile.getValue("_yawRatePIDControllerI", &value[0], sizeof(value))) _yawRatePIDControllerI = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _yawRatePIDControllerI\n\r");
+        }
+        if (_configFile.getValue("_yawRatePIDControllerD", &value[0], sizeof(value))) _yawRatePIDControllerD = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _yawRatePIDControllerD\n\r");
+        }
+        if (_configFile.getValue("_pitchRatePIDControllerP", &value[0], sizeof(value))) _pitchRatePIDControllerP = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerP\n\r");
+        }
+        if (_configFile.getValue("_pitchRatePIDControllerI", &value[0], sizeof(value))) _pitchRatePIDControllerI = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerI\n\r");
+        }
+        if (_configFile.getValue("_pitchRatePIDControllerD", &value[0], sizeof(value))) _pitchRatePIDControllerD = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _pitchRatePIDControllerD\n\r");
+        }
+        if (_configFile.getValue("_rollRatePIDControllerP", &value[0], sizeof(value))) _rollRatePIDControllerP = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _rollRatePIDControllerP\n\r");
+        }
+        if (_configFile.getValue("_rollRatePIDControllerI", &value[0], sizeof(value))) _rollRatePIDControllerI = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _rollRatePIDControllerI\n\r");
+        }
+        if (_configFile.getValue("_rollRatePIDControllerD", &value[0], sizeof(value))) _rollRatePIDControllerD = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _rollRatePIDControllerD\n\r");
+        }
+        
+        if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
+        }
+        if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
+        }
+        if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
+        }
+        if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
+        }
+        if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
+        }
+        if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
+        }
+        if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
+        }
+        if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
+        }
+        if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
+        }
+        if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for zero pitch\n\r");
+        }
+        if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
+            else
+        {
+            _wiredSerial.printf("Failed to get value for zero roll\n\r");
+        }
     }
     
-    if (_configFile.getValue("_yawStabPIDControllerP", &value[0], sizeof(value))) _yawStabPIDControllerP = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _yawStabPIDControllerP\n\r");
-    }
-    if (_configFile.getValue("_yawStabPIDControllerI", &value[0], sizeof(value))) _yawStabPIDControllerI = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _yawStabPIDControllerI\n\r");
-    }
-    if (_configFile.getValue("_yawStabPIDControllerD", &value[0], sizeof(value))) _yawStabPIDControllerD = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _yawStabPIDControllerD\n\r");
-    }
-    if (_configFile.getValue("_pitchStabPIDControllerP", &value[0], sizeof(value))) _pitchStabPIDControllerP = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerP\n\r");
-    }
-    if (_configFile.getValue("_pitchStabPIDControllerI", &value[0], sizeof(value))) _pitchStabPIDControllerI = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerI\n\r");
-    }
-    if (_configFile.getValue("_pitchStabPIDControllerD", &value[0], sizeof(value))) _pitchStabPIDControllerD = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _pitchStabPIDControllerD\n\r");
-    }
-    if (_configFile.getValue("_rollStabPIDControllerP", &value[0], sizeof(value))) _rollStabPIDControllerP = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _rollStabPIDControllerP\n\r");
-    }
-    if (_configFile.getValue("_rollStabPIDControllerI", &value[0], sizeof(value))) _rollStabPIDControllerI = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _rollStabPIDControllerI\n\r");
-    }
-    if (_configFile.getValue("_rollStabPIDControllerD", &value[0], sizeof(value))) _rollStabPIDControllerD = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for _rollStabPIDControllerD\n\r");
-    }
-    if (_configFile.getValue("_zeroPitch", &value[0], sizeof(value))) _zeroValues[1] = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for zero pitch\n\r");
-    }
-    if (_configFile.getValue("_zeroRoll", &value[0], sizeof(value))) _zeroValues[2] = atof(value);
-        else
-    {
-        _initialised = false;
-        _wiredSerial.printf("Failed to get value for zero roll\n\r");
-    }
-    
-    if(_initialised == true)
-    {
-        _wiredSerial.printf("Finished loading settings from config file\n\r");
-    }
-    else
-    {
-        _wiredSerial.printf("Failed to load settings from config file\n\r");
-    }
+    _wiredSerial.printf("Finished loading settings from config file\n\r");
 }
 
 //PID initialisation
@@ -216,7 +190,7 @@
 void InitialisePWM()
 {
     //500Hz
-    float period = 1.0 / MOTOR_PWM_FREQUENCY;
+    float period = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
     _motor1.period(period);
     _motor2.period(period);
     _motor3.period(period);
@@ -231,15 +205,22 @@
 
 //Setup
 void Setup()
-{   
+{ 
     //Setup wired serial coms
     _wiredSerial.baud(115200);
     
+    printf("\r\n");  
+    printf("*********************************************************************************\r\n");
+    printf("Starting Setup\r\n");
+    printf("*********************************************************************************\r\n");
+    
     //Setup wireless serial coms
     _wirelessSerial.baud(57600);
+    printf("Setup wireless serial\r\n");
     
     //Setup GPS serial comms
     //_gpsSerial.baud(115200);
+    //printf("Setup GPS serial\r\n");
     
     //Read config file
     LoadSettingsFromConfig();
@@ -249,35 +230,50 @@
     _rcMappedCommands[1] = 0;
     _rcMappedCommands[2] = 0;
     _rcMappedCommands[3] = 0;
+    printf("Setup initial RC commands\r\n");
+    
+    //Setup RC median filters
+    _yawMedianFilter = new medianFilter(10);
+    _pitchMedianFilter = new medianFilter(10);
+    _rollMedianFilter = new medianFilter(10);
+    _thrustMedianFilter = new medianFilter(10);
+    _channel7MedianFilter = new medianFilter(10);
+    _channel8MedianFilter = new medianFilter(10);
+    printf("Setup RC median filters\r\n");
     
     //Initialise PPM
     _ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
+    printf("Setup PPM\r\n");
 
     //Initialise IMU
+    wait(1);
     _freeIMU.init(true);
+    printf("Setup IMU\r\n");
     
     //Initialise PID
     InitialisePID();
+    printf("Setup PID\r\n");
     
     //Initialise PWM
     InitialisePWM();
+    printf("Setup PWM\r\n");
+    
+    //Set initialised flag
+    _initialised = true;
+    
+    printf("*********************************************************************************\r\n");
+    printf("Finished Setup\r\n");
+    printf("*********************************************************************************\r\n");
        
-    //Start new line
-    _wiredSerial.printf("\n\r");
-    _wiredSerial.printf("Finished initialising: %s\n\r", _initialised ? "true" : "false");
-    
     // Start threads
-    if(_initialised == true)
-    {
-        _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
-        _serialPortMonitorThread->set_priority(osPriorityLow);
-        _statusThread = new Thread(StatusThread);
-        _statusThread->set_priority(osPriorityIdle);
-        _flightControllerThread = new Thread (FlightControllerThread);
-        _flightControllerThread->set_priority(osPriorityRealtime);
-        _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
-        _rcCommandMonitorThread->set_priority(osPriorityHigh);
-    }
+    _flightControllerThread = new Thread (FlightControllerThread);
+    _flightControllerThread->set_priority(osPriorityRealtime);
+    _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
+    _rcCommandMonitorThread->set_priority(osPriorityHigh);
+    _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
+    _serialPortMonitorThread->set_priority(osPriorityLow);
+    _statusThread = new Thread(StatusThread);
+    _statusThread->set_priority(osPriorityIdle);
 }
 
 int main()