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@27:5956d5e3ff63, 2018-01-20 (annotated)

Committer:
edy05
Date:
Sat Jan 20 11:44:46 2018 +0000
Branch:
DistanceRegulation
Revision:
27:5956d5e3ff63
Parent:
26:11539036f0fb
Child:
30:4d1b3926a3cc
Child:
31:5f1737e480f3
Update performance;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
edy05 2:d172c9963f87 1 #include "mbed.h"
edy05 2:d172c9963f87 2 #include "rtos.h"
edy05 2:d172c9963f87 3 #include "hardware.h"
edy05 2:d172c9963f87 4 #include "definitions.h"
edy05 2:d172c9963f87 5
edy05 27:5956d5e3ff63 6 void distanceRegulationTask(void const *args);
edy05 26:11539036f0fb 7
edy05 27:5956d5e3ff63 8 RtosTimer *_groudRegulationUpdateTimer;
edy05 26:11539036f0fb 9
edy05 26:11539036f0fb 10 void distanceRegulationThread(void const *args){
edy05 26:11539036f0fb 11
edy05 27:5956d5e3ff63 12 pc.printf("Flight controller thread started\r\n");
edy05 27:5956d5e3ff63 13
edy05 26:11539036f0fb 14 _groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);
edy05 27:5956d5e3ff63 15 int rate = 5;
edy05 26:11539036f0fb 16 _groudRegulationUpdateTimer->start(rate);
edy05 26:11539036f0fb 17
edy05 26:11539036f0fb 18 Thread::wait(osWaitForever);
edy05 26:11539036f0fb 19
edy05 26:11539036f0fb 20 }
edy05 27:5956d5e3ff63 21
edy05 27:5956d5e3ff63 22 void distanceRegulationTask(void const *args){
edy05 27:5956d5e3ff63 23
edy05 27:5956d5e3ff63 24 //pc.printf("Flight controller task started\r\n");
edy05 27:5956d5e3ff63 25
edy05 2:d172c9963f87 26 uint16_t channels[CHANNELS];
edy05 2:d172c9963f87 27 int distance=0;
edy05 18:e0c5ab76f337 28 float groundDistancePIDValue;
edy05 19:9ceec179b140 29 int regulatedThrottle;
edy05 2:d172c9963f87 30
edy05 2:d172c9963f87 31 //setup PID
edy05 26:11539036f0fb 32 _groundDistance->setInputLimits(0, 300);
edy05 25:69190c222dbf 33 //_groundDistance->setOutputLimits(100, 300);
edy05 2:d172c9963f87 34 _groundDistance->setMode(AUTO_MODE);
edy05 25:69190c222dbf 35 //_groundDistance->setSetPoint(0.0);
edy05 25:69190c222dbf 36 //_groundDistance->setBias(0);
edy05 13:33024b5880b3 37 _groundDistance->setTunings(_P, _I, _D);
edy05 2:d172c9963f87 38
edy05 26:11539036f0fb 39 //Timer timer;
edy05 26:11539036f0fb 40
edy05 27:5956d5e3ff63 41
edy05 27:5956d5e3ff63 42 if(_groundRegulation){
edy05 27:5956d5e3ff63 43 //timer.reset();
edy05 27:5956d5e3ff63 44 //timer.start();
edy05 27:5956d5e3ff63 45
edy05 27:5956d5e3ff63 46 _ppmRegen->getAllChannels(channels);
edy05 27:5956d5e3ff63 47 //PID SETUP
edy05 27:5956d5e3ff63 48 if(_configChanges){
edy05 27:5956d5e3ff63 49 _configChanges = false;
edy05 27:5956d5e3ff63 50 _groundDistance->resetError();
edy05 27:5956d5e3ff63 51 //pc.printf("PID tunings changed \n\r");
edy05 27:5956d5e3ff63 52 _groundDistance->setTunings(_P, _I, _D);
edy05 27:5956d5e3ff63 53 _groundDistance->setSetPoint(_groundSetPoint);
edy05 27:5956d5e3ff63 54 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 27:5956d5e3ff63 55 _groundDistance->setBias(_bias);
edy05 27:5956d5e3ff63 56 }
edy05 27:5956d5e3ff63 57 distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 58 _groundDistance->setProcessValue(distance);
edy05 27:5956d5e3ff63 59 groundDistancePIDValue = _groundDistance->compute();
edy05 27:5956d5e3ff63 60 groundDistancePIDValue += _bias;
edy05 27:5956d5e3ff63 61
edy05 27:5956d5e3ff63 62
edy05 27:5956d5e3ff63 63 //Update PWM values
edy05 27:5956d5e3ff63 64 regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
edy05 27:5956d5e3ff63 65 if(channels[AUX2] < 1500){
edy05 27:5956d5e3ff63 66 channels[THROTTLE] = 1010;
edy05 13:33024b5880b3 67 }
edy05 27:5956d5e3ff63 68 if(regulatedThrottle > 2000)
edy05 27:5956d5e3ff63 69 regulatedThrottle = 2000;
edy05 27:5956d5e3ff63 70 if(regulatedThrottle < 1010)
edy05 27:5956d5e3ff63 71 regulatedThrottle = 1010;
edy05 27:5956d5e3ff63 72
edy05 27:5956d5e3ff63 73
edy05 27:5956d5e3ff63 74 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 75 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 76 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 27:5956d5e3ff63 77 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 2:d172c9963f87 78
edy05 27:5956d5e3ff63 79 // Generate regulated PWM
edy05 27:5956d5e3ff63 80 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 81 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 82 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 83 _throttle->pulsewidth_us(regulatedThrottle);
edy05 27:5956d5e3ff63 84 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 85 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 86 //timer.stop();
edy05 27:5956d5e3ff63 87 //pc.printf("Timer: %d \n\r", timer.read_us());
edy05 2:d172c9963f87 88
edy05 2:d172c9963f87 89 }
edy05 27:5956d5e3ff63 90 else {
edy05 27:5956d5e3ff63 91 //distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 92 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 93 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 94 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 27:5956d5e3ff63 95 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 27:5956d5e3ff63 96 // Generate PWM
edy05 27:5956d5e3ff63 97 _ppmRegen->getAllChannels(channels);
edy05 27:5956d5e3ff63 98 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 99 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 100 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 101 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 102 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 103 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 104 }
edy05 2:d172c9963f87 105
edy05 2:d172c9963f87 106 }