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@40:0aa1cefe80ab, 2018-05-15 (annotated)

Committer:
edy05
Date:
Tue May 15 10:34:35 2018 +0000
Branch:
DistanceRegulation
Revision:
40:0aa1cefe80ab
Parent:
39:93d8aa47f4ce
Child:
41:5fe200d20022
some updates; sonics added

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