altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/Controller_Loops.h@2:e7874762cc25, 2019-10-21 (annotated)

Committer:
altb2
Date:
Mon Oct 21 17:16:11 2019 +0000
Revision:
2:e7874762cc25
Parent:
Sources/Controller_Loops.h@1:d8c9f6b16279
Child:
3:bc24fee36ba3
Added additional Ekfs, tested AltHold (still some bugs, Problems at high Lift Rates)

Who changed what in which revision?

UserRevisionLine numberNew 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 1:d8c9f6b16279 30 PID_Cntrl rate_cntrl[3];
altb2 1:d8c9f6b16279 31 PID_Cntrl vel_cntrl[3];
altb2 1:d8c9f6b16279 32 PID_Cntrl pos_cntrl[3];
altb2 1:d8c9f6b16279 33 void enable_stabilized(void);
altb2 1:d8c9f6b16279 34 void enable_acro(void);
altb2 1:d8c9f6b16279 35 void enable_alt_hold(void);
altb2 1:d8c9f6b16279 36 void enable_vel_of_z_pos(void);
altb2 1:d8c9f6b16279 37 void disable_all(void);
altb2 1:d8c9f6b16279 38 void set_controller_limits(UAV);
altb2 0:a479dc61e931 39
altb2 0:a479dc61e931 40 private:
altb2 1:d8c9f6b16279 41
altb2 1:d8c9f6b16279 42 float Ts;
altb2 1:d8c9f6b16279 43 float Kv[3];
altb2 1:d8c9f6b16279 44
altb2 1:d8c9f6b16279 45 float scale_PM1_to_vel, scale_PM1_to_rate, scale_PM1_to_angle;
altb2 1:d8c9f6b16279 46
altb2 1:d8c9f6b16279 47 void loop(void);
altb2 0:a479dc61e931 48 bool Rate_Controller_Running;
altb2 0:a479dc61e931 49 bool Attitude_Controller_Running;
altb2 0:a479dc61e931 50 bool Yaw_Controller_Running;
altb2 0:a479dc61e931 51 bool Vel_Controller_Running;
altb2 0:a479dc61e931 52 bool Pos_Controller_Running;
altb2 0:a479dc61e931 53 bool Althold_Controller_Running;
altb2 2:e7874762cc25 54 void parametrize_controllers(void);
altb2 0:a479dc61e931 55
altb2 1:d8c9f6b16279 56 AHRS cntrl_ahrs;
altb2 0:a479dc61e931 57 Signal signal;
altb2 0:a479dc61e931 58 Thread thread;
altb2 0:a479dc61e931 59 Ticker ticker;
altb2 1:d8c9f6b16279 60 Mutex mutex;
altb2 0:a479dc61e931 61 void sendSignal();
altb2 0:a479dc61e931 62 uint8_t downsamp_1;
altb2 0:a479dc61e931 63 uint8_t downsamp_2;
altb2 0:a479dc61e931 64 uint8_t downsamp_counter_1;
altb2 0:a479dc61e931 65 uint8_t downsamp_counter_2;
altb2 0:a479dc61e931 66 float calc_des_pos(float);
altb2 2:e7874762cc25 67 float deadzone(float,float,float);
altb2 1:d8c9f6b16279 68 float PM1_2_F_Thrust(float);
altb2 0:a479dc61e931 69 float des_z;
altb2 0:a479dc61e931 70 float max_delta; // max tracking error in z
altb2 0:a479dc61e931 71 float max_climb_rate;
altb2 1:d8c9f6b16279 72 bool with_ahrs;
altb2 1:d8c9f6b16279 73 DigitalOut dout1;
altb2 0:a479dc61e931 74
altb2 0:a479dc61e931 75
altb2 1:d8c9f6b16279 76 };
altb2 0:a479dc61e931 77 #endif