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@32:c729e6da7f9a, 2018-02-17 (annotated)
- Committer:
- edy05
- Date:
- Sat Feb 17 15:48:49 2018 +0000
- Branch:
- DistanceRegulation
- Revision:
- 32:c729e6da7f9a
- Parent:
- 31:5f1737e480f3
- Child:
- 33:a2f9fea05cb9
not 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 | 32:c729e6da7f9a | 24 | pc.printf("Flight controller task started\r\n"); |
edy05 | 27:5956d5e3ff63 | 25 | |
edy05 | 2:d172c9963f87 | 26 | uint16_t channels[CHANNELS]; |
edy05 | 2:d172c9963f87 | 27 | int distance=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 | 32:c729e6da7f9a | 41 | _groundDistance->resetError(); |
edy05 | 32:c729e6da7f9a | 42 | //pc.printf("PID tunings changed \n\r"); |
edy05 | 32:c729e6da7f9a | 43 | _groundDistance->setTunings(_P, _I, _D); |
edy05 | 32:c729e6da7f9a | 44 | _groundDistance->setSetPoint(_groundSetPoint); |
edy05 | 32:c729e6da7f9a | 45 | _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput); |
edy05 | 32:c729e6da7f9a | 46 | _groundDistance->setBias(_bias); |
edy05 | 32:c729e6da7f9a | 47 | } |
edy05 | 32:c729e6da7f9a | 48 | } |
edy05 | 32:c729e6da7f9a | 49 | |
edy05 | 27:5956d5e3ff63 | 50 | if(_groundRegulation){ |
edy05 | 27:5956d5e3ff63 | 51 | //timer.reset(); |
edy05 | 27:5956d5e3ff63 | 52 | //timer.start(); |
edy05 | 27:5956d5e3ff63 | 53 | |
edy05 | 27:5956d5e3ff63 | 54 | _ppmRegen->getAllChannels(channels); |
edy05 | 32:c729e6da7f9a | 55 | |
edy05 | 27:5956d5e3ff63 | 56 | distance = _sonic->getDistan(); |
edy05 | 32:c729e6da7f9a | 57 | pc.printf("distance: %d \n\r", distance); |
edy05 | 27:5956d5e3ff63 | 58 | _groundDistance->setProcessValue(distance); |
edy05 | 27:5956d5e3ff63 | 59 | groundDistancePIDValue = _groundDistance->compute(); |
edy05 | 31:5f1737e480f3 | 60 | //groundDistancePIDValue += _bias; |
edy05 | 27:5956d5e3ff63 | 61 | |
edy05 | 27:5956d5e3ff63 | 62 | |
edy05 | 27:5956d5e3ff63 | 63 | //Update PWM values |
edy05 | 27:5956d5e3ff63 | 64 | regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue; |
edy05 | 27:5956d5e3ff63 | 65 | if(channels[AUX2] < 1500){ |
edy05 | 27:5956d5e3ff63 | 66 | channels[THROTTLE] = 1010; |
edy05 | 13:33024b5880b3 | 67 | } |
edy05 | 27:5956d5e3ff63 | 68 | if(regulatedThrottle > 2000) |
edy05 | 27:5956d5e3ff63 | 69 | regulatedThrottle = 2000; |
edy05 | 27:5956d5e3ff63 | 70 | if(regulatedThrottle < 1010) |
edy05 | 27:5956d5e3ff63 | 71 | regulatedThrottle = 1010; |
edy05 | 27:5956d5e3ff63 | 72 | |
edy05 | 27:5956d5e3ff63 | 73 | |
edy05 | 27:5956d5e3ff63 | 74 | //pc.printf("Sonic distance: %d \n\r", distance); |
edy05 | 27:5956d5e3ff63 | 75 | //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); |
edy05 | 27:5956d5e3ff63 | 76 | //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]); |
edy05 | 27:5956d5e3ff63 | 77 | //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle); |
edy05 | 2:d172c9963f87 | 78 | |
edy05 | 27:5956d5e3ff63 | 79 | // Generate regulated PWM |
edy05 | 27:5956d5e3ff63 | 80 | _roll->pulsewidth_us( channels[ROLL]); |
edy05 | 27:5956d5e3ff63 | 81 | _pitch->pulsewidth_us( channels[PITCH]); |
edy05 | 27:5956d5e3ff63 | 82 | _yaw->pulsewidth_us( channels[YAW]); |
edy05 | 27:5956d5e3ff63 | 83 | _throttle->pulsewidth_us(regulatedThrottle); |
edy05 | 27:5956d5e3ff63 | 84 | _aux1->pulsewidth_us( channels[AUX1]); |
edy05 | 27:5956d5e3ff63 | 85 | _aux2->pulsewidth_us( channels[AUX2]); |
edy05 | 27:5956d5e3ff63 | 86 | //timer.stop(); |
edy05 | 27:5956d5e3ff63 | 87 | //pc.printf("Timer: %d \n\r", timer.read_us()); |
edy05 | 2:d172c9963f87 | 88 | |
edy05 | 2:d172c9963f87 | 89 | } |
edy05 | 27:5956d5e3ff63 | 90 | else { |
edy05 | 27:5956d5e3ff63 | 91 | //distance = _sonic->getDistan(); |
edy05 | 27:5956d5e3ff63 | 92 | //pc.printf("Sonic distance: %d \n\r", distance); |
edy05 | 27:5956d5e3ff63 | 93 | //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue); |
edy05 | 32:c729e6da7f9a | 94 | pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]); |
edy05 | 27:5956d5e3ff63 | 95 | //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle); |
edy05 | 27:5956d5e3ff63 | 96 | // Generate PWM |
edy05 | 27:5956d5e3ff63 | 97 | _ppmRegen->getAllChannels(channels); |
edy05 | 27:5956d5e3ff63 | 98 | _roll->pulsewidth_us( channels[ROLL]); |
edy05 | 27:5956d5e3ff63 | 99 | _pitch->pulsewidth_us( channels[PITCH]); |
edy05 | 27:5956d5e3ff63 | 100 | _yaw->pulsewidth_us( channels[YAW]); |
edy05 | 27:5956d5e3ff63 | 101 | _throttle->pulsewidth_us(channels[THROTTLE]); |
edy05 | 27:5956d5e3ff63 | 102 | _aux1->pulsewidth_us( channels[AUX1]); |
edy05 | 27:5956d5e3ff63 | 103 | _aux2->pulsewidth_us( channels[AUX2]); |
edy05 | 27:5956d5e3ff63 | 104 | } |
edy05 | 2:d172c9963f87 | 105 | |
edy05 | 2:d172c9963f87 | 106 | } |