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?
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 | 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 | } |