branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Threads_and_main/main.cpp@2:e7874762cc25, 2019-10-21 (annotated)
- Committer:
- altb2
- Date:
- Mon Oct 21 17:16:11 2019 +0000
- Revision:
- 2:e7874762cc25
- Parent:
- Sources/main.cpp@1:d8c9f6b16279
- Child:
- 3:bc24fee36ba3
Added additional Ekfs, tested AltHold (still some bugs, Problems at high Lift Rates)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
altb2 | 2:e7874762cc25 | 1 | // ----------------------- MAIN FOR Quadcopt FIRMWARE -------------------------- |
altb2 | 2:e7874762cc25 | 2 | // October 2019, |
altb2 | 2:e7874762cc25 | 3 | // ----------------------------------------------------------------------------- |
altb2 | 2:e7874762cc25 | 4 | // tested on QK2 (good performance!! |
altb2 | 2:e7874762cc25 | 5 | |
altb2 | 0:a479dc61e931 | 6 | #define PI 3.141592653589793 |
altb2 | 0:a479dc61e931 | 7 | #include "mbed.h" |
altb2 | 0:a479dc61e931 | 8 | #define _MBED_ |
altb2 | 0:a479dc61e931 | 9 | #include "LinearCharacteristics.h" |
altb2 | 0:a479dc61e931 | 10 | #include "UAV.h" |
altb2 | 0:a479dc61e931 | 11 | #include "RCin.h" |
altb2 | 0:a479dc61e931 | 12 | #include "define_constants.h" |
altb2 | 0:a479dc61e931 | 13 | #include "PID_Cntrl.h" |
altb2 | 0:a479dc61e931 | 14 | #include "IIR_filter.h" |
altb2 | 0:a479dc61e931 | 15 | #include "data_structs.h" |
altb2 | 0:a479dc61e931 | 16 | #include "Signal.h" |
altb2 | 0:a479dc61e931 | 17 | #include "RangeFinder.h" |
altb2 | 0:a479dc61e931 | 18 | #include "AHRS.h" |
altb2 | 1:d8c9f6b16279 | 19 | #include "TFMini_i2c.h" |
altb2 | 1:d8c9f6b16279 | 20 | #include "data_structs.h" |
altb2 | 1:d8c9f6b16279 | 21 | #include "Controller_Loops.h" |
altb2 | 1:d8c9f6b16279 | 22 | #include "QK_StateMachine.h" |
altb2 | 1:d8c9f6b16279 | 23 | #include "Data_Logger.h" |
altb2 | 1:d8c9f6b16279 | 24 | #include "Read_Xtern_Sensors.h" |
altb2 | 1:d8c9f6b16279 | 25 | |
altb2 | 0:a479dc61e931 | 26 | |
altb2 | 0:a479dc61e931 | 27 | // ---------------------------------------------------------------------------- |
altb2 | 0:a479dc61e931 | 28 | // define IOs |
altb2 | 2:e7874762cc25 | 29 | Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer |
altb2 | 2:e7874762cc25 | 30 | PwmOut motor_pwms[4] = {PC_6,PC_8,PB_14,PA_5}; // the 4 PWM outs in vector form |
altb2 | 2:e7874762cc25 | 31 | I2C i2c(PB_9, PB_8); |
altb2 | 2:e7874762cc25 | 32 | RawSerial uart4lidar(PA_0, PA_1); |
altb2 | 2:e7874762cc25 | 33 | |
altb2 | 0:a479dc61e931 | 34 | // ---------------------------------------------------------------------------- |
altb2 | 2:e7874762cc25 | 35 | RCin myRC(PB_6); // read PWM of RC (Graupner Hott) sum signal |
altb2 | 0:a479dc61e931 | 36 | // ----------------------------------------------------------------------------- |
altb2 | 0:a479dc61e931 | 37 | // The copter! |
altb2 | 1:d8c9f6b16279 | 38 | UAV copter(QUAD2,AIRGEAR350); // Copter and Motor definition |
altb2 | 0:a479dc61e931 | 39 | // ----------------------------------------------------------------------------- |
altb2 | 2:e7874762cc25 | 40 | DATA_Xchange data; // data x-change between threads |
altb2 | 0:a479dc61e931 | 41 | // ----------------------------------------------------------------------------- |
altb2 | 0:a479dc61e931 | 42 | // sample rates |
altb2 | 2:e7874762cc25 | 43 | float Ts200 = 0.005f; // for fast controller |
altb2 | 2:e7874762cc25 | 44 | float Ts100 = 0.01f; // not used |
altb2 | 2:e7874762cc25 | 45 | float Ts50 = 0.02f; // ahrs, logging |
altb2 | 0:a479dc61e931 | 46 | float Ts25 = 0.04f; |
altb2 | 2:e7874762cc25 | 47 | float Ts10 = 0.1f; |
altb2 | 0:a479dc61e931 | 48 | // ----------------------------------- |
altb2 | 0:a479dc61e931 | 49 | float TsCntrl = Ts200; |
altb2 | 0:a479dc61e931 | 50 | // ----------------------------------- |
altb2 | 1:d8c9f6b16279 | 51 | // ------ THREADS --- THREADS --- THREADS --- THREADS --- THREADS --- |
altb2 | 1:d8c9f6b16279 | 52 | Controller_Loops controller(TsCntrl,4,4,true); // run ahrs within the controller Thread |
altb2 | 1:d8c9f6b16279 | 53 | // controllers are not properly initialized at this point (esp limits)!!! |
altb2 | 1:d8c9f6b16279 | 54 | //AHRS ahrs(1,Ts100,true); |
altb2 | 2:e7874762cc25 | 55 | Read_Xtern_Sensors xternal_sensors(Ts50,&uart4lidar,&i2c); // read external sensors (OF, Lidar etc. |
altb2 | 1:d8c9f6b16279 | 56 | QK_StateMachine main_statemachine(Ts50); // the main state machine |
altb2 | 2:e7874762cc25 | 57 | Data_Logger my_datalogger(31,Ts50); // the data logger |
altb2 | 0:a479dc61e931 | 58 | // ----------------------------------------------------------------------------- |
altb2 | 0:a479dc61e931 | 59 | // MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN |
altb2 | 0:a479dc61e931 | 60 | // ----------------------------------------------------------------------------- |
altb2 | 0:a479dc61e931 | 61 | int main() { |
altb2 | 1:d8c9f6b16279 | 62 | copter.calc_copter(); // calc the copter (length, etc) |
altb2 | 1:d8c9f6b16279 | 63 | controller.set_controller_limits(copter); // finalize init function for controllers |
altb2 | 1:d8c9f6b16279 | 64 | wait(.1); |
altb2 | 1:d8c9f6b16279 | 65 | for(uint8_t k=0;k < copter.nb_motors;k++) |
altb2 | 1:d8c9f6b16279 | 66 | motor_pwms[k].period_ms(5); |
altb2 | 1:d8c9f6b16279 | 67 | wait(0.1); |
altb2 | 0:a479dc61e931 | 68 | myRC.map_Channels(); |
altb2 | 1:d8c9f6b16279 | 69 | wait(.1); |
altb2 | 0:a479dc61e931 | 70 | while(!myRC.isAlive) |
altb2 | 0:a479dc61e931 | 71 | { |
altb2 | 0:a479dc61e931 | 72 | wait(.5); |
altb2 | 0:a479dc61e931 | 73 | pc.printf("Failed to receive RC - signal!\r\n"); |
altb2 | 0:a479dc61e931 | 74 | } |
altb2 | 1:d8c9f6b16279 | 75 | pc.printf("RC - signal is on!\r\n"); |
altb2 | 0:a479dc61e931 | 76 | wait(.5); |
altb2 | 1:d8c9f6b16279 | 77 | // start main threads: |
altb2 | 1:d8c9f6b16279 | 78 | controller.init_loop(); |
altb2 | 1:d8c9f6b16279 | 79 | wait(0.1); |
altb2 | 1:d8c9f6b16279 | 80 | controller.start_loop(); |
altb2 | 1:d8c9f6b16279 | 81 | wait(0.1); |
altb2 | 2:e7874762cc25 | 82 | xternal_sensors.start_loop(); |
altb2 | 1:d8c9f6b16279 | 83 | wait(0.1); |
altb2 | 1:d8c9f6b16279 | 84 | main_statemachine.start_loop(); |
altb2 | 1:d8c9f6b16279 | 85 | wait(0.1); |
altb2 | 1:d8c9f6b16279 | 86 | my_datalogger.start_logging(); |
altb2 | 2:e7874762cc25 | 87 | //ahrs.start_loop(); // not needed if ahrs is called in controller (see Thread init of controller) |
altb2 | 1:d8c9f6b16279 | 88 | wait(0.1); |
altb2 | 1:d8c9f6b16279 | 89 | while(1) |
altb2 | 1:d8c9f6b16279 | 90 | wait(1); |
altb2 | 1:d8c9f6b16279 | 91 | |
altb2 | 0:a479dc61e931 | 92 | } |