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@38:801fee0330e2, 2018-03-23 (annotated)

Committer:
edy05
Date:
Fri Mar 23 14:45:52 2018 +0000
Branch:
DistanceRegulation
Revision:
38:801fee0330e2
Parent:
36:ed8b7b7b6cfa
Child:
39:93d8aa47f4ce
goAhead added, not tested

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