Diff: main.cpp

Revision:

3:82665e39f1ea

Parent:

1:045edcf091f3

Child:

6:4c207e7b1203

--- a/main.cpp	Fri May 16 14:22:18 2014 +0000
+++ b/main.cpp	Thu Sep 18 08:45:46 2014 +0000
@@ -8,136 +8,13 @@
 #include "statusLights.h"
 #include "serialPortMonitor.h"
 #include "flightController.h"
-#include "MAF.h"
+#include "rcCommandMonitor.h"
 
 //Declarations
 void LoadSettingsFromConfig();
 void InitialisePID();
 void InitialisePWM();
 void Setup();
-void SignalRise();
-
-//VERY MESSY NNED TO TIDY
-
-//PPM STUFF
-Timer _PPMTimer;
-const int _numberOfPPMChannels=8; // This is the number of channels in your PPM signal
-unsigned char _currentPPMChannel=0; //This will point to the current channel in PPM frame. 
-int _PPMChannelValues[_numberOfPPMChannels]={0,0,0,0,0,0}; //This where the channels value is stored until frame is complete.
-unsigned int _PPMChannelTimes[_numberOfPPMChannels]={0,0,0,0,0,0}; //This where the channel pulse time value is stored until frame is complete.
-float C0s[_numberOfPPMChannels]; // Zeroth order coefficient for converting times to channels
-float C1s[_numberOfPPMChannels]; // First order coefficient for converting times to channels
-int _frameCount=0;
-int i;
-int _timeElapsedBetweenPPMInterrupts =0; //Keeps track of time between PPM interrupts
-int _miniumumSyncTime = 6000; // Minimum time of the sync pulse (us)
-int _shortPulseTime = 800; // If the pulse time for a channel is this short, something is wrong (us)
-int _pulseMin = 1000; // The minimum pulse time for a channel should be this long (us)
-int _pulseMax = 2000; // The maximum pulse time for a channel should be this long (us)
-int _channelOutputMin = RCMIN; // Desired minimum value for outputs
-int _channelOutputMax = RCMAX; // Desired maximum value for outputs
-const int JCCount=4; //Number of joystick channels
-unsigned char JoystickChannels[JCCount]={0,1,2,3}; // List of joystick channels
-char dum1,dum2;
-
-//RC command filters
-MAF _thrustFilter;
-MAF _yawFilter;
-MAF _pitchRateFilter;
-MAF _rollRateFilter;
-MAF _pitchStabFilter;
-MAF _rollStabFilter;
-
-//Here were all the work decoding the PPM signal takes place
-void SignalRise()
-{
-    _timeElapsedBetweenPPMInterrupts = _PPMTimer.read_us(); // Since we are measuring from signal rise to signal rise, note that _timeElapsedBetweenPPMInterrupts includes the fixed separator time as well
-    if (_timeElapsedBetweenPPMInterrupts < _shortPulseTime)
-    { 
-        return; //Channel timing too short; ignore, it's a glitch. Don't move to the next channel
-    }
-    __disable_irq();
-    //_PPMSignal.rise(NULL);
-    _PPMTimer.reset();  
-    if ((_timeElapsedBetweenPPMInterrupts > _miniumumSyncTime ) && (_currentPPMChannel != 0))
-    {   
-        //somehow before reaching the end of PPM frame you read   "New" frame signal???
-        //Ok, it happens. Just ignore this frame and start a new one        
-        _currentPPMChannel=0;              
-    }
-    if ((_timeElapsedBetweenPPMInterrupts > _miniumumSyncTime ) && (_currentPPMChannel == 0))
-    {
-        // This is good. You've received "New" frame signal as expected
-        __enable_irq();    
-        //_PPMSignal.rise(&SignalRise);   
-        return;
-    }
- 
-    // Process current channel. This is a correct channel in a correct frame so far
-    _PPMChannelValues[_currentPPMChannel]= C0s[_currentPPMChannel] + C1s[_currentPPMChannel]*_timeElapsedBetweenPPMInterrupts; // Normalize reading (Min: 900us Max: 1900 us). This is my readings, yours can be different
-    _PPMChannelTimes[_currentPPMChannel] = _timeElapsedBetweenPPMInterrupts;
-    _currentPPMChannel++;
-    
-    if (_currentPPMChannel==_numberOfPPMChannels ) 
-    {  
-        // great!, you've a complete correct frame. Set CanUpdate and start a new frame
-        _frameCount++;
-        _currentPPMChannel=0;
-        
-        //Aux Channels
-        float channel5 = map(_PPMChannelValues[4], RCMIN, RCMAX, 0, 1);
-        float channel6 = map(_PPMChannelValues[5], RCMIN, RCMAX, 0, 1);
-        float channel7 = map(_PPMChannelValues[6], RCMIN, RCMAX, 0, 1);
-        float channel8 = map(_PPMChannelValues[7], RCMIN, RCMAX, 0, 1);
-        
-        //Arm
-        if(channel5 > 0.4) 
-        {
-            //Zero gyro
-            _freeIMU.zeroGyro();
-            _armed = true;
-        }
-        else _armed = false;
-        
-        //Mode
-        if(channel6 > 0.4)
-        {
-            _rate = true;
-            _stab = false;
-        }
-        else
-        {
-            _rate = false;
-            _stab = true;
-        }
-        
-        //Filtering
-        float thrust = _thrustFilter.update(map(_PPMChannelValues[2], RCMIN, RCMAX, RC_THRUST_MIN, RC_THRUST_MAX));
-        float yaw = _yawFilter.update(map(_PPMChannelValues[3], RCMIN, RCMAX, RC_YAW_RATE_MIN, RC_YAW_RATE_MAX));
-        float rollRate = _rollRateFilter.update(map(_PPMChannelValues[0], RCMIN, RCMAX, RC_ROLL_RATE_MIN, RC_ROLL_RATE_MAX));
-        float pitchRate = _pitchRateFilter.update(map(_PPMChannelValues[1], RCMIN, RCMAX, RC_PITCH_RATE_MIN, RC_PITCH_RATE_MAX));
-        float rollStab = _rollStabFilter.update(map(_PPMChannelValues[0], RCMIN, RCMAX, RC_ROLL_ANGLE_MIN, RC_ROLL_ANGLE_MAX));
-        float pitchStab = _pitchStabFilter.update(map(_PPMChannelValues[1], RCMIN, RCMAX, RC_PITCH_ANGLE_MIN, RC_PITCH_ANGLE_MAX));
-        
-        //Set rc commands
-        //Rate mode
-        if(_rate == true && _stab == false)
-        {       
-            _rcMappedCommands[0] = yaw;
-            _rcMappedCommands[1] = pitchRate;
-            _rcMappedCommands[2] = rollRate;
-            _rcMappedCommands[3] = thrust;
-        }
-        else // Stab mode
-        {
-            _rcMappedCommands[0] = yaw;
-            _rcMappedCommands[1] = pitchStab;
-            _rcMappedCommands[2] = rollStab;
-            _rcMappedCommands[3] = thrust;
-        }            
-    }
-    __enable_irq();
-}
 
 //Loads settings from the config file
 void LoadSettingsFromConfig()
