branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Threads_and_main/Controller_Loops.h@4:dc844fde64d7, 2019-11-22 (annotated)
- Committer:
- Kiwicjam
- Date:
- Fri Nov 22 08:40:26 2019 +0000
- Revision:
- 4:dc844fde64d7
- Parent:
- 3:bc24fee36ba3
Workin set, not running,
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 0:a479dc61e931 | 1 | #ifndef CONTROLLER_LOOPS_H_ |
altb2 | 0:a479dc61e931 | 2 | #define CONTROLLER_LOOPS_H_ |
altb2 | 0:a479dc61e931 | 3 | |
altb2 | 1:d8c9f6b16279 | 4 | #include "mbed.h" |
altb2 | 1:d8c9f6b16279 | 5 | #include "AHRS.h" |
altb2 | 1:d8c9f6b16279 | 6 | #include "Signal.h" |
altb2 | 1:d8c9f6b16279 | 7 | #include "UAV.h" |
altb2 | 1:d8c9f6b16279 | 8 | #include "RCin.h" |
altb2 | 1:d8c9f6b16279 | 9 | #include "PID_Cntrl.h" |
altb2 | 1:d8c9f6b16279 | 10 | |
altb2 | 1:d8c9f6b16279 | 11 | |
altb2 | 1:d8c9f6b16279 | 12 | |
altb2 | 1:d8c9f6b16279 | 13 | // need a few external stuff |
altb2 | 1:d8c9f6b16279 | 14 | extern DATA_Xchange data; |
altb2 | 1:d8c9f6b16279 | 15 | extern UAV copter; |
altb2 | 1:d8c9f6b16279 | 16 | extern PwmOut motor_pwms[4]; |
altb2 | 1:d8c9f6b16279 | 17 | extern Serial pc; |
altb2 | 1:d8c9f6b16279 | 18 | extern RCin myRC; |
altb2 | 1:d8c9f6b16279 | 19 | |
altb2 | 1:d8c9f6b16279 | 20 | class Controller_Loops{ |
altb2 | 0:a479dc61e931 | 21 | public: |
altb2 | 1:d8c9f6b16279 | 22 | Controller_Loops(float ,uint8_t ,uint8_t ); |
altb2 | 1:d8c9f6b16279 | 23 | Controller_Loops(float ,uint8_t ,uint8_t, bool ); // use this, if ahrs has no extra thread |
altb2 | 1:d8c9f6b16279 | 24 | virtual ~Controller_Loops(); |
altb2 | 1:d8c9f6b16279 | 25 | void init_loop(void); |
altb2 | 1:d8c9f6b16279 | 26 | void start_loop(void); |
altb2 | 1:d8c9f6b16279 | 27 | void reset_all(void); |
altb2 | 1:d8c9f6b16279 | 28 | void reset_des_z(void); |
altb2 | 1:d8c9f6b16279 | 29 | void reset_des_z(float); |
altb2 | 3:bc24fee36ba3 | 30 | void reset_des_yaw(float); |
altb2 | 1:d8c9f6b16279 | 31 | PID_Cntrl rate_cntrl[3]; |
altb2 | 1:d8c9f6b16279 | 32 | PID_Cntrl vel_cntrl[3]; |
altb2 | 1:d8c9f6b16279 | 33 | PID_Cntrl pos_cntrl[3]; |
altb2 | 1:d8c9f6b16279 | 34 | void enable_stabilized(void); |
altb2 | 1:d8c9f6b16279 | 35 | void enable_acro(void); |
altb2 | 1:d8c9f6b16279 | 36 | void enable_alt_hold(void); |
altb2 | 1:d8c9f6b16279 | 37 | void enable_vel_of_z_pos(void); |
altb2 | 1:d8c9f6b16279 | 38 | void disable_all(void); |
altb2 | 1:d8c9f6b16279 | 39 | void set_controller_limits(UAV); |
altb2 | 3:bc24fee36ba3 | 40 | float des_z; |
altb2 | 3:bc24fee36ba3 | 41 | float des_yaw; |
altb2 | 0:a479dc61e931 | 42 | |
altb2 | 0:a479dc61e931 | 43 | private: |
altb2 | 1:d8c9f6b16279 | 44 | |
altb2 | 1:d8c9f6b16279 | 45 | float Ts; |
altb2 | 1:d8c9f6b16279 | 46 | float Kv[3]; |
altb2 | 1:d8c9f6b16279 | 47 | |
altb2 | 1:d8c9f6b16279 | 48 | float scale_PM1_to_vel, scale_PM1_to_rate, scale_PM1_to_angle; |
altb2 | 1:d8c9f6b16279 | 49 | |
altb2 | 1:d8c9f6b16279 | 50 | void loop(void); |
altb2 | 0:a479dc61e931 | 51 | bool Rate_Controller_Running; |
altb2 | 0:a479dc61e931 | 52 | bool Attitude_Controller_Running; |
altb2 | 0:a479dc61e931 | 53 | bool Yaw_Controller_Running; |
altb2 | 0:a479dc61e931 | 54 | bool Vel_Controller_Running; |
altb2 | 0:a479dc61e931 | 55 | bool Pos_Controller_Running; |
altb2 | 0:a479dc61e931 | 56 | bool Althold_Controller_Running; |
altb2 | 2:e7874762cc25 | 57 | void parametrize_controllers(void); |
altb2 | 0:a479dc61e931 | 58 | |
altb2 | 1:d8c9f6b16279 | 59 | AHRS cntrl_ahrs; |
altb2 | 0:a479dc61e931 | 60 | Signal signal; |
altb2 | 0:a479dc61e931 | 61 | Thread thread; |
altb2 | 0:a479dc61e931 | 62 | Ticker ticker; |
altb2 | 1:d8c9f6b16279 | 63 | Mutex mutex; |
altb2 | 0:a479dc61e931 | 64 | void sendSignal(); |
altb2 | 0:a479dc61e931 | 65 | uint8_t downsamp_1; |
altb2 | 0:a479dc61e931 | 66 | uint8_t downsamp_2; |
altb2 | 0:a479dc61e931 | 67 | uint8_t downsamp_counter_1; |
altb2 | 0:a479dc61e931 | 68 | uint8_t downsamp_counter_2; |
altb2 | 0:a479dc61e931 | 69 | float calc_des_pos(float); |
altb2 | 3:bc24fee36ba3 | 70 | float calc_des_yaw(float); |
altb2 | 2:e7874762cc25 | 71 | float deadzone(float,float,float); |
altb2 | 1:d8c9f6b16279 | 72 | float PM1_2_F_Thrust(float); |
altb2 | 0:a479dc61e931 | 73 | float max_delta; // max tracking error in z |
altb2 | 3:bc24fee36ba3 | 74 | float max_delta_yaw; // max tracking error in yaw |
altb2 | 0:a479dc61e931 | 75 | float max_climb_rate; |
altb2 | 3:bc24fee36ba3 | 76 | float max_yaw_rate; |
altb2 | 1:d8c9f6b16279 | 77 | bool with_ahrs; |
altb2 | 1:d8c9f6b16279 | 78 | DigitalOut dout1; |
altb2 | 0:a479dc61e931 | 79 | |
altb2 | 0:a479dc61e931 | 80 | |
altb2 | 1:d8c9f6b16279 | 81 | }; |
altb2 | 0:a479dc61e931 | 82 | #endif |