![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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:
- 2018-02-25
- Branch:
- DistanceRegulation
- Revision:
- 34:5dca557e982f
- Parent:
- 33:a2f9fea05cb9
- Child:
- 35:b09e19c855f6
File content as of revision 34:5dca557e982f:
#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 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{ _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("%d \n\r", distance1); //pc.printf("distance2: %d \n\r", distance2); _groundDistance->setProcessValue(distance1); groundDistancePIDValue = _groundDistance->compute(); //groundDistancePIDValue += _bias; //Update PWM values regulatedThrottle = channels[THROTTLE] + 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]); } }