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); } }