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@41:5fe200d20022, 2018-05-22 (annotated)

Committer:
edy05
Date:
Tue May 22 19:43:09 2018 +0000
Branch:
DistanceRegulation
Revision:
41:5fe200d20022
Parent:
40:0aa1cefe80ab 
final updates

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 41:5fe200d20022 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 41:5fe200d20022 18 osThreadId id;
edy05 41:5fe200d20022 19 id = Thread::gettid();
edy05 41:5fe200d20022 20 pc.printf("regulation gettid 0x%08X \n\r", id);
edy05 41:5fe200d20022 21
edy05 26:11539036f0fb 22 Thread::wait(osWaitForever);
edy05 26:11539036f0fb 23
edy05 26:11539036f0fb 24 }
edy05 27:5956d5e3ff63 25
edy05 27:5956d5e3ff63 26 void distanceRegulationTask(void const *args){
edy05 27:5956d5e3ff63 27
edy05 41:5fe200d20022 28 //osThreadId id;
edy05 41:5fe200d20022 29 //id = Thread::gettid();
edy05 41:5fe200d20022 30 //pc.printf("regulation task gettid 0x%08X \n\r", id);
edy05 41:5fe200d20022 31
edy05 2:d172c9963f87 32 uint16_t channels[CHANNELS];
edy05 36:ed8b7b7b6cfa 33 float distance1=0;
edy05 18:e0c5ab76f337 34 float groundDistancePIDValue;
edy05 19:9ceec179b140 35 int regulatedThrottle;
edy05 2:d172c9963f87 36
edy05 32:c729e6da7f9a 37 //PID SETUP
edy05 32:c729e6da7f9a 38 if(_configChanges){
edy05 32:c729e6da7f9a 39 _configChanges = false;
edy05 32:c729e6da7f9a 40 if(_onlyDistanChanged){
edy05 32:c729e6da7f9a 41 _onlyDistanChanged = false;
edy05 32:c729e6da7f9a 42 //change only set point
edy05 32:c729e6da7f9a 43 pc.printf("Only distance changed \n\r");
edy05 32:c729e6da7f9a 44 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 45 }
edy05 32:c729e6da7f9a 46 else{
edy05 35:b09e19c855f6 47 pc.printf("P %f \n\r", _P);
edy05 35:b09e19c855f6 48 pc.printf("I %f \n\r", _I);
edy05 35:b09e19c855f6 49 pc.printf("D %f \n\r", _D);
edy05 35:b09e19c855f6 50 pc.printf("setpoint %f \n\r", _groundSetPoint);
edy05 35:b09e19c855f6 51 pc.printf("bias %f \n\r", _bias);
edy05 32:c729e6da7f9a 52 _groundDistance->resetError();
edy05 32:c729e6da7f9a 53 _groundDistance->setTunings(_P, _I, _D);
edy05 32:c729e6da7f9a 54 _groundDistance->setSetPoint(_groundSetPoint);
edy05 32:c729e6da7f9a 55 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 32:c729e6da7f9a 56 }
edy05 32:c729e6da7f9a 57 }
edy05 32:c729e6da7f9a 58
edy05 34:5dca557e982f 59 //distance1 = _sonic->getDistan1();
edy05 41:5fe200d20022 60 //pc.printf(" distance1 %f \n\r", distance1);
edy05 34:5dca557e982f 61
edy05 27:5956d5e3ff63 62 if(_groundRegulation){
edy05 41:5fe200d20022 63 distance1 = _sonic->getDistan1();
edy05 27:5956d5e3ff63 64 _ppmRegen->getAllChannels(channels);
edy05 41:5fe200d20022 65 _groundDistance->setProcessValue(distance1);
edy05 32:c729e6da7f9a 66
edy05 41:5fe200d20022 67 //landing
edy05 41:5fe200d20022 68 if(channels[AUX1] >= 1300){
edy05 41:5fe200d20022 69 //pc.printf("landing \n\r");
edy05 41:5fe200d20022 70 if(distance1 <= LANDING_HEIGHT)
edy05 41:5fe200d20022 71 channels[AUX2] = 1000;
edy05 41:5fe200d20022 72 _groundDistance->setLandingPoint((float)LANDING_HEIGHT);
edy05 41:5fe200d20022 73 }
edy05 41:5fe200d20022 74
edy05 27:5956d5e3ff63 75 groundDistancePIDValue = _groundDistance->compute();
edy05 27:5956d5e3ff63 76
edy05 27:5956d5e3ff63 77 //Update PWM values
edy05 35:b09e19c855f6 78 regulatedThrottle = channels[THROTTLE] + _bias + groundDistancePIDValue;
edy05 27:5956d5e3ff63 79 if(channels[AUX2] < 1500){
edy05 27:5956d5e3ff63 80 channels[THROTTLE] = 1010;
edy05 13:33024b5880b3 81 }
edy05 41:5fe200d20022 82
edy05 41:5fe200d20022 83 // limits of throttle
edy05 34:5dca557e982f 84 if(regulatedThrottle > THROTTLE_LIMIT)
edy05 34:5dca557e982f 85 regulatedThrottle = THROTTLE_LIMIT;
edy05 27:5956d5e3ff63 86 if(regulatedThrottle < 1010)
edy05 27:5956d5e3ff63 87 regulatedThrottle = 1010;
edy05 27:5956d5e3ff63 88
edy05 41:5fe200d20022 89 // goAhead and back
edy05 41:5fe200d20022 90 if(_groundDistance->quadStabilized() && _goAhead){
edy05 41:5fe200d20022 91 //pc.printf("going agead \n\r");
edy05 41:5fe200d20022 92 if(_frontDistance <= 40){
edy05 41:5fe200d20022 93 _frontWall = true;
edy05 41:5fe200d20022 94 _backWall = false;
edy05 41:5fe200d20022 95 }
edy05 41:5fe200d20022 96 if(_backDistance <= 40){
edy05 41:5fe200d20022 97 _frontWall = false;
edy05 41:5fe200d20022 98 _backWall = true;
edy05 41:5fe200d20022 99 }
edy05 41:5fe200d20022 100 if(_frontWall){
edy05 41:5fe200d20022 101 //pc.printf(" going backward \n\r");
edy05 41:5fe200d20022 102 _pitch->pulsewidth_us(channels[PITCH] - PITCH_MOVE_BACKWARD);
edy05 41:5fe200d20022 103 }
edy05 41:5fe200d20022 104 if(_backWall){
edy05 41:5fe200d20022 105 //pc.printf(" going forward \n\r");
edy05 41:5fe200d20022 106 _pitch->pulsewidth_us(channels[PITCH] + PITCH_MOVE_FORWARD);
edy05 41:5fe200d20022 107 }
edy05 41:5fe200d20022 108
edy05 41:5fe200d20022 109
edy05 41:5fe200d20022 110 //if(_frontDistance > 40 && _backDistance > 40)
edy05 41:5fe200d20022 111 // _pitch->pulsewidth_us(channels[PITCH]);
edy05 41:5fe200d20022 112
edy05 38:801fee0330e2 113 }
edy05 38:801fee0330e2 114 else{
edy05 41:5fe200d20022 115 _pitch->pulsewidth_us(channels[PITCH]);
edy05 38:801fee0330e2 116 }
edy05 40:0aa1cefe80ab 117
edy05 40:0aa1cefe80ab 118 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 119 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 120 _throttle->pulsewidth_us(regulatedThrottle);
edy05 27:5956d5e3ff63 121 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 122 _aux2->pulsewidth_us( channels[AUX2]);
edy05 2:d172c9963f87 123
edy05 2:d172c9963f87 124 }
edy05 41:5fe200d20022 125
edy05 27:5956d5e3ff63 126 else {
edy05 41:5fe200d20022 127 if(_groundDistance->quadStabilized())
edy05 41:5fe200d20022 128 _groundDistance->setNotStabelized();
edy05 33:a2f9fea05cb9 129
edy05 41:5fe200d20022 130
edy05 27:5956d5e3ff63 131 // Generate PWM
edy05 27:5956d5e3ff63 132 _ppmRegen->getAllChannels(channels);
edy05 33:a2f9fea05cb9 133
edy05 27:5956d5e3ff63 134 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 135 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 136 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 137 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 138 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 139 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 140 }
edy05 2:d172c9963f87 141
edy05 41:5fe200d20022 142
edy05 2:d172c9963f87 143 }