altb_pmic / Mbed OS IndNav_QK3_T265

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?

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