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@33:a2f9fea05cb9, 2018-02-18 (annotated)

Committer:
edy05
Date:
Sun Feb 18 17:10:48 2018 +0000
Branch:
DistanceRegulation
Revision:
33:a2f9fea05cb9
Parent:
32:c729e6da7f9a
Child:
34:5dca557e982f
stable Rel1

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 31:5f1737e480f3 15 int rate = (1.0 / FLIGHT_CONTROLLER_FREQUENCY) * 1000;
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 33:a2f9fea05cb9 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 32:c729e6da7f9a 31 //PID SETUP
edy05 32:c729e6da7f9a 32 if(_configChanges){
edy05 32:c729e6da7f9a 33 _configChanges = false;
edy05 32:c729e6da7f9a 34 if(_onlyDistanChanged){
edy05 32:c729e6da7f9a 35 _onlyDistanChanged = false;
edy05 32:c729e6da7f9a 36 //change only set point
edy05 32:c729e6da7f9a 37 pc.printf("Only distance changed \n\r");
edy05 32:c729e6da7f9a 38 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 39 }
edy05 32:c729e6da7f9a 40 else{
edy05 32:c729e6da7f9a 41 _groundDistance->resetError();
edy05 32:c729e6da7f9a 42 //pc.printf("PID tunings changed \n\r");
edy05 32:c729e6da7f9a 43 _groundDistance->setTunings(_P, _I, _D);
edy05 32:c729e6da7f9a 44 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 45 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 32:c729e6da7f9a 46 _groundDistance->setBias(_bias);
edy05 32:c729e6da7f9a 47 }
edy05 32:c729e6da7f9a 48 }
edy05 32:c729e6da7f9a 49
edy05 27:5956d5e3ff63 50 if(_groundRegulation){
edy05 27:5956d5e3ff63 51 //timer.reset();
edy05 27:5956d5e3ff63 52 //timer.start();
edy05 27:5956d5e3ff63 53
edy05 27:5956d5e3ff63 54 _ppmRegen->getAllChannels(channels);
edy05 32:c729e6da7f9a 55
edy05 27:5956d5e3ff63 56 distance = _sonic->getDistan();
edy05 33:a2f9fea05cb9 57 //pc.printf("distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 58 _groundDistance->setProcessValue(distance);
edy05 27:5956d5e3ff63 59 groundDistancePIDValue = _groundDistance->compute();
edy05 31:5f1737e480f3 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 33:a2f9fea05cb9 94
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 33:a2f9fea05cb9 98 //pc.printf("channel value: %d \n\r", channels[ROLL]);
edy05 33:a2f9fea05cb9 99 //pc.printf("channel value: %d \n\r", channels[PITCH]);
edy05 33:a2f9fea05cb9 100 //pc.printf("channel value: %d \n\r", channels[YAW]);
edy05 33:a2f9fea05cb9 101 //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
edy05 33:a2f9fea05cb9 102 //pc.printf("channel value: %d \n\r", channels[AUX1]);
edy05 33:a2f9fea05cb9 103 //pc.printf("channel value: %d \n\r", channels[AUX2]);
edy05 33:a2f9fea05cb9 104
edy05 27:5956d5e3ff63 105 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 106 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 107 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 108 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 109 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 110 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 111 }
edy05 2:d172c9963f87 112
edy05 2:d172c9963f87 113 }