Eduard Medla / Mbed 2 deprecated Autonomous_quadcopter

Regenerating PPM signal based on distances from ultrasonic sensors, ESP8266 for connectin via wifi. Autonomous quadcopter behaviour, autonomou height holding. Flying direction based on front and back ultrasonic sensors.

Dependencies:   ConfigFile HCSR04 PID PPM2 mbed-rtos mbed

distanceRegulation.h

Committer:
edy05
Date:
2017-12-01
Branch:
DistanceRegulation
Revision:
26:11539036f0fb
Parent:
25:69190c222dbf
Child:
27:5956d5e3ff63

File content as of revision 26:11539036f0fb:

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


//RtosTimer *_groudRegulationUpdateTimer;

/*
void distanceRegulationThread(void const *args){
    
    _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);   
    int rate = 2; 
    _groudRegulationUpdateTimer->start(rate);
    
    Thread::wait(osWaitForever);
    
}
*/
void distanceRegulationTask(){
    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;
    
    while(1){
        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 {
            
            //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]);
        }
        
        Thread::wait(2);
            
        
    }
    
}