File content as of revision 4:dc844fde64d7:

// ----------------------- 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"

#include "RST265.h"
RawSerial rsUart(PA_2, PA_3, 115200);
RST265 rsdev(&rsUart);


// ----------------------------------------------------------------------------
// 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() {
    // T265 test
    pc.printf("\n\rSerial Test...%d\n", 115200);
    DigitalIn button(USER_BUTTON);
    
    bool pushed = false; 
    while(1) {
        // register button pressed and request data
        if(!button && !pushed){
            pc.printf("\r\nsending request..");
            pushed = true;
            rsdev.request();    
        }else if (button && pushed){
            // reset button
            pushed = false;    
        } 
        
        if(rsdev.run()){
            // new data ready 
            pc.printf("\r\n received response"); 
        }
    }      
    /*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);*/
            
}