File content as of revision 31:5f1737e480f3:

#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);
    
    Thread::wait(osWaitForever);
    
}

void distanceRegulationTask(void const *args){
    
    //pc.printf("Flight controller task started\r\n");
    
    uint16_t channels[CHANNELS];
    int distance=0;
    float groundDistancePIDValue;
    int regulatedThrottle;
    
    if(_groundRegulation){
        //timer.reset();
        //timer.start();
        
        _ppmRegen->getAllChannels(channels);
        //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{
                _groundDistance->resetError();
                //pc.printf("PID tunings changed \n\r");    
                _groundDistance->setTunings(_P, _I, _D);
                _groundDistance->setSetPoint(_groundSetPoint);
                _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
                _groundDistance->setBias(_bias);
            }
        }
        distance = _sonic->getDistan();
        _groundDistance->setProcessValue(distance);
        groundDistancePIDValue = _groundDistance->compute();
        //groundDistancePIDValue += _bias;
        
        
        //Update PWM values
        regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
        if(channels[AUX2] < 1500){
            channels[THROTTLE] = 1010;    
        }
        if(regulatedThrottle > 2000)
            regulatedThrottle = 2000;
        if(regulatedThrottle < 1010)
            regulatedThrottle = 1010;
        
        
        //pc.printf("Sonic distance: %d \n\r", distance);
        //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
        //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
        //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
        
        // Generate  regulated PWM
        _roll->pulsewidth_us(    channels[ROLL]);
        _pitch->pulsewidth_us(   channels[PITCH]);
        _yaw->pulsewidth_us(     channels[YAW]);
        _throttle->pulsewidth_us(regulatedThrottle);
        _aux1->pulsewidth_us(    channels[AUX1]);
        _aux2->pulsewidth_us(    channels[AUX2]);
        //timer.stop();
        //pc.printf("Timer: %d \n\r", timer.read_us());
        
    }
    else {
        //distance = _sonic->getDistan();
        //pc.printf("Sonic distance: %d \n\r", distance);
        //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); 
        //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
        //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
        // 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]);
    }
    
}