distanceRegulation.h

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "hardware.h"
00004 #include "definitions.h"
00005 
00006 void distanceRegulationTask(void const *args);
00007 
00008 RtosTimer *_groudRegulationUpdateTimer;
00009 
00010 void distanceRegulationThread(void const *args){
00011     
00012     pc.printf("Flight controller thread started\r\n");
00013     
00014     _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void*)0);   
00015     int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; 
00016     _groudRegulationUpdateTimer->start(rate);
00017     
00018     osThreadId id;
00019     id = Thread::gettid();
00020     pc.printf("regulation  gettid 0x%08X \n\r", id);
00021     
00022     Thread::wait(osWaitForever);
00023     
00024 }
00025 
00026 void distanceRegulationTask(void const *args){
00027     
00028     //osThreadId id;
00029     //id = Thread::gettid();
00030     //pc.printf("regulation task gettid 0x%08X \n\r", id);
00031    
00032     uint16_t channels[CHANNELS];
00033     float distance1=0;
00034     float groundDistancePIDValue;
00035     int regulatedThrottle;
00036     
00037     //PID SETUP
00038     if(_configChanges){
00039         _configChanges = false;
00040         if(_onlyDistanChanged){
00041             _onlyDistanChanged = false;
00042             //change only set point
00043             pc.printf("Only distance changed \n\r");
00044             _groundDistance->setSetPoint(_groundSetPoint);
00045         }
00046         else{
00047             pc.printf("P %f \n\r", _P);
00048             pc.printf("I %f \n\r", _I);
00049             pc.printf("D %f \n\r", _D);
00050             pc.printf("setpoint %f \n\r", _groundSetPoint);
00051             pc.printf("bias %f \n\r", _bias);
00052             _groundDistance->resetError();
00053             _groundDistance->setTunings(_P, _I, _D);
00054             _groundDistance->setSetPoint(_groundSetPoint);
00055             _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
00056         }
00057     }
00058     
00059     //distance1 = _sonic->getDistan1();
00060     //pc.printf("        distance1 %f \n\r", distance1);
00061     
00062     if(_groundRegulation){
00063         distance1 = _sonic->getDistan1();
00064         _ppmRegen->getAllChannels(channels);
00065         _groundDistance->setProcessValue(distance1);
00066         
00067         //landing
00068         if(channels[AUX1] >= 1300){
00069             //pc.printf("landing \n\r");
00070             if(distance1 <= LANDING_HEIGHT)
00071                 channels[AUX2] = 1000;
00072             _groundDistance->setLandingPoint((float)LANDING_HEIGHT);                
00073         }
00074         
00075         groundDistancePIDValue = _groundDistance->compute();
00076         
00077         //Update PWM values
00078         regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
00079         if(channels[AUX2] < 1500){
00080             channels[THROTTLE] = 1010;    
00081         }
00082         
00083         // limits of throttle
00084         if(regulatedThrottle > THROTTLE_LIMIT)
00085             regulatedThrottle = THROTTLE_LIMIT;
00086         if(regulatedThrottle < 1010)
00087             regulatedThrottle = 1010;
00088         
00089         // goAhead and back
00090         if(_groundDistance->quadStabilized() && _goAhead){
00091             //pc.printf("going agead \n\r");
00092             if(_frontDistance <= 40){
00093                 _frontWall = true;
00094                 _backWall = false;
00095             }
00096             if(_backDistance <= 40){
00097                 _frontWall = false;
00098                 _backWall = true;
00099             }
00100             if(_frontWall){
00101                 //pc.printf("     going backward \n\r");
00102                 _pitch->pulsewidth_us(channels[PITCH] - PITCH_MOVE_BACKWARD);   
00103             }
00104             if(_backWall){
00105                 //pc.printf("     going forward \n\r");
00106                 _pitch->pulsewidth_us(channels[PITCH] + PITCH_MOVE_FORWARD);  
00107             }
00108             
00109             
00110             //if(_frontDistance > 40 && _backDistance > 40)
00111             //    _pitch->pulsewidth_us(channels[PITCH]);
00112             
00113         }
00114         else{
00115             _pitch->pulsewidth_us(channels[PITCH]);        
00116         }
00117         
00118         _roll->pulsewidth_us(    channels[ROLL]);
00119         _yaw->pulsewidth_us(     channels[YAW]);
00120         _throttle->pulsewidth_us(regulatedThrottle);
00121         _aux1->pulsewidth_us(    channels[AUX1]);
00122         _aux2->pulsewidth_us(    channels[AUX2]);
00123         
00124     }
00125     
00126     else {
00127         if(_groundDistance->quadStabilized())
00128             _groundDistance->setNotStabelized();
00129         
00130         
00131         // Generate PWM
00132         _ppmRegen->getAllChannels(channels);
00133         
00134         _roll->pulsewidth_us(    channels[ROLL]);
00135         _pitch->pulsewidth_us(   channels[PITCH]);
00136         _yaw->pulsewidth_us(     channels[YAW]);
00137         _throttle->pulsewidth_us(channels[THROTTLE]);
00138         _aux1->pulsewidth_us(    channels[AUX1]);
00139         _aux2->pulsewidth_us(    channels[AUX2]);
00140     }
00141     
00142     
00143 }