altb_pmic
branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Diff: Sources/main.cpp
- Revision:
- 0:a479dc61e931
- Child:
- 1:d8c9f6b16279
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Sources/main.cpp Wed Oct 02 15:31:12 2019 +0000
@@ -0,0 +1,478 @@
+#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 "data_logger.h"
+#include "RangeFinder.h"
+#include "AHRS.h"
+#include "PX4Flow.h"
+#include "TFmini.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);
+// ----------------------------------------------------------------------------
+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);
+// -----------------------------------------------------------------------------
+// Timers
+Timer global_timer; // in main loop
+// -----------------------------------------------------------------------------
+// The copter!
+UAV copter(QUAD2,AIRGEAR350);
+
+// -----------------------------------------------------------------------------
+// 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];
+
+// -----------------------------------------------------------------------------
+// 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
+// -----------------------------------------------------------------------------
+// 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);
+ myRC.map_Channels();
+ myRC.cou=0;
+ myMotor.calc_n2pwm_9k5();
+ //myMotor.calc_n2pwm_8k4();
+ global_timer.reset();
+ global_timer.start();
+ wait(1);
+ while(!myRC.isAlive)
+ {
+ wait(.5);
+ pc.printf("Failed to receive RC - signal!\r\n");
+ }
+ pc.printf("RC - signal 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();
+ }
+}
+// -----------------------------------------------------------------------------
+// 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