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@31:5f1737e480f3, 2018-02-04 (annotated)

Committer:
edy05
Date:
Sun Feb 04 15:21:12 2018 +0000
Branch:
DistanceRegulation
Revision:
31:5f1737e480f3
Parent:
27:5956d5e3ff63
Child:
32:c729e6da7f9a
Better Server respond to ground regulation

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 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 27:5956d5e3ff63 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 27:5956d5e3ff63 31 if(_groundRegulation){
edy05 27:5956d5e3ff63 32 //timer.reset();
edy05 27:5956d5e3ff63 33 //timer.start();
edy05 27:5956d5e3ff63 34
edy05 27:5956d5e3ff63 35 _ppmRegen->getAllChannels(channels);
edy05 27:5956d5e3ff63 36 //PID SETUP
edy05 27:5956d5e3ff63 37 if(_configChanges){
edy05 27:5956d5e3ff63 38 _configChanges = false;
edy05 31:5f1737e480f3 39
edy05 31:5f1737e480f3 40 if(_onlyDistanChanged){
edy05 31:5f1737e480f3 41 _onlyDistanChanged = false;
edy05 31:5f1737e480f3 42 //change only set point
edy05 31:5f1737e480f3 43 pc.printf("Only distance changed \n\r");
edy05 31:5f1737e480f3 44 _groundDistance->setSetPoint(_groundSetPoint);
edy05 31:5f1737e480f3 45 }
edy05 31:5f1737e480f3 46 else{
edy05 31:5f1737e480f3 47 _groundDistance->resetError();
edy05 31:5f1737e480f3 48 //pc.printf("PID tunings changed \n\r");
edy05 31:5f1737e480f3 49 _groundDistance->setTunings(_P, _I, _D);
edy05 31:5f1737e480f3 50 _groundDistance->setSetPoint(_groundSetPoint);
edy05 31:5f1737e480f3 51 _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
edy05 31:5f1737e480f3 52 _groundDistance->setBias(_bias);
edy05 31:5f1737e480f3 53 }
edy05 27:5956d5e3ff63 54 }
edy05 27:5956d5e3ff63 55 distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 56 _groundDistance->setProcessValue(distance);
edy05 27:5956d5e3ff63 57 groundDistancePIDValue = _groundDistance->compute();
edy05 31:5f1737e480f3 58 //groundDistancePIDValue += _bias;
edy05 27:5956d5e3ff63 59
edy05 27:5956d5e3ff63 60
edy05 27:5956d5e3ff63 61 //Update PWM values
edy05 27:5956d5e3ff63 62 regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
edy05 27:5956d5e3ff63 63 if(channels[AUX2] < 1500){
edy05 27:5956d5e3ff63 64 channels[THROTTLE] = 1010;
edy05 13:33024b5880b3 65 }
edy05 27:5956d5e3ff63 66 if(regulatedThrottle > 2000)
edy05 27:5956d5e3ff63 67 regulatedThrottle = 2000;
edy05 27:5956d5e3ff63 68 if(regulatedThrottle < 1010)
edy05 27:5956d5e3ff63 69 regulatedThrottle = 1010;
edy05 27:5956d5e3ff63 70
edy05 27:5956d5e3ff63 71
edy05 27:5956d5e3ff63 72 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 73 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 74 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 27:5956d5e3ff63 75 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 2:d172c9963f87 76
edy05 27:5956d5e3ff63 77 // Generate regulated PWM
edy05 27:5956d5e3ff63 78 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 79 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 80 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 81 _throttle->pulsewidth_us(regulatedThrottle);
edy05 27:5956d5e3ff63 82 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 83 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 84 //timer.stop();
edy05 27:5956d5e3ff63 85 //pc.printf("Timer: %d \n\r", timer.read_us());
edy05 2:d172c9963f87 86
edy05 2:d172c9963f87 87 }
edy05 27:5956d5e3ff63 88 else {
edy05 27:5956d5e3ff63 89 //distance = _sonic->getDistan();
edy05 27:5956d5e3ff63 90 //pc.printf("Sonic distance: %d \n\r", distance);
edy05 27:5956d5e3ff63 91 //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
edy05 27:5956d5e3ff63 92 //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
edy05 27:5956d5e3ff63 93 //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
edy05 27:5956d5e3ff63 94 // Generate PWM
edy05 27:5956d5e3ff63 95 _ppmRegen->getAllChannels(channels);
edy05 27:5956d5e3ff63 96 _roll->pulsewidth_us( channels[ROLL]);
edy05 27:5956d5e3ff63 97 _pitch->pulsewidth_us( channels[PITCH]);
edy05 27:5956d5e3ff63 98 _yaw->pulsewidth_us( channels[YAW]);
edy05 27:5956d5e3ff63 99 _throttle->pulsewidth_us(channels[THROTTLE]);
edy05 27:5956d5e3ff63 100 _aux1->pulsewidth_us( channels[AUX1]);
edy05 27:5956d5e3ff63 101 _aux2->pulsewidth_us( channels[AUX2]);
edy05 27:5956d5e3ff63 102 }
edy05 2:d172c9963f87 103
edy05 2:d172c9963f87 104 }