altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Threads_and_main/main.cpp@4:dc844fde64d7, 2019-11-22 (annotated)

Committer:
Kiwicjam
Date:
Fri Nov 22 08:40:26 2019 +0000
Revision:
4:dc844fde64d7
Parent:
3:bc24fee36ba3 
Workin set, not running,

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
Kiwicjam 4:dc844fde64d7 26 #include "RST265.h"
Kiwicjam 4:dc844fde64d7 27 RawSerial rsUart(PA_2, PA_3, 115200);
Kiwicjam 4:dc844fde64d7 28 RST265 rsdev(&rsUart);
Kiwicjam 4:dc844fde64d7 29
altb2 0:a479dc61e931 30
altb2 0:a479dc61e931 31 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 32 // define IOs
altb2 2:e7874762cc25 33 Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer
altb2 2:e7874762cc25 34 PwmOut motor_pwms[4] = {PC_6,PC_8,PB_14,PA_5}; // the 4 PWM outs in vector form
altb2 2:e7874762cc25 35 I2C i2c(PB_9, PB_8);
altb2 2:e7874762cc25 36 RawSerial uart4lidar(PA_0, PA_1);
altb2 2:e7874762cc25 37
altb2 0:a479dc61e931 38 // ----------------------------------------------------------------------------
altb2 2:e7874762cc25 39 RCin myRC(PB_6); // read PWM of RC (Graupner Hott) sum signal
altb2 0:a479dc61e931 40 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 41 // The copter!
altb2 1:d8c9f6b16279 42 UAV copter(QUAD2,AIRGEAR350); // Copter and Motor definition
altb2 0:a479dc61e931 43 // -----------------------------------------------------------------------------
altb2 2:e7874762cc25 44 DATA_Xchange data; // data x-change between threads
altb2 0:a479dc61e931 45 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 46 // sample rates
altb2 2:e7874762cc25 47 float Ts200 = 0.005f; // for fast controller
altb2 2:e7874762cc25 48 float Ts100 = 0.01f; // not used
altb2 2:e7874762cc25 49 float Ts50 = 0.02f; // ahrs, logging
altb2 0:a479dc61e931 50 float Ts25 = 0.04f;
altb2 2:e7874762cc25 51 float Ts10 = 0.1f;
altb2 0:a479dc61e931 52 // -----------------------------------
altb2 0:a479dc61e931 53 float TsCntrl = Ts200;
altb2 0:a479dc61e931 54 // -----------------------------------
altb2 1:d8c9f6b16279 55 // ------ THREADS --- THREADS --- THREADS --- THREADS --- THREADS ---
altb2 1:d8c9f6b16279 56 Controller_Loops controller(TsCntrl,4,4,true); // run ahrs within the controller Thread
altb2 1:d8c9f6b16279 57 // controllers are not properly initialized at this point (esp limits)!!!
Kiwicjam 4:dc844fde64d7 58 AHRS ahrs(1,Ts100,true);
altb2 2:e7874762cc25 59 Read_Xtern_Sensors xternal_sensors(Ts50,&uart4lidar,&i2c); // read external sensors (OF, Lidar etc.
altb2 1:d8c9f6b16279 60 QK_StateMachine main_statemachine(Ts50); // the main state machine
altb2 3:bc24fee36ba3 61 Data_Logger my_datalogger(37,Ts50); // the data logger
altb2 0:a479dc61e931 62 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 63 // MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN
altb2 0:a479dc61e931 64 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 65 int main() {
Kiwicjam 4:dc844fde64d7 66 // T265 test
Kiwicjam 4:dc844fde64d7 67 pc.printf("\n\rSerial Test...%d\n", 115200);
Kiwicjam 4:dc844fde64d7 68 DigitalIn button(USER_BUTTON);
Kiwicjam 4:dc844fde64d7 69
Kiwicjam 4:dc844fde64d7 70 bool pushed = false;
Kiwicjam 4:dc844fde64d7 71 while(1) {
Kiwicjam 4:dc844fde64d7 72 // register button pressed and request data
Kiwicjam 4:dc844fde64d7 73 if(!button && !pushed){
Kiwicjam 4:dc844fde64d7 74 pc.printf("\r\nsending request..");
Kiwicjam 4:dc844fde64d7 75 pushed = true;
Kiwicjam 4:dc844fde64d7 76 rsdev.request();
Kiwicjam 4:dc844fde64d7 77 }else if (button && pushed){
Kiwicjam 4:dc844fde64d7 78 // reset button
Kiwicjam 4:dc844fde64d7 79 pushed = false;
Kiwicjam 4:dc844fde64d7 80 }
Kiwicjam 4:dc844fde64d7 81
Kiwicjam 4:dc844fde64d7 82 if(rsdev.run()){
Kiwicjam 4:dc844fde64d7 83 // new data ready
Kiwicjam 4:dc844fde64d7 84 pc.printf("\r\n received response");
Kiwicjam 4:dc844fde64d7 85 }
Kiwicjam 4:dc844fde64d7 86 }
Kiwicjam 4:dc844fde64d7 87 /*copter.calc_copter(); // calc the copter (length, etc)
altb2 1:d8c9f6b16279 88 controller.set_controller_limits(copter); // finalize init function for controllers
altb2 1:d8c9f6b16279 89 wait(.1);
altb2 1:d8c9f6b16279 90 for(uint8_t k=0;k < copter.nb_motors;k++)
altb2 1:d8c9f6b16279 91 motor_pwms[k].period_ms(5);
altb2 1:d8c9f6b16279 92 wait(0.1);
altb2 0:a479dc61e931 93 myRC.map_Channels();
altb2 1:d8c9f6b16279 94 wait(.1);
altb2 0:a479dc61e931 95 while(!myRC.isAlive)
altb2 0:a479dc61e931 96 {
altb2 0:a479dc61e931 97 wait(.5);
altb2 0:a479dc61e931 98 pc.printf("Failed to receive RC - signal!\r\n");
altb2 0:a479dc61e931 99 }
altb2 1:d8c9f6b16279 100 pc.printf("RC - signal is on!\r\n");
altb2 0:a479dc61e931 101 wait(.5);
altb2 1:d8c9f6b16279 102 // start main threads:
altb2 1:d8c9f6b16279 103 controller.init_loop();
altb2 1:d8c9f6b16279 104 wait(0.1);
altb2 1:d8c9f6b16279 105 controller.start_loop();
altb2 1:d8c9f6b16279 106 wait(0.1);
altb2 2:e7874762cc25 107 xternal_sensors.start_loop();
altb2 1:d8c9f6b16279 108 wait(0.1);
altb2 1:d8c9f6b16279 109 main_statemachine.start_loop();
altb2 1:d8c9f6b16279 110 wait(0.1);
altb2 1:d8c9f6b16279 111 my_datalogger.start_logging();
altb2 2:e7874762cc25 112 //ahrs.start_loop(); // not needed if ahrs is called in controller (see Thread init of controller)
altb2 1:d8c9f6b16279 113 wait(0.1);
altb2 1:d8c9f6b16279 114 while(1)
Kiwicjam 4:dc844fde64d7 115 wait(1);*/
altb2 1:d8c9f6b16279 116
altb2 0:a479dc61e931 117 }