altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Sources/Controller_Loops.h@1:d8c9f6b16279, 2019-10-09 (annotated)

Committer:
altb2
Date:
Wed Oct 09 13:47:43 2019 +0000
Revision:
1:d8c9f6b16279
Parent:
0:a479dc61e931 
Newly designed CopterCode Okt 2019, based on Quadcopt_QK2, this Version flies in Stabilized mode, Alt hold not tested yet

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 0:a479dc61e931 54
altb2 1:d8c9f6b16279 55 AHRS cntrl_ahrs;
altb2 0:a479dc61e931 56 Signal signal;
altb2 0:a479dc61e931 57 Thread thread;
altb2 0:a479dc61e931 58 Ticker ticker;
altb2 1:d8c9f6b16279 59 Mutex mutex;
altb2 0:a479dc61e931 60 void sendSignal();
altb2 0:a479dc61e931 61 uint8_t downsamp_1;
altb2 0:a479dc61e931 62 uint8_t downsamp_2;
altb2 0:a479dc61e931 63 uint8_t downsamp_counter_1;
altb2 0:a479dc61e931 64 uint8_t downsamp_counter_2;
altb2 0:a479dc61e931 65 float calc_des_pos(float);
altb2 1:d8c9f6b16279 66 float PM1_2_F_Thrust(float);
altb2 0:a479dc61e931 67 float des_z;
altb2 0:a479dc61e931 68 float max_delta; // max tracking error in z
altb2 0:a479dc61e931 69 float max_climb_rate;
altb2 1:d8c9f6b16279 70 bool with_ahrs;
altb2 1:d8c9f6b16279 71 DigitalOut dout1;
altb2 0:a479dc61e931 72
altb2 0:a479dc61e931 73
altb2 1:d8c9f6b16279 74 };
altb2 0:a479dc61e931 75 #endif