Diff: main.cpp

Revision:

9:7b194f83e567

Parent:

7:bc5822aa8878

--- a/main.cpp	Tue Feb 10 20:58:12 2015 +0000
+++ b/main.cpp	Sun Feb 22 20:10:12 2015 +0000
@@ -9,6 +9,7 @@
 #include "serialPortMonitor.h"
 #include "flightController.h"
 #include "rcCommandMonitor.h"
+#include "altitudeMonitor.h"
 
 //Declarations
 void LoadSettingsFromConfig();
@@ -209,20 +210,16 @@
     //Setup wired serial coms
     _wiredSerial.baud(115200);
     
-    _buzzer = 0;
-    
     printf("\r\n");  
     printf("*********************************************************************************\r\n");
     printf("Starting Setup\r\n");
     printf("*********************************************************************************\r\n");
     
+     //Disable buzzer
+    _buzzer = 0;
+    
     //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();
@@ -232,61 +229,56 @@
     _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");
-    
+    _yawMedianFilter = new medianFilter(5);
+    _pitchMedianFilter = new medianFilter(5);
+    _rollMedianFilter = new medianFilter(5);
+    _thrustMedianFilter = new medianFilter(5);
+
     //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 MaxBotix ping sensor
+    _maxBotixTimer.start();
+    
+    //Initialise GPS
+    _gps.baud(115200);
     
     //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 threads
     _flightControllerThread = new Thread (FlightControllerThread);
     _flightControllerThread->set_priority(osPriorityRealtime);
     _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
     _rcCommandMonitorThread->set_priority(osPriorityHigh);
+    _altitudeMonitorThread = new Thread (AltitudeMonitorThread);
+    _altitudeMonitorThread->set_priority(osPriorityHigh);
     _serialPortMonitorThread = new Thread (SerialPortMonitorThread);
     _serialPortMonitorThread->set_priority(osPriorityLow);
     _statusThread = new Thread(StatusThread);
     _statusThread->set_priority(osPriorityIdle);
+    
+    Thread::wait(1000);
+    
+    printf("*********************************************************************************\r\n");
+    printf("Finished Setup\r\n");
+    printf("*********************************************************************************\r\n");
 }
 
 int main()
-{
+{   
     Setup(); 
     
-    // Wait here forever
-    Thread::wait(osWaitForever); 
-    
-    /*while(true)
-    {
-        printf("%f\r\n", _ypr[1]);    
-    }*/
+    Thread::wait(osWaitForever);
 }
\ No newline at end of file