![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
branch for tests with T265
Dependencies: Lib_Cntrl AHRS Lib_Misc
Sources/main.cpp
- Committer:
- altb2
- Date:
- 2019-10-02
- Revision:
- 0:a479dc61e931
- Child:
- 1:d8c9f6b16279
File content as of revision 0:a479dc61e931:
#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 */