File content as of revision 41:5fe200d20022:

#include "mbed.h"
#include "rtos.h"
#include "hardware.h"
#include "definitions.h"

void distanceRegulationTask(void const *args);

RtosTimer *_groudRegulationUpdateTimer;

void distanceRegulationThread(void const *args){
    
    pc.printf("Flight controller thread started\r\n");
    
    _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void*)0);   
    int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000; 
    _groudRegulationUpdateTimer->start(rate);
    
    osThreadId id;
    id = Thread::gettid();
    pc.printf("regulation  gettid 0x%08X \n\r", id);
    
    Thread::wait(osWaitForever);
    
}

void distanceRegulationTask(void const *args){
    
    //osThreadId id;
    //id = Thread::gettid();
    //pc.printf("regulation task gettid 0x%08X \n\r", id);
   
    uint16_t channels[CHANNELS];
    float distance1=0;
    float groundDistancePIDValue;
    int regulatedThrottle;
    
    //PID SETUP
    if(_configChanges){
        _configChanges = false;
        if(_onlyDistanChanged){
            _onlyDistanChanged = false;
            //change only set point
            pc.printf("Only distance changed \n\r");
            _groundDistance->setSetPoint(_groundSetPoint);
        }
        else{
            pc.printf("P %f \n\r", _P);
            pc.printf("I %f \n\r", _I);
            pc.printf("D %f \n\r", _D);
            pc.printf("setpoint %f \n\r", _groundSetPoint);
            pc.printf("bias %f \n\r", _bias);
            _groundDistance->resetError();
            _groundDistance->setTunings(_P, _I, _D);
            _groundDistance->setSetPoint(_groundSetPoint);
            _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
        }
    }
    
    //distance1 = _sonic->getDistan1();
    //pc.printf("        distance1 %f \n\r", distance1);
    
    if(_groundRegulation){
        distance1 = _sonic->getDistan1();
        _ppmRegen->getAllChannels(channels);
        _groundDistance->setProcessValue(distance1);
        
        //landing
        if(channels[AUX1] >= 1300){
            //pc.printf("landing \n\r");
            if(distance1 <= LANDING_HEIGHT)
                channels[AUX2] = 1000;
            _groundDistance->setLandingPoint((float)LANDING_HEIGHT);                
        }
        
        groundDistancePIDValue = _groundDistance->compute();
        
        //Update PWM values
        regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
        if(channels[AUX2] < 1500){
            channels[THROTTLE] = 1010;    
        }
        
        // limits of throttle
        if(regulatedThrottle > THROTTLE_LIMIT)
            regulatedThrottle = THROTTLE_LIMIT;
        if(regulatedThrottle < 1010)
            regulatedThrottle = 1010;
        
        // goAhead and back
        if(_groundDistance->quadStabilized() && _goAhead){
            //pc.printf("going agead \n\r");
            if(_frontDistance <= 40){
                _frontWall = true;
                _backWall = false;
            }
            if(_backDistance <= 40){
                _frontWall = false;
                _backWall = true;
            }
            if(_frontWall){
                //pc.printf("     going backward \n\r");
                _pitch->pulsewidth_us(channels[PITCH] - PITCH_MOVE_BACKWARD);   
            }
            if(_backWall){
                //pc.printf("     going forward \n\r");
                _pitch->pulsewidth_us(channels[PITCH] + PITCH_MOVE_FORWARD);  
            }
            
            
            //if(_frontDistance > 40 && _backDistance > 40)
            //    _pitch->pulsewidth_us(channels[PITCH]);
            
        }
        else{
            _pitch->pulsewidth_us(channels[PITCH]);        
        }
        
        _roll->pulsewidth_us(    channels[ROLL]);
        _yaw->pulsewidth_us(     channels[YAW]);
        _throttle->pulsewidth_us(regulatedThrottle);
        _aux1->pulsewidth_us(    channels[AUX1]);
        _aux2->pulsewidth_us(    channels[AUX2]);
        
    }
    
    else {
        if(_groundDistance->quadStabilized())
            _groundDistance->setNotStabelized();
        
        
        // Generate PWM
        _ppmRegen->getAllChannels(channels);
        
        _roll->pulsewidth_us(    channels[ROLL]);
        _pitch->pulsewidth_us(   channels[PITCH]);
        _yaw->pulsewidth_us(     channels[YAW]);
        _throttle->pulsewidth_us(channels[THROTTLE]);
        _aux1->pulsewidth_us(    channels[AUX1]);
        _aux2->pulsewidth_us(    channels[AUX2]);
    }
    
    
}