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@35:b09e19c855f6, 2018-03-03 (annotated)

Committer:
edy05
Date:
Sat Mar 03 15:38:10 2018 +0000
Branch:
DistanceRegulation
Revision:
35:b09e19c855f6
Parent:
34:5dca557e982f
Child:
36:ed8b7b7b6cfa
not working - pid result problem

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 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 35:b09e19c855f6 67 pc.printf("%d \n\r", distance1);
edy05 34:5dca557e982f 68 //pc.printf("distance2: %d \n\r", distance2);
edy05 34:5dca557e982f 69 _groundDistance->setProcessValue(distance1);
edy05 27:5956d5e3ff63 70 groundDistancePIDValue = _groundDistance->compute();
edy05 35:b09e19c855f6 71 pc.printf("pid value %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 72
edy05 27:5956d5e3ff63 73 //Update PWM values
edy05 35:b09e19c855f6 74 regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
edy05 27:5956d5e3ff63 75 if(channels[AUX2] < 1500){
edy05 27:5956d5e3ff63 76 channels[THROTTLE] = 1010;
edy05 13:33024b5880b3 77 }
edy05 34:5dca557e982f 78 if(regulatedThrottle > THROTTLE_LIMIT)
edy05 34:5dca557e982f 79 regulatedThrottle = THROTTLE_LIMIT;
edy05 27:5956d5e3ff63 80 if(regulatedThrottle < 1010)
edy05 27:5956d5e3ff63 81 regulatedThrottle = 1010;
edy05 27:5956d5e3ff63 82
edy05 27:5956d5e3ff63 83
edy05 35:b09e19c855f6 84 pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 35:b09e19c855f6 85 pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 35:b09e19c855f6 86 pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 2:d172c9963f87 87
edy05 27:5956d5e3ff63 88 // Generate regulated PWM
edy05 27:5956d5e3ff63 89 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 90 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 91 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 92 _throttle->pulsewidth_us(regulatedThrottle);
edy05 27:5956d5e3ff63 93 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 94 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 95 //timer.stop();
edy05 27:5956d5e3ff63 96 //pc.printf("Timer: %d \n\r", timer.read_us());
edy05 2:d172c9963f87 97
edy05 2:d172c9963f87 98 }
edy05 27:5956d5e3ff63 99 else {
edy05 27:5956d5e3ff63 100 //distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 101 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 102 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 33:a2f9fea05cb9 103
edy05 27:5956d5e3ff63 104 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 27:5956d5e3ff63 105 // Generate PWM
edy05 27:5956d5e3ff63 106 _ppmRegen->getAllChannels(channels);
edy05 33:a2f9fea05cb9 107 //pc.printf("channel value: %d \n\r", channels[ROLL]);
edy05 33:a2f9fea05cb9 108 //pc.printf("channel value: %d \n\r", channels[PITCH]);
edy05 33:a2f9fea05cb9 109 //pc.printf("channel value: %d \n\r", channels[YAW]);
edy05 33:a2f9fea05cb9 110 //pc.printf("channel value: %d \n\r", channels[THROTTLE]);
edy05 33:a2f9fea05cb9 111 //pc.printf("channel value: %d \n\r", channels[AUX1]);
edy05 33:a2f9fea05cb9 112 //pc.printf("channel value: %d \n\r", channels[AUX2]);
edy05 33:a2f9fea05cb9 113
edy05 27:5956d5e3ff63 114 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 115 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 116 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 117 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 118 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 119 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 120 }
edy05 2:d172c9963f87 121
edy05 2:d172c9963f87 122 }