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-05-22
- Branch:
- DistanceRegulation
- Revision:
- 41:5fe200d20022
- Parent:
- 40:0aa1cefe80ab
File content as of revision 41:5fe200d20022:
#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); osThreadId id; id = Thread::gettid(); pc.printf("regulation gettid 0x%08X \n\r", id); Thread::wait(osWaitForever); } void distanceRegulationTask(void const *args){ //osThreadId id; //id = Thread::gettid(); //pc.printf("regulation task gettid 0x%08X \n\r", id); uint16_t channels[CHANNELS]; float distance1=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(); _groundDistance->setTunings(_P, _I, _D); _groundDistance->setSetPoint(_groundSetPoint); _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput); } } //distance1 = _sonic->getDistan1(); //pc.printf(" distance1 %f \n\r", distance1); if(_groundRegulation){ distance1 = _sonic->getDistan1(); _ppmRegen->getAllChannels(channels); _groundDistance->setProcessValue(distance1); //landing if(channels[AUX1] >= 1300){ //pc.printf("landing \n\r"); if(distance1 <= LANDING_HEIGHT) channels[AUX2] = 1000; _groundDistance->setLandingPoint((float)LANDING_HEIGHT); } groundDistancePIDValue = _groundDistance->compute(); //Update PWM values regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue; if(channels[AUX2] < 1500){ channels[THROTTLE] = 1010; } // limits of throttle if(regulatedThrottle > THROTTLE_LIMIT) regulatedThrottle = THROTTLE_LIMIT; if(regulatedThrottle < 1010) regulatedThrottle = 1010; // goAhead and back if(_groundDistance->quadStabilized() && _goAhead){ //pc.printf("going agead \n\r"); if(_frontDistance <= 40){ _frontWall = true; _backWall = false; } if(_backDistance <= 40){ _frontWall = false; _backWall = true; } if(_frontWall){ //pc.printf(" going backward \n\r"); _pitch->pulsewidth_us(channels[PITCH] - PITCH_MOVE_BACKWARD); } if(_backWall){ //pc.printf(" going forward \n\r"); _pitch->pulsewidth_us(channels[PITCH] + PITCH_MOVE_FORWARD); } //if(_frontDistance > 40 && _backDistance > 40) // _pitch->pulsewidth_us(channels[PITCH]); } else{ _pitch->pulsewidth_us(channels[PITCH]); } _roll->pulsewidth_us( channels[ROLL]); _yaw->pulsewidth_us( channels[YAW]); _throttle->pulsewidth_us(regulatedThrottle); _aux1->pulsewidth_us( channels[AUX1]); _aux2->pulsewidth_us( channels[AUX2]); } else { if(_groundDistance->quadStabilized()) _groundDistance->setNotStabelized(); // 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]); } }