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@36:ed8b7b7b6cfa, 2018-03-04 (annotated)

Committer:
edy05
Date:
Sun Mar 04 15:17:38 2018 +0000
Branch:
DistanceRegulation
Revision:
36:ed8b7b7b6cfa
Parent:
35:b09e19c855f6
Child:
38:801fee0330e2
stable, PID fixed rate

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 36:ed8b7b7b6cfa 27 float 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 35:b09e19c855f6 42 pc.printf("P %f \n\r", _P);
edy05 35:b09e19c855f6 43 pc.printf("I %f \n\r", _I);
edy05 35:b09e19c855f6 44 pc.printf("D %f \n\r", _D);
edy05 35:b09e19c855f6 45 pc.printf("setpoint %f \n\r", _groundSetPoint);
edy05 35:b09e19c855f6 46 pc.printf("bias %f \n\r", _bias);
edy05 32:c729e6da7f9a 47 _groundDistance->resetError();
edy05 32:c729e6da7f9a 48 //pc.printf("PID tunings changed \n\r");
edy05 32:c729e6da7f9a 49 _groundDistance->setTunings(_P, _I, _D);
edy05 32:c729e6da7f9a 50 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 51 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 35:b09e19c855f6 52 //_groundDistance->setBias(_bias);
edy05 32:c729e6da7f9a 53 }
edy05 32:c729e6da7f9a 54 }
edy05 32:c729e6da7f9a 55
edy05 34:5dca557e982f 56 //distance1 = _sonic->getDistan1();
edy05 34:5dca557e982f 57 //pc.printf("%d \n\r", distance1);
edy05 34:5dca557e982f 58
edy05 27:5956d5e3ff63 59 if(_groundRegulation){
edy05 27:5956d5e3ff63 60 //timer.reset();
edy05 27:5956d5e3ff63 61 //timer.start();
edy05 27:5956d5e3ff63 62
edy05 27:5956d5e3ff63 63 _ppmRegen->getAllChannels(channels);
edy05 32:c729e6da7f9a 64
edy05 34:5dca557e982f 65 distance1 = _sonic->getDistan1();
edy05 34:5dca557e982f 66 //distance2 = _sonic->getDistan2();
edy05 36:ed8b7b7b6cfa 67 //pc.printf("%f \n\r", distance1);
edy05 36:ed8b7b7b6cfa 68 //float p = _groundDistance->getPParam();
edy05 36:ed8b7b7b6cfa 69 //float i = _groundDistance->getIParam();
edy05 36:ed8b7b7b6cfa 70 //float d = _groundDistance->getDParam();
edy05 36:ed8b7b7b6cfa 71 //float max = _groundDistance->getInMax();
edy05 36:ed8b7b7b6cfa 72 //pc.printf("%f \n\r", max);
edy05 36:ed8b7b7b6cfa 73 //pc.printf("%f \n\r", p);
edy05 36:ed8b7b7b6cfa 74 //pc.printf("%f \n\r", i);
edy05 36:ed8b7b7b6cfa 75 //pc.printf("%f \n\r", d);
edy05 34:5dca557e982f 76 //pc.printf("distance2: %d \n\r", distance2);
edy05 34:5dca557e982f 77 _groundDistance->setProcessValue(distance1);
edy05 27:5956d5e3ff63 78 groundDistancePIDValue = _groundDistance->compute();
edy05 36:ed8b7b7b6cfa 79 //pc.printf("pid value %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 80
edy05 27:5956d5e3ff63 81 //Update PWM values
edy05 35:b09e19c855f6 82 regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
edy05 27:5956d5e3ff63 83 if(channels[AUX2] < 1500){
edy05 27:5956d5e3ff63 84 channels[THROTTLE] = 1010;
edy05 13:33024b5880b3 85 }
edy05 34:5dca557e982f 86 if(regulatedThrottle > THROTTLE_LIMIT)
edy05 34:5dca557e982f 87 regulatedThrottle = THROTTLE_LIMIT;
edy05 27:5956d5e3ff63 88 if(regulatedThrottle < 1010)
edy05 27:5956d5e3ff63 89 regulatedThrottle = 1010;
edy05 27:5956d5e3ff63 90
edy05 27:5956d5e3ff63 91
edy05 36:ed8b7b7b6cfa 92 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 36:ed8b7b7b6cfa 93 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 36:ed8b7b7b6cfa 94 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 2:d172c9963f87 95
edy05 27:5956d5e3ff63 96 // Generate regulated PWM
edy05 27:5956d5e3ff63 97 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 98 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 99 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 100 _throttle->pulsewidth_us(regulatedThrottle);
edy05 27:5956d5e3ff63 101 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 102 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 103 //timer.stop();
edy05 27:5956d5e3ff63 104 //pc.printf("Timer: %d \n\r", timer.read_us());
edy05 2:d172c9963f87 105
edy05 2:d172c9963f87 106 }
edy05 27:5956d5e3ff63 107 else {
edy05 27:5956d5e3ff63 108 //distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 109 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 110 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 33:a2f9fea05cb9 111
edy05 27:5956d5e3ff63 112 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 27:5956d5e3ff63 113 // Generate PWM
edy05 27:5956d5e3ff63 114 _ppmRegen->getAllChannels(channels);
edy05 33:a2f9fea05cb9 115 //pc.printf("channel value: %d \n\r", channels[ROLL]);
edy05 33:a2f9fea05cb9 116 //pc.printf("channel value: %d \n\r", channels[PITCH]);
edy05 33:a2f9fea05cb9 117 //pc.printf("channel value: %d \n\r", channels[YAW]);
edy05 33:a2f9fea05cb9 118 //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
edy05 33:a2f9fea05cb9 119 //pc.printf("channel value: %d \n\r", channels[AUX1]);
edy05 33:a2f9fea05cb9 120 //pc.printf("channel value: %d \n\r", channels[AUX2]);
edy05 33:a2f9fea05cb9 121
edy05 27:5956d5e3ff63 122 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 123 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 124 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 125 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 126 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 127 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 128 }
edy05 2:d172c9963f87 129
edy05 2:d172c9963f87 130 }