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