![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Diff: Sources/main.cpp
- Revision:
- 1:d8c9f6b16279
- Parent:
- 0:a479dc61e931
diff -r a479dc61e931 -r d8c9f6b16279 Sources/main.cpp --- a/Sources/main.cpp Wed Oct 02 15:31:12 2019 +0000 +++ b/Sources/main.cpp Wed Oct 09 13:47:43 2019 +0000 @@ -9,470 +9,91 @@ #include "IIR_filter.h" #include "data_structs.h" #include "Signal.h" -#include "data_logger.h" #include "RangeFinder.h" #include "AHRS.h" -#include "PX4Flow.h" -#include "TFmini.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 -RawSerial uart4lidar(PA_0, PA_1); -PwmOut motor_pwms[4]; -motor_pwms[0] = new PwmOut(PC_6); -motor_pwms[1] = new PwmOut(PC_8); -motor_pwms[2] = new PwmOut(PB_14); -motor_pwms[3] = new PwmOut(PB_9); +PwmOut motor_pwms[4] = {PC_6,PC_8,PB_14,PB_9}; // ---------------------------------------------------------------------------- RCin myRC(PB_6); // read PWM of RC (Graupner Hott) // ---------------------------------------------------------------------------- -// Ultrasonic Range finder (US), read up to 4 signals -RangeFinder rf(PA_10, PB_3, PB_5, PB_4 , 10, 5782.0, 0.02, 12000); // 20 Hz parametrization -TFmini tfmini(uart4lidar); -// ---------------------------------------------------------------------------- // PX4 Optical flow (OF), use same i2c like imu I2C i2c(PC_9, PA_8); PX4Flow optical_flow(i2c); +TFMini_i2c tfmini(&i2c,0x10); + // ----------------------------------------------------------------------------- // Timers Timer global_timer; // in main loop // ----------------------------------------------------------------------------- // The copter! -UAV copter(QUAD2,AIRGEAR350); - +UAV copter(QUAD2,AIRGEAR350); // Copter and Motor definition // ----------------------------------------------------------------------------- -// Characteristic for thrust, -//LinearCharacteristics PM1_2_F_Thrust; // map +-1 to Thrust -float PM1_2_F_Thrust(float); // replace linear char. with nonlinear below - -// ----------------------------------------------------------------------------- -// States and communication -char Message[150]; - +DATA_Xchange data; // ----------------------------------------------------------------------------- // sample rates float Ts200 = 0.005f; float Ts100 = 0.01f; float Ts50 = 0.02f; float Ts25 = 0.04f; -float Ts10 = 0.1f; -float Ts4 = 0.25f; // ----------------------------------- float TsCntrl = Ts200; -float TsSDLog = Ts50; // ----------------------------------- -// some stuff -float Mxy_limit = 0.0f; -uint8_t CS=INIT; // for main state machine -float motorspeed[8]; // motor speeds in rad/sec -AHRS ahrs(1,Ts100,true); // ahrs filter, 1=EKF,,true=do recalibration of imu - -float Kv = 4.0; -// ----------------------------------------------------------------------------- -// use pottis for scaling (temporarily) -LinearCharacteristics map_pwm_2_P(864,1600,0.35,1.5); -LinearCharacteristics map_pwm_2_I(864,1600,2.0, 8.0); -LinearCharacteristics map_pwm_2_PX(864,1600,1.0, 4.0); // RP-Winkelregler -LinearCharacteristics map_pwm_2_scale(864,1600,1.0,2.0); - -// ----------------------------------------------------------------------------- -// my functions (below) -void updateLoop(void); // THE MAIN LOOP!!!! -void motor_mix(float *,float ,float *); // map torques Mx,My,Mz and thrust to motor speeds -void read_US_OF_sensors(void); // read extra sensors (low frequency) -// -void RPY_controller(float *, uint8_t , uint8_t, float *); // the main controller for Stabilized and Acro mode -// ----------------------------------------------------------------------------- -// - some extra functions -float limit_minmax(float ,float ,float ); -float deadzone(float,float,float); -void sendSignal_mainLoop(void); -void sendSignal_read_US_OF_sensors(void); -void sendSignal_Output(void); -// ----------------------------------------------------------------------------- // some values needed float dt; // time elapsed in current state -float Mxyz[3]; // torques -float cur_pos[3]; -bool disarm_flag=false; -bool flipm = false; // used while arming motors -float F_thrust; // Thrust of motors -float z_des = 0.0; -// ----------------------------------------------------------------------------- -// ----------- MBED OS ---------------------------- -Thread thread_mainLoop(osPriorityNormal, 4096); -Thread thread_read_US_OF_sensors(osPriorityBelowNormal, 4096); -Thread thread_UAV_state(osPriorityLow, 4096); -Thread thread_Output(osPriorityIdle, 4096); -// Signals -Signal signal_mainLoop; -Signal signal_read_US_OF_sensors; -Signal signal_read_OF_sensors; -Signal signal_UAV_state; -Signal signal_Output; -// tickers -Ticker mainLoop; // Main loop at 100Hz -Ticker US_OF_sensors_loop; -Ticker UAV_state; // check if UAV crashed / flying / landed etc. -Ticker Output; // Output loop at 4Hz (Writing and saving) - -// ----------------------------------------------------------------------------- -// data logger: -data_logger my_logger(35); // number shows # of logged data columns +// ------ 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_sens(Ts50); // read external sensors (OF, Lidar etc. +QK_StateMachine main_statemachine(Ts50); // the main state machine +Data_Logger my_datalogger(22,Ts50); // the data logger // ----------------------------------------------------------------------------- // MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN // ----------------------------------------------------------------------------- int main() { i2c.frequency(400000); - att_cntrl.P_Roll = Kv; // was 8 // p-controller different attitudes - att_cntrl.P_Pitch = Kv; - // --- initialize PWMS - wait(1); - uint32_t pwm_init_us = 500; - PWMOut1.period_ms(5); - PWMOut1.pulsewidth_us(pwm_init_us); - PWMOut2.period_ms(5); - PWMOut2.pulsewidth_us(pwm_init_us); - PWMOut3.period_ms(5); - PWMOut3.pulsewidth_us(pwm_init_us); - PWMOut4.period_ms(5); - PWMOut4.pulsewidth_us(pwm_init_us); - copter.calc_copter(myMotor); + 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(); - myRC.cou=0; - myMotor.calc_n2pwm_9k5(); - //myMotor.calc_n2pwm_8k4(); + wait(0.01); global_timer.reset(); global_timer.start(); - wait(1); + wait(.1); while(!myRC.isAlive) { wait(.5); pc.printf("Failed to receive RC - signal!\r\n"); } - pc.printf("RC - signal on!\r\n"); + pc.printf("RC - signal is on!\r\n"); wait(.5); - if(1){ - // start main threads: - thread_mainLoop.start(&updateLoop); - mainLoop.attach(&sendSignal_mainLoop, TsCntrl); - thread_read_US_OF_sensors.start(&read_US_OF_sensors); - US_OF_sensors_loop.attach(&sendSignal_read_US_OF_sensors,Ts50); - my_logger.start_logging(TsSDLog); - my_logger.pause_logging(); - } + // start main threads: + controller.init_loop(); + wait(0.1); + controller.start_loop(); + wait(0.1); + xternal_sens.start_loop(); + wait(0.1); + main_statemachine.start_loop(); + wait(0.1); + my_datalogger.start_logging(); + //ahrs.start_loop(); + wait(0.1); + while(1) + wait(1); + } -// ----------------------------------------------------------------------------- -// main loop with state handling and controllers -// ----------------------------------------------------------------------------- -void updateLoop(void){ - pc.printf("Start Main loop...\r\n"); - bool flightmode_changed = false; - float old_dt=0; - uint8_t flm_temp = 1; - uint8_t flm_loc = 1; - float des_values[3]; // temporary variable for desired values - des_values[0]=0.0; // temporary variable for desired values - des_values[1]=0.0; // temporary variable for desired values - des_values[2]=0.0; // temporary variable for desired values - while(1){ - thread_mainLoop.signal_wait(signal_mainLoop); - old_dt = dt; - dt = global_timer.read(); // time elapsed in current state - my_logger.data_vector[24] = dt - old_dt; // time delta for analyses - if(myRC.flightmode_changed){ // This is triggered bei RC-PWM readout - flm_temp = myRC.get_flightmode(); - if(flm_temp == VEL_OF_Z_POS & optical_flow.avg_scaled_quality() <150) // do not change to that flightmode, if OF quality ist too low - { - myRC.flightmode_changed = false; // set flag back, but do nothing else! - } - else{ - flm_loc = flm_temp; - pc.printf("Changed FM to %d \r\n",flm_loc); - my_logger.data_vector[23]=(float)flm_loc; - myRC.flightmode_changed = false; - flightmode_changed = true; - } - } - switch(CS) { - case INIT: // at very beginning - if (dt > 5.0f && (myRC.PM1[1]<-0.9f) && (myRC.PM1[4]>0.9f)){ - CS = INIT_MOTORS; // switch to FLAT state - global_timer.reset(); - printf("INIT_MOTORS\r\n"); - } - break; // CS: INIT - case INIT_MOTORS: // - if(dt < 2.0f && flipm){ // after 2 sec armed - for(uint8_t m=1;m <= copter.nb_motors;m++) - motorspeed[m]=100.0; - flipm = false; - } - else if(dt < 2.0f){ // after 2 sec armed - for(uint8_t m=1;m <= copter.nb_motors;m++) - motorspeed[m]=0.0f; - flipm = true; - } - else{ - CS = WAIT_ARMING; - global_timer.reset(); - pc.printf("Motors initialized\r\n"); - } - break; // CS: INIT_MOTORS - case WAIT_ARMING: // - if(flm_loc == RTL) // HACK!: update scaled Parameters - { - /*rate_cntrl_Roll.scale_PID_param(map_pwm_2_scale(myRC.pwms[10])); - rate_cntrl_Pitch.scale_PID_param(map_pwm_2_scale(myRC.pwms[10])); - att_cntrl.P_Roll = map_pwm_2_scale(myRC.pwms[11]) * Kv; - att_cntrl.P_Pitch = map_pwm_2_scale(myRC.pwms[11]) * Kv;*/ - my_logger.continue_logging(); - } - if((myRC.PM1[1] < -.9) && (myRC.PM1[4]>0.9f) ){ // use CH1 (Roll) and CH4 (Yaw) to arm (stick to middle) - CS = ARMING; - my_logger.continue_logging(); - pc.printf("Rate Cntrl P: %2.3f\r\n",rate_cntrl_Roll.get_P_gain()); - pc.printf("Att Cntrl P: %2.3f\r\n",att_cntrl.P_Roll); - global_timer.reset(); - } - break; // CS: WAIT_ARMING - case ARMING: // - if((myRC.PM1[1] >= -.9) || (myRC.PM1[4]<.9f)){ - CS = WAIT_ARMING; - global_timer.reset(); - } - else if(dt > 1.0f){ // after 1 sec armed - CS = START_MOTORS; - global_timer.reset(); - pc.printf("Armed, start motors at %3.0f rad/s \r\n",copter.static_thrust_n*0.1f); - } - break; // CS: ARMING - case START_MOTORS: // - for(uint8_t m=1;m <= copter.nb_motors;m++) - motorspeed[m]=copter.static_thrust_n*0.1f; - if (dt > 1.5f){ - CS = FLIGHT; - global_timer.reset(); - Mxy_limit = copter.max_Mxy; - rate_cntrl_Roll.reset(0.0); - rate_cntrl_Roll.set_limits(-Mxy_limit,Mxy_limit); - rate_cntrl_Pitch.reset(0.0); - rate_cntrl_Pitch.set_limits(-Mxy_limit,Mxy_limit); - // rate_cntrl_Roll_rolloff.reset(0.0); - // rate_cntrl_Pitch_rolloff.reset(0.0); - rate_cntrl_Yaw.reset(0.0); - rate_cntrl_Yaw.set_limits(-copter.max_Mz,copter.max_Mz); - pos_cntrl_z.reset(0.0); - z_des = 0.0; - } - break; // CS: START_MOTORS - case FLIGHT: - // 2nd SWITCH-CASE - switch(flm_loc){ - case STABILIZED: // STABILIZED: Control RP-Angles with Sticks, Yaw-Rate with Sticks, Thrust with Sticks - des_values[0] = myRC.PM1[1] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle - des_values[1] = myRC.PM1[2] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle - des_values[2] = -myRC.PM1[4]* 0.8f;//integrate_CH4_2_yaw_angle(myRC.PM1[4]);; // integrate +- 1 to yaw angle - F_thrust = PM1_2_F_Thrust(myRC.PM1[3]); // desired thrust - RPY_controller(des_values, 1,1,Mxyz); - break; // flm_loc: STABILIZED - case ACRO: // ACRO: Control Rates with sticks, Thrust with Sticks - des_values[0] = myRC.PM1[1]*2.5f; // +- 1 correspond to +-1 rad/s desired rate - des_values[1] = myRC.PM1[2]*2.5f; // +- 1 correspond to +-1 rad/s desired rate - des_values[2] = -myRC.PM1[4]*1.5f; // +- 1 correspond to +-1 rad/s desired rate - F_thrust = PM1_2_F_Thrust(myRC.PM1[3]); // desired thrust - RPY_controller(des_values, 0,1,Mxyz); - break; // flm_loc: ACRO - case ALT_HOLD: // Altitde Hold (with US Sensor) - if(flightmode_changed){ - cur_pos[2] = ahrs.xyzUS[2]; - pos_cntrl_z.reset(F_thrust); - flightmode_changed = false; - // pc.printf("Initialized z:%1.3f\r\n",cur_pos[2]); - } - des_values[0] = myRC.PM1[1] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle - des_values[1] = myRC.PM1[2] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle - des_values[2] = -myRC.PM1[4]* 0.8f; //integrate_CH4_2_yaw_angle(myRC.PM1[4]);; // integrate +- 1 to yaw angle - // cur_pos[2] += 0.8 * TsCntrl * deadzone(myRC.PM1[3],-0.15,0.15); - // cur_pos[2] = limit_minmax(cur_pos[2],-1.0,0.8); - //F_thrust = pos_cntrl_z(cur_pos[2] - ahrs.xyzUS[2]); - F_thrust = pos_cntrl_z(0.8f - ahrs.xyzLIDAR[2]); // use fixed value here and use analog Sensor - RPY_controller(des_values, 1,1,Mxyz); - break; // flm_loc: ALT_HOLD - case VEL_OF_Z_POS: // This is the velocity mode with optical flow sensor - if(flightmode_changed){ - vel_cntrl_x.reset(0.0); - vel_cntrl_y.reset(0.0); - // pos_cntrl_z.reset(F_thrust); // carfull!!! only valid if this state comes from ALT_HOLD - flightmode_changed = false; - } - des_values[1] = vel_cntrl_x(1.5f * myRC.PM1[2] - ahrs.v_xyzOF[0]); // Pitchachse: korrekt: positices pitchen führt zu Bewegung in +x - des_values[0] = -vel_cntrl_y(-1.5f * myRC.PM1[1] - ahrs.v_xyzOF[1]);; // Rollachse: Diese ist gedreht: Positives Rollen fuehrt zu Bewegung -y, Hebel nach recht ebenfalls!!! - des_values[2] = -myRC.PM1[4]* 0.8f; - F_thrust = pos_cntrl_z(0.8f - ahrs.xyzLIDAR[2]); // use fixed value here and use analog Sensor - RPY_controller(des_values, 1,1,Mxyz); - break; // flm_loc: VEL_OF_Z_POS - default: break; - } // end of switch(flm_loc) - // ... continue FLIGHT case - motor_mix(Mxyz,F_thrust,motorspeed); // MAp TOrques to Motor speeds - if((myRC.PM1[1] > 0.8f) && (myRC.PM1[4]<-0.8f) && (myRC.PM1[3]<-0.8f) && disarm_flag == false){ // use CH1 (Roll) and CH4 (Yaw) to disarm (stick to leftright) - disarm_flag=true; - global_timer.reset(); - } - if(disarm_flag == true && dt > 0.8f){ - disarm_flag=false; - global_timer.reset(); - CS = MOTOR_STOPPED; - my_logger.pause_logging(); - pc.printf("UAV disarmed\r\n"); - } - break; // case FLIGHT - case MOTOR_STOPPED: - for(uint8_t m=1;m <= copter.nb_motors;m++) - motorspeed[m]=0.0f; - if(dt>1){ - global_timer.reset(); - CS = WAIT_ARMING; - } // case MOTOR_STOPPED - break; - default: break; - } // end of switch(CS) - // ------- Datalogger - my_logger.data_vector[15] = Mxyz[0]; - my_logger.data_vector[16] = Mxyz[1]; - my_logger.data_vector[17] = Mxyz[2]; - my_logger.data_vector[18]=F_thrust; - my_logger.data_vector[19]=motorspeed[1]; - my_logger.data_vector[20]=motorspeed[2]; - my_logger.data_vector[21]=motorspeed[3]; - my_logger.data_vector[22]=motorspeed[4]; - my_logger.data_vector[25]=des_values[0]; - my_logger.data_vector[26]=des_values[1]; - my_logger.data_vector[27]=des_values[2]; - // pc.printf("%3.3f %3.3f %3.3f \r\n",des_values[0],des_values[1],des_values[2]); - } // of while(1) (the thread) -} -//----------------------------------------------------------------------------- -// Read Ultrasonic range finder Sensors (via PWM) and Optcal Flow Sensors -// WARNING: US-Sensors interfere each other, thus the sensorw are read sequentially -void read_US_OF_sensors(void){ - while(1) - { - thread_read_US_OF_sensors.signal_wait(signal_read_US_OF_sensors); - /* - rf.read_and_filter(rf.current_sensor++); - if(rf.current_sensor >= rf.n_sensors) // loop over 4 Sensors from step to step.(only 1/4 sampling rate) - rf.current_sensor = 0; - //for(uint8_t k=0;k<rf.n_sensors;k++)printf("S%d: %1.3f ",k,rf.val[k]);printf("\r\n"); - ahrs.xyzUS[0] = 1.0f-(rf.val[2]+rf.val[3])/2; // mind x positive! If Sensors measure larger distance, it is towards -x !!! - // distance 1m is the origin! - ahrs.xyzUS[1] = rf.val[1]; // larger position of sensor 1 means +y (correct direction) - ahrs.xyzUS[2] = rf.val[0]; // z-sensor - ahrs.rxryrzUS[2] = (rf.val[3]-rf.val[2])/0.4; // Yaw axis - my_logger.data_vector[18]=rf.val[0]; - */ - ahrs.xyzAS[2] = 2.311f *exp(-6.5514f*ai1.read())+0.2017f; - ahrs.xyzLIDAR[2] = tfmini()*0.001f; - my_logger.data_vector[14]=ahrs.xyzLIDAR[2]; - //my_logger.data_vector[22]=ahrs.xyzAS[2]; - if(optical_flow.update_integral()) { - ahrs.v_xyzOF[0]= vel_cntrl_x_rolloff(-optical_flow.avg_flowx()*0.001f + ahrs.raw_gy2gy(ahrs.imu.gyroY) * ahrs.xyzLIDAR[2]); - ahrs.v_xyzOF[1]= vel_cntrl_y_rolloff( optical_flow.avg_flowy()*0.001f - ahrs.raw_gx2gx(ahrs.imu.gyroX) * ahrs.xyzLIDAR[2]); - ahrs.xyzOF[2]= 0.0f;//((float)optical_flow.ground_distance())*0.001f; - my_logger.data_vector[10]=ahrs.v_xyzOF[0]; - my_logger.data_vector[11]=ahrs.v_xyzOF[1]; - my_logger.data_vector[13]=(float)optical_flow.avg_scaled_quality(); - my_logger.data_vector[28]=optical_flow.gyro_x_rate(); - my_logger.data_vector[29]=optical_flow.gyro_y_rate(); - my_logger.data_vector[30]=optical_flow.gyro_z_rate(); - } - } -} - - -/* **************************************************************************** -void RPY_controller(float *des_values, uint8_t rate_and_att,float* Mxyz){ - Roll/Pitch/Yaw Controller, - INPUT: - des_values desired angle ( mode stabilized aso), rotation rates (acro) - rate_and_att logic for flight mode (1 = cascade control) - *Mxyz resulting values -***************************************************************************** */ -void RPY_controller(float *des_values, uint8_t rate_and_att,uint8_t filtertype,float* Mxyz){ - float rate_des[4]; -//-------------------------------- - if(rate_and_att ==1){ // this is for Mode STABILIZED (and Loiter aso) - rate_des[1] = att_cntrl.P_Roll * (des_values[0] - ahrs.getRoll(filtertype)); - rate_des[2] = att_cntrl.P_Pitch * (des_values[1] - ahrs.getPitch(filtertype)); - rate_des[3] = des_values[2];//att_cntrl.P_Yaw * (des_values[2] - ahrs.getYawRadians()); - } - else{ // this is for Mode ACRO - rate_des[1] = des_values[0]; - rate_des[2] = des_values[1]; - rate_des[3] = des_values[2]; - } - // Mxyz[0]= rate_cntrl_Roll_rolloff( rate_cntrl_Roll(rate_des[1] - ahrs.raw_gx2gx(ahrs.imu.gyroX)) ); - // Mxyz[1]= rate_cntrl_Pitch_rolloff( rate_cntrl_Pitch(rate_des[2] - ahrs.raw_gy2gy(ahrs.imu.gyroY)) ); - float gx = ahrs.raw_gx2gx(ahrs.imu.gyroX); - float gy = ahrs.raw_gy2gy(ahrs.imu.gyroY); - Mxyz[0]= rate_cntrl_Roll(rate_des[1] - gx, gx); - Mxyz[1]= rate_cntrl_Pitch(rate_des[2] - gy, gy); - Mxyz[2]= rate_cntrl_Yaw(rate_des[3] - ahrs.raw_gz2gz(ahrs.imu.gyroZ)); - } -// ***************************************************************************** -// ***************************************************************************** -float limit_minmax(float val,float mi,float ma){ - if(val < mi) - val = mi; - if(val > ma) - val = ma; - return val; -} - -// ***************************************************************************** -// Dead zone, from lo ... hi -float deadzone(float x,float lo,float hi){ -return (x>hi)*(x-hi)+(x<lo)*(x-lo); -} - -// ***************************************************************************** -// nonlinear characteristics for thrust -float PM1_2_F_Thrust(float x) -{ - return 9.81f*copter.weight*(0.5f*x+1.1f - 0.06f/(x+1.1f)); // y=0.5*x+1.1-0.06./(x+1.1); -} - - -void sendSignal_mainLoop(void){ - thread_mainLoop.signal_set(signal_mainLoop); -} -void sendSignal_read_US_OF_sensors(void){ - thread_read_US_OF_sensors.signal_set(signal_read_US_OF_sensors); -} - -void sendSignal_UAV_state(void){ - thread_UAV_state.signal_set(signal_UAV_state); -} -void sendSignal_Output(void){ - thread_Output.signal_set(signal_Output); -} - -/* -Thread thread_mainLoop(osPriorityNormal, 4096); -Thread thread_AHRS(osPriorityBelowNormal, 4096); -Thread thread_UAV_state(osPriorityLow, 4096); -Thread thread_Output(osPriorityIdle, 4096); - -Signal signal_mainLoop; -Signal signal_AHRS; -Signal signal_UAV_state; -Signal signal_Output; - -Ticker AHRS; // Madgwick filter for AHRS -Ticker mainLoop; // Main loop at 200Hz -Ticker UAV_state; // check if UAV crashed / flying / landed etc. -Ticker Output; // Output loop at 10Hz (Writing and saving -*/ \ No newline at end of file