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@34:5dca557e982f, 2018-02-25 (annotated)

Committer:
edy05
Date:
Sun Feb 25 18:32:13 2018 +0000
Branch:
DistanceRegulation
Revision:
34:5dca557e982f
Parent:
33:a2f9fea05cb9
Child:
35:b09e19c855f6
server - fixed sendpage - I hope

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 34:5dca557e982f 27 int distance1=0;
edy05 34:5dca557e982f 28 int distance2=0;
edy05 18:e0c5ab76f337 29 float groundDistancePIDValue;
edy05 19:9ceec179b140 30 int regulatedThrottle;
edy05 2:d172c9963f87 31
edy05 32:c729e6da7f9a 32 //PID SETUP
edy05 32:c729e6da7f9a 33 if(_configChanges){
edy05 32:c729e6da7f9a 34 _configChanges = false;
edy05 32:c729e6da7f9a 35 if(_onlyDistanChanged){
edy05 32:c729e6da7f9a 36 _onlyDistanChanged = false;
edy05 32:c729e6da7f9a 37 //change only set point
edy05 32:c729e6da7f9a 38 pc.printf("Only distance changed \n\r");
edy05 32:c729e6da7f9a 39 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 40 }
edy05 32:c729e6da7f9a 41 else{
edy05 32:c729e6da7f9a 42 _groundDistance->resetError();
edy05 32:c729e6da7f9a 43 //pc.printf("PID tunings changed \n\r");
edy05 32:c729e6da7f9a 44 _groundDistance->setTunings(_P, _I, _D);
edy05 32:c729e6da7f9a 45 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 46 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 32:c729e6da7f9a 47 _groundDistance->setBias(_bias);
edy05 32:c729e6da7f9a 48 }
edy05 32:c729e6da7f9a 49 }
edy05 32:c729e6da7f9a 50
edy05 34:5dca557e982f 51 //distance1 = _sonic->getDistan1();
edy05 34:5dca557e982f 52 //pc.printf("%d \n\r", distance1);
edy05 34:5dca557e982f 53
edy05 27:5956d5e3ff63 54 if(_groundRegulation){
edy05 27:5956d5e3ff63 55 //timer.reset();
edy05 27:5956d5e3ff63 56 //timer.start();
edy05 27:5956d5e3ff63 57
edy05 27:5956d5e3ff63 58 _ppmRegen->getAllChannels(channels);
edy05 32:c729e6da7f9a 59
edy05 34:5dca557e982f 60 distance1 = _sonic->getDistan1();
edy05 34:5dca557e982f 61 //distance2 = _sonic->getDistan2();
edy05 34:5dca557e982f 62 //pc.printf("%d \n\r", distance1);
edy05 34:5dca557e982f 63 //pc.printf("distance2: %d \n\r", distance2);
edy05 34:5dca557e982f 64 _groundDistance->setProcessValue(distance1);
edy05 27:5956d5e3ff63 65 groundDistancePIDValue = _groundDistance->compute();
edy05 31:5f1737e480f3 66 //groundDistancePIDValue += _bias;
edy05 27:5956d5e3ff63 67
edy05 27:5956d5e3ff63 68
edy05 27:5956d5e3ff63 69 //Update PWM values
edy05 27:5956d5e3ff63 70 regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
edy05 27:5956d5e3ff63 71 if(channels[AUX2] < 1500){
edy05 27:5956d5e3ff63 72 channels[THROTTLE] = 1010;
edy05 13:33024b5880b3 73 }
edy05 34:5dca557e982f 74 if(regulatedThrottle > THROTTLE_LIMIT)
edy05 34:5dca557e982f 75 regulatedThrottle = THROTTLE_LIMIT;
edy05 27:5956d5e3ff63 76 if(regulatedThrottle < 1010)
edy05 27:5956d5e3ff63 77 regulatedThrottle = 1010;
edy05 27:5956d5e3ff63 78
edy05 27:5956d5e3ff63 79
edy05 27:5956d5e3ff63 80 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 81 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 27:5956d5e3ff63 82 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 2:d172c9963f87 83
edy05 27:5956d5e3ff63 84 // Generate regulated PWM
edy05 27:5956d5e3ff63 85 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 86 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 87 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 88 _throttle->pulsewidth_us(regulatedThrottle);
edy05 27:5956d5e3ff63 89 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 90 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 91 //timer.stop();
edy05 27:5956d5e3ff63 92 //pc.printf("Timer: %d \n\r", timer.read_us());
edy05 2:d172c9963f87 93
edy05 2:d172c9963f87 94 }
edy05 27:5956d5e3ff63 95 else {
edy05 27:5956d5e3ff63 96 //distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 97 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 98 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 33:a2f9fea05cb9 99
edy05 27:5956d5e3ff63 100 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 27:5956d5e3ff63 101 // Generate PWM
edy05 27:5956d5e3ff63 102 _ppmRegen->getAllChannels(channels);
edy05 33:a2f9fea05cb9 103 //pc.printf("channel value: %d \n\r", channels[ROLL]);
edy05 33:a2f9fea05cb9 104 //pc.printf("channel value: %d \n\r", channels[PITCH]);
edy05 33:a2f9fea05cb9 105 //pc.printf("channel value: %d \n\r", channels[YAW]);
edy05 33:a2f9fea05cb9 106 //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
edy05 33:a2f9fea05cb9 107 //pc.printf("channel value: %d \n\r", channels[AUX1]);
edy05 33:a2f9fea05cb9 108 //pc.printf("channel value: %d \n\r", channels[AUX2]);
edy05 33:a2f9fea05cb9 109
edy05 27:5956d5e3ff63 110 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 111 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 112 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 113 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 114 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 115 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 116 }
edy05 2:d172c9963f87 117
edy05 2:d172c9963f87 118 }