@@ -290,7 +167,7 @@
 //PID initialisation
 void InitialisePID()
 {
-    float updateTime = 1.0 / UPDATE_FREQUENCY;
+    float updateTime = 1.0 / FLIGHT_CONTROLLER_FREQUENCY;
     
     _yawRatePIDController = new PID(_yawRatePIDControllerP, _yawRatePIDControllerI, _yawRatePIDControllerD, updateTime);
     _yawRatePIDController->setInputLimits(IMU_YAW_RATE_MIN, IMU_YAW_RATE_MAX);
@@ -361,6 +238,9 @@
     //Setup wireless serial coms
     _wirelessSerial.baud(57600);
     
+    //Setup GPS serial comms
+    //_gpsSerial.baud(115200);
+    
     //Read config file
     LoadSettingsFromConfig();
     
@@ -369,6 +249,9 @@
     _rcMappedCommands[1] = 0;
     _rcMappedCommands[2] = 0;
     _rcMappedCommands[3] = 0;
+    
+    //Initialise PPM
+    _ppm = new PPM(_interruptPin, RC_OUT_MIN, RC_OUT_MAX, RC_IN_MIN, RC_IN_MAX, RC_CHANNELS, RC_THROTTLE_CHANNEL);
 
     //Initialise IMU
     _freeIMU.init(true);
@@ -378,33 +261,10 @@
     
     //Initialise PWM
     InitialisePWM();
-    
-    //Initalise PPM
-    _PPMSignal.mode (PullUp);
-    _PPMSignal.rise(&SignalRise); //Attach SignalRise routine to handle _PPMSignal rise
-    _PPMTimer.start();
-    
-    //Initialize all channels to produce "sane" outputs (depending on your definition of sanity ;-).
-    C1s[0] = 1.0*(_channelOutputMax-_channelOutputMin)/(_pulseMax-_pulseMin);
-    C0s[0] = 1.0*_channelOutputMin - _pulseMin*C1s[0];
-    for (i=1; i<_numberOfPPMChannels; i++)
-    {
-        C1s[i]=C1s[0];
-        C0s[i]=C0s[0];
-    }    
-    
-    // Initialize joystick channels using saved calibration information
-    FILE *fpi = fopen("/local/coefs.txt", "r");  // Open coefficient file on the local file system for reading
-    for(i=0; i<JCCount; i++)
-    {
-        fscanf(fpi, "%f%c%f%c",&C0s[JoystickChannels[i]],&dum1,&C1s[JoystickChannels[i]],&dum2);
-    }
-    fclose(fpi);
-    _PPMTimer.reset();
-    
+       
     //Start new line
     _wiredSerial.printf("\n\r");
-    _wiredSerial.printf("Finished initialising\n\r");
+    _wiredSerial.printf("Finished initialising: %s\n\r", _initialised ? "true" : "false");
     
     // Start threads
     if(_initialised == true)
@@ -415,10 +275,11 @@
         _statusThread->set_priority(osPriorityIdle);
         _flightControllerThread = new Thread (FlightControllerThread);
         _flightControllerThread->set_priority(osPriorityRealtime);
+        _rcCommandMonitorThread = new Thread (RcCommandMonitorThread);
+        _rcCommandMonitorThread->set_priority(osPriorityHigh);
     }
 }
 
-//MAIN LOOP//////////////////////////////////////////////////////////////////////////
 int main()
 {
     Setup();