Eduard Medla
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
Diff: distanceRegulation.h
- Branch:
- DistanceRegulation
- Revision:
- 27:5956d5e3ff63
- Parent:
- 26:11539036f0fb
- Child:
- 30:4d1b3926a3cc
- Child:
- 31:5f1737e480f3
--- a/distanceRegulation.h Fri Dec 01 11:11:23 2017 +0000
+++ b/distanceRegulation.h Sat Jan 20 11:44:46 2018 +0000
@@ -3,21 +3,26 @@
#include "hardware.h"
#include "definitions.h"
+void distanceRegulationTask(void const *args);
-//RtosTimer *_groudRegulationUpdateTimer;
+RtosTimer *_groudRegulationUpdateTimer;
-/*
void distanceRegulationThread(void const *args){
+ pc.printf("Flight controller thread started\r\n");
+
_groudRegulationUpdateTimer = new RtosTimer(distanceRegulationTask, osTimerPeriodic, (void *)0);
- int rate = 2;
+ int rate = 5;
_groudRegulationUpdateTimer->start(rate);
Thread::wait(osWaitForever);
}
-*/
-void distanceRegulationTask(){
+
+void distanceRegulationTask(void const *args){
+
+ //pc.printf("Flight controller task started\r\n");
+
uint16_t channels[CHANNELS];
int distance=0;
float groundDistancePIDValue;
@@ -33,74 +38,69 @@
//Timer timer;
- while(1){
- if(_groundRegulation){
- //timer.reset();
- //timer.start();
-
- _ppmRegen->getAllChannels(channels);
- //PID SETUP
- if(_configChanges){
- _configChanges = false;
- _groundDistance->resetError();
- //pc.printf("PID tunings changed \n\r");
- _groundDistance->setTunings(_P, _I, _D);
- _groundDistance->setSetPoint(_groundSetPoint);
- _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
- _groundDistance->setBias(_bias);
- }
- distance = _sonic->getDistan();
- _groundDistance->setProcessValue(distance);
- groundDistancePIDValue = _groundDistance->compute();
- groundDistancePIDValue += _bias;
-
-
- //Update PWM values
- regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
- if(channels[AUX2] < 1500){
- channels[THROTTLE] = 1010;
- }
- if(regulatedThrottle > 2000)
- regulatedThrottle = 2000;
- if(regulatedThrottle < 1010)
- regulatedThrottle = 1010;
-
-
- pc.printf("Sonic distance: %d \n\r", distance);
- //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
- //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
- //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
-
- // Generate regulated PWM
- _roll->pulsewidth_us( channels[ROLL]);
- _pitch->pulsewidth_us( channels[PITCH]);
- _yaw->pulsewidth_us( channels[YAW]);
- _throttle->pulsewidth_us(regulatedThrottle);
- _aux1->pulsewidth_us( channels[AUX1]);
- _aux2->pulsewidth_us( channels[AUX2]);
- //timer.stop();
- //pc.printf("Timer: %d \n\r", timer.read_us());
-
+
+ if(_groundRegulation){
+ //timer.reset();
+ //timer.start();
+
+ _ppmRegen->getAllChannels(channels);
+ //PID SETUP
+ if(_configChanges){
+ _configChanges = false;
+ _groundDistance->resetError();
+ //pc.printf("PID tunings changed \n\r");
+ _groundDistance->setTunings(_P, _I, _D);
+ _groundDistance->setSetPoint(_groundSetPoint);
+ _groundDistance->setOutputLimits(_groundPidMinOutput, _groundPidMaxOutput);
+ _groundDistance->setBias(_bias);
+ }
+ distance = _sonic->getDistan();
+ _groundDistance->setProcessValue(distance);
+ groundDistancePIDValue = _groundDistance->compute();
+ groundDistancePIDValue += _bias;
+
+
+ //Update PWM values
+ regulatedThrottle = channels[THROTTLE] + groundDistancePIDValue;
+ if(channels[AUX2] < 1500){
+ channels[THROTTLE] = 1010;
}
- else {
-
- //pc.printf("Sonic distance: %d \n\r", distance);
- //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
- //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
- //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
- // Generate PWM
- _ppmRegen->getAllChannels(channels);
- _roll->pulsewidth_us( channels[ROLL]);
- _pitch->pulsewidth_us( channels[PITCH]);
- _yaw->pulsewidth_us( channels[YAW]);
- _throttle->pulsewidth_us(channels[THROTTLE]);
- _aux1->pulsewidth_us( channels[AUX1]);
- _aux2->pulsewidth_us( channels[AUX2]);
- }
+ if(regulatedThrottle > 2000)
+ regulatedThrottle = 2000;
+ if(regulatedThrottle < 1010)
+ regulatedThrottle = 1010;
+
+
+ //pc.printf("Sonic distance: %d \n\r", distance);
+ //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
+ //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
+ //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
- Thread::wait(2);
-
+ // Generate regulated PWM
+ _roll->pulsewidth_us( channels[ROLL]);
+ _pitch->pulsewidth_us( channels[PITCH]);
+ _yaw->pulsewidth_us( channels[YAW]);
+ _throttle->pulsewidth_us(regulatedThrottle);
+ _aux1->pulsewidth_us( channels[AUX1]);
+ _aux2->pulsewidth_us( channels[AUX2]);
+ //timer.stop();
+ //pc.printf("Timer: %d \n\r", timer.read_us());
}
+ else {
+ //distance = _sonic->getDistan();
+ //pc.printf("Sonic distance: %d \n\r", distance);
+ //pc.printf("channel throttle pid value: %f \n\r", groundDistancePIDValue);
+ //pc.printf("channel throttle original value: %d \n\r", channels[THROTTLE]);
+ //pc.printf("channel throttle final value: %d \n\r", regulatedThrottle);
+ // Generate PWM
+ _ppmRegen->getAllChannels(channels);
+ _roll->pulsewidth_us( channels[ROLL]);
+ _pitch->pulsewidth_us( channels[PITCH]);
+ _yaw->pulsewidth_us( channels[YAW]);
+ _throttle->pulsewidth_us(channels[THROTTLE]);
+ _aux1->pulsewidth_us( channels[AUX1]);
+ _aux2->pulsewidth_us( channels[AUX2]);
+ }
}
\ No newline at end of file