altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Sources/main.cpp@1:d8c9f6b16279, 2019-10-09 (annotated)

Committer:
altb2
Date:
Wed Oct 09 13:47:43 2019 +0000
Revision:
1:d8c9f6b16279
Parent:
0:a479dc61e931 
Newly designed CopterCode Okt 2019, based on Quadcopt_QK2, this Version flies in Stabilized mode, Alt hold not tested yet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:a479dc61e931 1 #define PI 3.141592653589793
altb2 0:a479dc61e931 2 #include "mbed.h"
altb2 0:a479dc61e931 3 #define _MBED_
altb2 0:a479dc61e931 4 #include "LinearCharacteristics.h"
altb2 0:a479dc61e931 5 #include "UAV.h"
altb2 0:a479dc61e931 6 #include "RCin.h"
altb2 0:a479dc61e931 7 #include "define_constants.h"
altb2 0:a479dc61e931 8 #include "PID_Cntrl.h"
altb2 0:a479dc61e931 9 #include "IIR_filter.h"
altb2 0:a479dc61e931 10 #include "data_structs.h"
altb2 0:a479dc61e931 11 #include "Signal.h"
altb2 0:a479dc61e931 12 #include "RangeFinder.h"
altb2 0:a479dc61e931 13 #include "AHRS.h"
altb2 1:d8c9f6b16279 14 #include "TFMini_i2c.h"
altb2 1:d8c9f6b16279 15 #include "data_structs.h"
altb2 1:d8c9f6b16279 16 #include "Controller_Loops.h"
altb2 1:d8c9f6b16279 17 #include "QK_StateMachine.h"
altb2 1:d8c9f6b16279 18 #include "Data_Logger.h"
altb2 1:d8c9f6b16279 19 #include "Read_Xtern_Sensors.h"
altb2 1:d8c9f6b16279 20
altb2 0:a479dc61e931 21
altb2 0:a479dc61e931 22 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 23 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 24 // define IOs
altb2 0:a479dc61e931 25 Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer
altb2 1:d8c9f6b16279 26 PwmOut motor_pwms[4] = {PC_6,PC_8,PB_14,PB_9};
altb2 0:a479dc61e931 27 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 28 RCin myRC(PB_6); // read PWM of RC (Graupner Hott)
altb2 0:a479dc61e931 29 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 30 // PX4 Optical flow (OF), use same i2c like imu
altb2 0:a479dc61e931 31 I2C i2c(PC_9, PA_8);
altb2 0:a479dc61e931 32 PX4Flow optical_flow(i2c);
altb2 1:d8c9f6b16279 33 TFMini_i2c tfmini(&i2c,0x10);
altb2 1:d8c9f6b16279 34
altb2 0:a479dc61e931 35 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 36 // Timers
altb2 0:a479dc61e931 37 Timer global_timer; // in main loop
altb2 0:a479dc61e931 38 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 39 // The copter!
altb2 1:d8c9f6b16279 40 UAV copter(QUAD2,AIRGEAR350); // Copter and Motor definition
altb2 0:a479dc61e931 41 // -----------------------------------------------------------------------------
altb2 1:d8c9f6b16279 42 DATA_Xchange data;
altb2 0:a479dc61e931 43 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 44 // sample rates
altb2 0:a479dc61e931 45 float Ts200 = 0.005f;
altb2 0:a479dc61e931 46 float Ts100 = 0.01f;
altb2 0:a479dc61e931 47 float Ts50 = 0.02f;
altb2 0:a479dc61e931 48 float Ts25 = 0.04f;
altb2 0:a479dc61e931 49 // -----------------------------------
altb2 0:a479dc61e931 50 float TsCntrl = Ts200;
altb2 0:a479dc61e931 51 // -----------------------------------
altb2 0:a479dc61e931 52 // some values needed
altb2 0:a479dc61e931 53 float dt; // time elapsed in current state
altb2 1:d8c9f6b16279 54 // ------ THREADS --- THREADS --- THREADS --- THREADS --- THREADS ---
altb2 1:d8c9f6b16279 55 Controller_Loops controller(TsCntrl,4,4,true); // run ahrs within the controller Thread
altb2 1:d8c9f6b16279 56 // controllers are not properly initialized at this point (esp limits)!!!
altb2 1:d8c9f6b16279 57 //AHRS ahrs(1,Ts100,true);
altb2 1:d8c9f6b16279 58 Read_Xtern_Sensors xternal_sens(Ts50); // read external sensors (OF, Lidar etc.
altb2 1:d8c9f6b16279 59 QK_StateMachine main_statemachine(Ts50); // the main state machine
altb2 1:d8c9f6b16279 60 Data_Logger my_datalogger(22,Ts50); // the data logger
altb2 0:a479dc61e931 61 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 62 // MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN
altb2 0:a479dc61e931 63 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 64 int main() {
altb2 0:a479dc61e931 65 i2c.frequency(400000);
altb2 1:d8c9f6b16279 66 copter.calc_copter(); // calc the copter (length, etc)
altb2 1:d8c9f6b16279 67 controller.set_controller_limits(copter); // finalize init function for controllers
altb2 1:d8c9f6b16279 68 wait(.1);
altb2 1:d8c9f6b16279 69 for(uint8_t k=0;k < copter.nb_motors;k++)
altb2 1:d8c9f6b16279 70 motor_pwms[k].period_ms(5);
altb2 1:d8c9f6b16279 71 wait(0.1);
altb2 0:a479dc61e931 72 myRC.map_Channels();
altb2 1:d8c9f6b16279 73 wait(0.01);
altb2 0:a479dc61e931 74 global_timer.reset();
altb2 0:a479dc61e931 75 global_timer.start();
altb2 1:d8c9f6b16279 76 wait(.1);
altb2 0:a479dc61e931 77 while(!myRC.isAlive)
altb2 0:a479dc61e931 78 {
altb2 0:a479dc61e931 79 wait(.5);
altb2 0:a479dc61e931 80 pc.printf("Failed to receive RC - signal!\r\n");
altb2 0:a479dc61e931 81 }
altb2 1:d8c9f6b16279 82 pc.printf("RC - signal is on!\r\n");
altb2 0:a479dc61e931 83 wait(.5);
altb2 1:d8c9f6b16279 84 // start main threads:
altb2 1:d8c9f6b16279 85 controller.init_loop();
altb2 1:d8c9f6b16279 86 wait(0.1);
altb2 1:d8c9f6b16279 87 controller.start_loop();
altb2 1:d8c9f6b16279 88 wait(0.1);
altb2 1:d8c9f6b16279 89 xternal_sens.start_loop();
altb2 1:d8c9f6b16279 90 wait(0.1);
altb2 1:d8c9f6b16279 91 main_statemachine.start_loop();
altb2 1:d8c9f6b16279 92 wait(0.1);
altb2 1:d8c9f6b16279 93 my_datalogger.start_logging();
altb2 1:d8c9f6b16279 94 //ahrs.start_loop();
altb2 1:d8c9f6b16279 95 wait(0.1);
altb2 1:d8c9f6b16279 96 while(1)
altb2 1:d8c9f6b16279 97 wait(1);
altb2 1:d8c9f6b16279 98
altb2 0:a479dc61e931 99 }