altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/Controller_Loops.h@3:bc24fee36ba3, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:54:10 2019 +0000
Revision:
3:bc24fee36ba3
Parent:
2:e7874762cc25 
- with complementFilter for Yaw (Magneteometer), Lidar, AltHold running, Flow sensor reading, control sth wrong, Yaw not tested in detail

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 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