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@39:93d8aa47f4ce, 2018-03-25 (annotated)
- Committer:
- edy05
- Date:
- Sun Mar 25 12:57:04 2018 +0000
- Branch:
- DistanceRegulation
- Revision:
- 39:93d8aa47f4ce
- Parent:
- 38:801fee0330e2
- Child:
- 40:0aa1cefe80ab
stabilization tested and working
Who changed what in which revision?
User | Revision | Line number | New 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 | 34:5dca557e982f | 56 | //pc.printf("%d \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 | 38:801fee0330e2 | 96 | if(_goAhead == true){ |
edy05 | 38:801fee0330e2 | 97 | _roll->pulsewidth_us( REGULATED_ROLL); |
edy05 | 38:801fee0330e2 | 98 | } |
edy05 | 38:801fee0330e2 | 99 | else{ |
edy05 | 38:801fee0330e2 | 100 | _roll->pulsewidth_us( channels[ROLL]); |
edy05 | 38:801fee0330e2 | 101 | } |
edy05 | 38:801fee0330e2 | 102 | |
edy05 | 27:5956d5e3ff63 | 103 | _pitch->pulsewidth_us( channels[PITCH]); |
edy05 | 27:5956d5e3ff63 | 104 | _yaw->pulsewidth_us( channels[YAW]); |
edy05 | 27:5956d5e3ff63 | 105 | _throttle->pulsewidth_us(regulatedThrottle); |
edy05 | 27:5956d5e3ff63 | 106 | _aux1->pulsewidth_us( channels[AUX1]); |
edy05 | 27:5956d5e3ff63 | 107 | _aux2->pulsewidth_us( channels[AUX2]); |
edy05 | 27:5956d5e3ff63 | 108 | //timer.stop(); |
edy05 | 27:5956d5e3ff63 | 109 | //pc.printf("Timer: %d \n\r", timer.read_us()); |
edy05 | 2:d172c9963f87 | 110 | |
edy05 | 2:d172c9963f87 | 111 | } |
edy05 | 27:5956d5e3ff63 | 112 | else { |
edy05 | 27:5956d5e3ff63 | 113 | //distance = _sonic->getDistan(); |
edy05 | 27:5956d5e3ff63 | 114 | //pc.printf("Sonic distance: %d \n\r", distance); |
edy05 | 27:5956d5e3ff63 | 115 | //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); |
edy05 | 33:a2f9fea05cb9 | 116 | |
edy05 | 27:5956d5e3ff63 | 117 | //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle); |
edy05 | 27:5956d5e3ff63 | 118 | // Generate PWM |
edy05 | 27:5956d5e3ff63 | 119 | _ppmRegen->getAllChannels(channels); |
edy05 | 33:a2f9fea05cb9 | 120 | //pc.printf("channel value: %d \n\r", channels[ROLL]); |
edy05 | 33:a2f9fea05cb9 | 121 | //pc.printf("channel value: %d \n\r", channels[PITCH]); |
edy05 | 33:a2f9fea05cb9 | 122 | //pc.printf("channel value: %d \n\r", channels[YAW]); |
edy05 | 33:a2f9fea05cb9 | 123 | //pc.printf("channel value: %d \n\r", channels[THROTTLE]); |
edy05 | 33:a2f9fea05cb9 | 124 | //pc.printf("channel value: %d \n\r", channels[AUX1]); |
edy05 | 33:a2f9fea05cb9 | 125 | //pc.printf("channel value: %d \n\r", channels[AUX2]); |
edy05 | 33:a2f9fea05cb9 | 126 | |
edy05 | 27:5956d5e3ff63 | 127 | _roll->pulsewidth_us( channels[ROLL]); |
edy05 | 27:5956d5e3ff63 | 128 | _pitch->pulsewidth_us( channels[PITCH]); |
edy05 | 27:5956d5e3ff63 | 129 | _yaw->pulsewidth_us( channels[YAW]); |
edy05 | 27:5956d5e3ff63 | 130 | _throttle->pulsewidth_us(channels[THROTTLE]); |
edy05 | 27:5956d5e3ff63 | 131 | _aux1->pulsewidth_us( channels[AUX1]); |
edy05 | 27:5956d5e3ff63 | 132 | _aux2->pulsewidth_us( channels[AUX2]); |
edy05 | 27:5956d5e3ff63 | 133 | } |
edy05 | 2:d172c9963f87 | 134 | |
edy05 | 2:d172c9963f87 | 135 | } |