Diff: Threads_and_main/main.cpp

Revision:

2:e7874762cc25

Parent:

1:d8c9f6b16279

Child:

3:bc24fee36ba3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Threads_and_main/main.cpp	Mon Oct 21 17:16:11 2019 +0000
@@ -0,0 +1,92 @@
+// ----------------------- 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(31,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);
+            
+}