altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/main.cpp@3:bc24fee36ba3, 2019-10-28 (annotated)

Committer:
altb2
Date:
Mon Oct 28 07:54:10 2019 +0000
Revision:
3:bc24fee36ba3
Parent:
2:e7874762cc25
Child:
4:dc844fde64d7
- 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 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 3:bc24fee36ba3 57 Data_Logger my_datalogger(37,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 }