File content as of revision 36:ed8b7b7b6cfa:

#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];
    float distance1=0;
    int distance2=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();
            //pc.printf("PID tunings changed \n\r");    
            _groundDistance->setTunings(_P, _I, _D);
            _groundDistance->setSetPoint(_groundSetPoint);
            _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
            //_groundDistance->setBias(_bias);
        }
    }
    
    //distance1 = _sonic->getDistan1();
    //pc.printf("%d \n\r", distance1);
    
    if(_groundRegulation){
        //timer.reset();
        //timer.start();
        
        _ppmRegen->getAllChannels(channels);
        
        distance1 = _sonic->getDistan1();
        //distance2 = _sonic->getDistan2();
        //pc.printf("%f \n\r", distance1);
        //float p = _groundDistance->getPParam();
        //float i = _groundDistance->getIParam();
        //float d = _groundDistance->getDParam();
        //float max = _groundDistance->getInMax();
        //pc.printf("%f \n\r", max);
        //pc.printf("%f \n\r", p);
        //pc.printf("%f \n\r", i);
        //pc.printf("%f \n\r", d);
        //pc.printf("distance2: %d \n\r", distance2);
        _groundDistance->setProcessValue(distance1);
        groundDistancePIDValue = _groundDistance->compute();
        //pc.printf("pid value %f \n\r", groundDistancePIDValue);
        
        //Update PWM values
        regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
        if(channels[AUX2] < 1500){
            channels[THROTTLE] = 1010;    
        }
        if(regulatedThrottle > THROTTLE_LIMIT)
            regulatedThrottle = THROTTLE_LIMIT;
        if(regulatedThrottle < 1010)
            regulatedThrottle = 1010;
        
        
        //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 final value: %d \n\r", regulatedThrottle);
        // Generate PWM
        _ppmRegen->getAllChannels(channels);
        //pc.printf("channel value: %d \n\r", channels[ROLL]);
        //pc.printf("channel value: %d \n\r", channels[PITCH]);
        //pc.printf("channel value: %d \n\r", channels[YAW]);
        //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
        //pc.printf("channel value: %d \n\r", channels[AUX1]);
        //pc.printf("channel value: %d \n\r", channels[AUX2]);
        
        _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]);
    }
    
}