altb_pmic / Mbed OS IndNav_QK3_T265

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?

UserRevisionLine numberNew 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 }