branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Threads_and_main/main.cpp
- Committer:
- altb2
- Date:
- 2019-10-28
- Revision:
- 3:bc24fee36ba3
- Parent:
- 2:e7874762cc25
- Child:
- 4:dc844fde64d7
File content as of revision 3:bc24fee36ba3:
// ----------------------- MAIN FOR Quadcopt FIRMWARE -------------------------- // October 2019, // ----------------------------------------------------------------------------- // tested on QK2 (good performance!! #define PI 3.141592653589793 #include "mbed.h" #define _MBED_ #include "LinearCharacteristics.h" #include "UAV.h" #include "RCin.h" #include "define_constants.h" #include "PID_Cntrl.h" #include "IIR_filter.h" #include "data_structs.h" #include "Signal.h" #include "RangeFinder.h" #include "AHRS.h" #include "TFMini_i2c.h" #include "data_structs.h" #include "Controller_Loops.h" #include "QK_StateMachine.h" #include "Data_Logger.h" #include "Read_Xtern_Sensors.h" // ---------------------------------------------------------------------------- // define IOs Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer PwmOut motor_pwms[4] = {PC_6,PC_8,PB_14,PA_5}; // the 4 PWM outs in vector form I2C i2c(PB_9, PB_8); RawSerial uart4lidar(PA_0, PA_1); // ---------------------------------------------------------------------------- RCin myRC(PB_6); // read PWM of RC (Graupner Hott) sum signal // ----------------------------------------------------------------------------- // The copter! UAV copter(QUAD2,AIRGEAR350); // Copter and Motor definition // ----------------------------------------------------------------------------- DATA_Xchange data; // data x-change between threads // ----------------------------------------------------------------------------- // sample rates float Ts200 = 0.005f; // for fast controller float Ts100 = 0.01f; // not used float Ts50 = 0.02f; // ahrs, logging float Ts25 = 0.04f; float Ts10 = 0.1f; // ----------------------------------- float TsCntrl = Ts200; // ----------------------------------- // ------ THREADS --- THREADS --- THREADS --- THREADS --- THREADS --- Controller_Loops controller(TsCntrl,4,4,true); // run ahrs within the controller Thread // controllers are not properly initialized at this point (esp limits)!!! //AHRS ahrs(1,Ts100,true); Read_Xtern_Sensors xternal_sensors(Ts50,&uart4lidar,&i2c); // read external sensors (OF, Lidar etc. QK_StateMachine main_statemachine(Ts50); // the main state machine Data_Logger my_datalogger(37,Ts50); // the data logger // ----------------------------------------------------------------------------- // MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN // ----------------------------------------------------------------------------- int main() { copter.calc_copter(); // calc the copter (length, etc) controller.set_controller_limits(copter); // finalize init function for controllers wait(.1); for(uint8_t k=0;k < copter.nb_motors;k++) motor_pwms[k].period_ms(5); wait(0.1); myRC.map_Channels(); wait(.1); while(!myRC.isAlive) { wait(.5); pc.printf("Failed to receive RC - signal!\r\n"); } pc.printf("RC - signal is on!\r\n"); wait(.5); // start main threads: controller.init_loop(); wait(0.1); controller.start_loop(); wait(0.1); xternal_sensors.start_loop(); wait(0.1); main_statemachine.start_loop(); wait(0.1); my_datalogger.start_logging(); //ahrs.start_loop(); // not needed if ahrs is called in controller (see Thread init of controller) wait(0.1); while(1) wait(1); }