File content as of revision 30:4d1b3926a3cc:

#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;
    
    //setup PID
    _groundDistance->setInputLimits(0, 300);
    //_groundDistance->setOutputLimits(100, 300);
    _groundDistance->setMode(AUTO_MODE);
    //_groundDistance->setSetPoint(0.0);
    //_groundDistance->setBias(0);
    _groundDistance->setTunings(_P, _I, _D);
    
    //Timer timer;
    
    
    if(_groundRegulation){
        //timer.reset();
        //timer.start();
        
        _ppmRegen->getAllChannels(channels);
        //PID SETUP
        if(_configChanges){
            _configChanges = false;
            _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]);
    }
    
}