altb_pmic
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