Diff: flightController.h

Revision:

6:4c207e7b1203

Parent:

5:7b7db24ef6eb

Child:

7:bc5822aa8878

--- a/flightController.h	Mon Sep 22 10:16:31 2014 +0000
+++ b/flightController.h	Thu Jan 22 18:03:22 2015 +0000
@@ -18,7 +18,9 @@
 
 // A thread to control flight
 void FlightControllerThread(void const *args) 
-{         
+{    
+    printf("Flight controller thread started\r\n");
+     
     //Update Timer
     _flightControllerUpdateTimer = new RtosTimer(FlightControllerTask, osTimerPeriodic, (void *)0);
     int updateTime = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
@@ -98,10 +100,10 @@
     }
     
     //Testing
-    _ratePIDControllerOutputs[0] = 0; // yaw
+    //_ratePIDControllerOutputs[0] = 0; // yaw
     //_ratePIDControllerOutputs[1] = 0; // pitch
     //_ratePIDControllerOutputs[2] = 0; // roll
-    _stabPIDControllerOutputs[0] = 0; // yaw
+    //_stabPIDControllerOutputs[0] = 0; // yaw
     //_stabPIDControllerOutputs[1] = 0; // pitch
     //_stabPIDControllerOutputs[2] = 0; // roll
 
@@ -181,8 +183,26 @@
 
 void GetAttitude()
 {
+    //Use the 2 spare channels to alter the offset
+    if(_levelOffset == true)
+    {
+        float pitchOffset =_channel7MedianFilter->process(Map(_rcCommands[6], RC_OUT_MIN, RC_OUT_MAX, -5, 5));
+        float rollOffset =_channel8MedianFilter->process(Map(_rcCommands[7], RC_OUT_MIN, RC_OUT_MAX, -5, 5));
+        
+        _zeroValues[1] = _oldZeroValues[1] + pitchOffset;
+        _zeroValues[2] = _oldZeroValues[2] + rollOffset;
+    }
+    else 
+    {
+        _oldZeroValues[1] = _zeroValues[1];
+        _oldZeroValues[2] = _zeroValues[2];
+    }
+    
+    //_zeroValues[1] = _rcCommands[6];
+    //_zeroValues[2] = _rcCommands[7];
+    
     //Get raw data from IMU
-    _freeIMU.getYawPitchRoll(_ypr); // try get euler to get full rotation in each axis
+    _freeIMU.getYawPitchRoll(_ypr);
     _freeIMU.getRate(_gyroRate);
     
     //Take off zero values to account for any angle inbetween the PCB level and ground
@@ -190,19 +210,20 @@
     _ypr[2] = _ypr[2] - _zeroValues[2];
     
     //Swap pitch and roll angle because IMU is mounted at a right angle to the board
+    float yaw = _ypr[0];
     float pitch = _ypr[2];
-    float roll = -_ypr[1]; //Needs to be negative
+    float roll = - _ypr[1]; //Needs to be negative
+    _ypr[0] = yaw;
     _ypr[1] = pitch;
     _ypr[2] = roll;
     
-    //Swap pitch and roll rate because IMU is mounted at a right angle to the board
-    pitch = _gyroRate[2];
-    roll = -_gyroRate[1];
+    //Swap pitch, roll and yaw? rate because IMU is mounted at a right angle to the board
+    yaw = _gyroRate[2];
+    pitch = _gyroRate[0];
+    roll = _gyroRate[1];
+    _gyroRate[0] = yaw;
     _gyroRate[1] = pitch;
     _gyroRate[2] = roll;
-    
-    //Try swapping yaw
-    //_ypr[0] = - _ypr[0];
 }
 
 void NotFlying()