UavHeightControl.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00029
00030
00035
00036
00037 #ifndef UAV_HEIGHTCONTROL_TASK_H_
00038 #define UAV_HEIGHTCONTROL_TASK_H_
00039
00040 #include <Task.h>
00041 #include <Resource.h>
00042 #include <Uav.h>
00043 #include <CommonOptions.h>
00044 #include <Keyboard.h>
00045
00046 #include <iostream>
00047
00048 #ifndef MIP_PLATFORM_ARM
00049 #include <DraWin.h>
00050 #endif
00051
00052 #define M2INCH 0.0254
00053
00054 namespace MipTasks{
00055
00057
00058
00062 enum UavHeightControlStates{
00063 UAV_HEIGHT_CONTROL_FIRST_RUN,
00064 UAV_HEIGHT_CONTROL_NORMAL_RUN_BKS,
00065 UAV_HEIGHT_CONTROL_NORMAL_RUN_PID,
00066 UAV_HEIGHT_CONTROL_ONOFF_MOTORS,
00067 UAV_HEIGHT_CONTROL_STATES_NUM
00068 };
00069
00072 static const char* UavHeightControlStatesNames[UAV_HEIGHT_CONTROL_STATES_NUM] = {
00073 "FirstRun",
00074 "NormalRun_bks",
00075 "NormalRun_pid",
00076 "OnOffMotors"
00077 };
00078
00079
00083 class UavHeightControlOptions : public Options {
00084 public:
00086 IntOption* controller_type;
00087
00088 DecimalOption* gain1;
00089 DecimalOption* gain2;
00090 DecimalOption* gain3;
00091
00092 DecimalOption* Kp;
00093 DecimalOption* Kd;
00094 DecimalOption* Ki;
00095
00096 DecimalOption* thrust_scale_bks;
00097 DecimalOption* thrust_scale_pid;
00098
00099 BoolOption* direct_control;
00100 DecimalOption* height_ref;
00101
00103 UavHeightControlOptions();
00104
00105 string getObjectName() const {
00106 return "UavHeightControlOptions";
00107 }
00108 };
00109
00110
00114 class UavHeightControl : public Task{
00115 private:
00116
00118 static const TaskPlate _plate = UHC_TASK;
00119
00121 static const long int _maxDurSec = 0, _maxDurUsec = 100000;
00122 static const long int _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 50000;
00123 static const long int _minSamplPeriodSec = 0, _minSamplPeriodUsec = 40000;
00124
00126 Uav* _uav;
00127
00129 UavHeightControlOptions _options;
00130
00132 int _exampleOfIntVariable;
00133 double _exampleOfDecimalVariable;
00134 UavHeightControlStates _state;
00135
00137 void _firstRun();
00138 void _normalRun_pid();
00139 void _normalRun_bks();
00140 void _onoff_motors();
00141
00143 void readInput();
00144
00146 Decimal posZ_des;
00147
00149 Time t_motor;
00150
00152 Decimal s_mes[4];
00153 Decimal v_est[4];
00154 Decimal t_mes[4];
00155 Decimal last_t_sonar;
00156 Decimal last_quad_time;
00157 Decimal sonar_h, sonar_dh, sonar_h_last;
00158 Decimal sonar_ddh, sonar_dh_last;
00159 Decimal g_est;
00160 Decimal thrust_control;
00161
00162 void dirtyderivative();
00163
00165 Decimal c1;
00166 Decimal c2;
00167 Decimal c3;
00168 Decimal thrust_scale_bks;
00169 Decimal backstepping();
00170
00171 Decimal Kp;
00172 Decimal Kd;
00173 Decimal Ki;
00174
00175 Decimal integral;
00176
00177 Decimal thrust_scale_pid;
00178 Decimal pid();
00179 Decimal discrete_pid();
00180
00181 Decimal error_k_1, error_k_2, u_k_1;
00182
00183 Keyboard* _keyboard;
00184
00186 ofstream logfile;
00187 public:
00189 UavHeightControl(ResourcePointers resources,int argc, const char* argv[]);
00190
00192 ~UavHeightControl();
00193
00196 TaskPlate getPlate() const {
00197 return _plate;
00198 }
00200 Time getMaxDuration(){
00201 return Time(_maxDurSec,_maxDurUsec);
00202 }
00204 Time getMaxSamplPeriod(){
00205 return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
00206 }
00208 Time getMinSamplPeriod(){
00209 return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
00210 }
00211
00213 TaskOutputs run();
00214 };
00215
00216
00217
00218
00219 };
00220
00221 #endif
00222
00223
00224
00225
00226