altb_pmic / Mbed OS IndNav_QK3_T265

branch for tests with T265

Dependencies:   Lib_Cntrl AHRS Lib_Misc

Sources/main.cpp@0:a479dc61e931, 2019-10-02 (annotated)

Committer:
altb2
Date:
Wed Oct 02 15:31:12 2019 +0000
Revision:
0:a479dc61e931
Child:
1:d8c9f6b16279
first setup, not running!!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
altb2 0:a479dc61e931 1 #define PI 3.141592653589793
altb2 0:a479dc61e931 2 #include "mbed.h"
altb2 0:a479dc61e931 3 #define _MBED_
altb2 0:a479dc61e931 4 #include "LinearCharacteristics.h"
altb2 0:a479dc61e931 5 #include "UAV.h"
altb2 0:a479dc61e931 6 #include "RCin.h"
altb2 0:a479dc61e931 7 #include "define_constants.h"
altb2 0:a479dc61e931 8 #include "PID_Cntrl.h"
altb2 0:a479dc61e931 9 #include "IIR_filter.h"
altb2 0:a479dc61e931 10 #include "data_structs.h"
altb2 0:a479dc61e931 11 #include "Signal.h"
altb2 0:a479dc61e931 12 #include "data_logger.h"
altb2 0:a479dc61e931 13 #include "RangeFinder.h"
altb2 0:a479dc61e931 14 #include "AHRS.h"
altb2 0:a479dc61e931 15 #include "PX4Flow.h"
altb2 0:a479dc61e931 16 #include "TFmini.h"
altb2 0:a479dc61e931 17
altb2 0:a479dc61e931 18 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 19 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 20 // define IOs
altb2 0:a479dc61e931 21 Serial pc(SERIAL_TX, SERIAL_RX,115200); // serial connection via USB - programmer
altb2 0:a479dc61e931 22 RawSerial uart4lidar(PA_0, PA_1);
altb2 0:a479dc61e931 23 PwmOut motor_pwms[4];
altb2 0:a479dc61e931 24 motor_pwms[0] = new PwmOut(PC_6);
altb2 0:a479dc61e931 25 motor_pwms[1] = new PwmOut(PC_8);
altb2 0:a479dc61e931 26 motor_pwms[2] = new PwmOut(PB_14);
altb2 0:a479dc61e931 27 motor_pwms[3] = new PwmOut(PB_9);
altb2 0:a479dc61e931 28 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 29 RCin myRC(PB_6); // read PWM of RC (Graupner Hott)
altb2 0:a479dc61e931 30 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 31 // Ultrasonic Range finder (US), read up to 4 signals
altb2 0:a479dc61e931 32 RangeFinder rf(PA_10, PB_3, PB_5, PB_4 , 10, 5782.0, 0.02, 12000); // 20 Hz parametrization
altb2 0:a479dc61e931 33 TFmini tfmini(uart4lidar);
altb2 0:a479dc61e931 34 // ----------------------------------------------------------------------------
altb2 0:a479dc61e931 35 // PX4 Optical flow (OF), use same i2c like imu
altb2 0:a479dc61e931 36 I2C i2c(PC_9, PA_8);
altb2 0:a479dc61e931 37 PX4Flow optical_flow(i2c);
altb2 0:a479dc61e931 38 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 39 // Timers
altb2 0:a479dc61e931 40 Timer global_timer; // in main loop
altb2 0:a479dc61e931 41 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 42 // The copter!
altb2 0:a479dc61e931 43 UAV copter(QUAD2,AIRGEAR350);
altb2 0:a479dc61e931 44
altb2 0:a479dc61e931 45 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 46 // Characteristic for thrust,
altb2 0:a479dc61e931 47 //LinearCharacteristics PM1_2_F_Thrust; // map +-1 to Thrust
altb2 0:a479dc61e931 48 float PM1_2_F_Thrust(float); // replace linear char. with nonlinear below
altb2 0:a479dc61e931 49
altb2 0:a479dc61e931 50 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 51 // States and communication
altb2 0:a479dc61e931 52 char Message[150];
altb2 0:a479dc61e931 53
altb2 0:a479dc61e931 54 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 55 // sample rates
altb2 0:a479dc61e931 56 float Ts200 = 0.005f;
altb2 0:a479dc61e931 57 float Ts100 = 0.01f;
altb2 0:a479dc61e931 58 float Ts50 = 0.02f;
altb2 0:a479dc61e931 59 float Ts25 = 0.04f;
altb2 0:a479dc61e931 60 float Ts10 = 0.1f;
altb2 0:a479dc61e931 61 float Ts4 = 0.25f;
altb2 0:a479dc61e931 62 // -----------------------------------
altb2 0:a479dc61e931 63 float TsCntrl = Ts200;
altb2 0:a479dc61e931 64 float TsSDLog = Ts50;
altb2 0:a479dc61e931 65 // -----------------------------------
altb2 0:a479dc61e931 66 // some stuff
altb2 0:a479dc61e931 67 float Mxy_limit = 0.0f;
altb2 0:a479dc61e931 68 uint8_t CS=INIT; // for main state machine
altb2 0:a479dc61e931 69 float motorspeed[8]; // motor speeds in rad/sec
altb2 0:a479dc61e931 70 AHRS ahrs(1,Ts100,true); // ahrs filter, 1=EKF,,true=do recalibration of imu
altb2 0:a479dc61e931 71
altb2 0:a479dc61e931 72 float Kv = 4.0;
altb2 0:a479dc61e931 73 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 74 // use pottis for scaling (temporarily)
altb2 0:a479dc61e931 75 LinearCharacteristics map_pwm_2_P(864,1600,0.35,1.5);
altb2 0:a479dc61e931 76 LinearCharacteristics map_pwm_2_I(864,1600,2.0, 8.0);
altb2 0:a479dc61e931 77 LinearCharacteristics map_pwm_2_PX(864,1600,1.0, 4.0); // RP-Winkelregler
altb2 0:a479dc61e931 78 LinearCharacteristics map_pwm_2_scale(864,1600,1.0,2.0);
altb2 0:a479dc61e931 79
altb2 0:a479dc61e931 80 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 81 // my functions (below)
altb2 0:a479dc61e931 82 void updateLoop(void); // THE MAIN LOOP!!!!
altb2 0:a479dc61e931 83 void motor_mix(float *,float ,float *); // map torques Mx,My,Mz and thrust to motor speeds
altb2 0:a479dc61e931 84 void read_US_OF_sensors(void); // read extra sensors (low frequency)
altb2 0:a479dc61e931 85 //
altb2 0:a479dc61e931 86 void RPY_controller(float *, uint8_t , uint8_t, float *); // the main controller for Stabilized and Acro mode
altb2 0:a479dc61e931 87 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 88 // - some extra functions
altb2 0:a479dc61e931 89 float limit_minmax(float ,float ,float );
altb2 0:a479dc61e931 90 float deadzone(float,float,float);
altb2 0:a479dc61e931 91 void sendSignal_mainLoop(void);
altb2 0:a479dc61e931 92 void sendSignal_read_US_OF_sensors(void);
altb2 0:a479dc61e931 93 void sendSignal_Output(void);
altb2 0:a479dc61e931 94 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 95 // some values needed
altb2 0:a479dc61e931 96 float dt; // time elapsed in current state
altb2 0:a479dc61e931 97 float Mxyz[3]; // torques
altb2 0:a479dc61e931 98 float cur_pos[3];
altb2 0:a479dc61e931 99 bool disarm_flag=false;
altb2 0:a479dc61e931 100 bool flipm = false; // used while arming motors
altb2 0:a479dc61e931 101 float F_thrust; // Thrust of motors
altb2 0:a479dc61e931 102 float z_des = 0.0;
altb2 0:a479dc61e931 103 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 104 // ----------- MBED OS ----------------------------
altb2 0:a479dc61e931 105 Thread thread_mainLoop(osPriorityNormal, 4096);
altb2 0:a479dc61e931 106 Thread thread_read_US_OF_sensors(osPriorityBelowNormal, 4096);
altb2 0:a479dc61e931 107 Thread thread_UAV_state(osPriorityLow, 4096);
altb2 0:a479dc61e931 108 Thread thread_Output(osPriorityIdle, 4096);
altb2 0:a479dc61e931 109 // Signals
altb2 0:a479dc61e931 110 Signal signal_mainLoop;
altb2 0:a479dc61e931 111 Signal signal_read_US_OF_sensors;
altb2 0:a479dc61e931 112 Signal signal_read_OF_sensors;
altb2 0:a479dc61e931 113 Signal signal_UAV_state;
altb2 0:a479dc61e931 114 Signal signal_Output;
altb2 0:a479dc61e931 115 // tickers
altb2 0:a479dc61e931 116 Ticker mainLoop; // Main loop at 100Hz
altb2 0:a479dc61e931 117 Ticker US_OF_sensors_loop;
altb2 0:a479dc61e931 118 Ticker UAV_state; // check if UAV crashed / flying / landed etc.
altb2 0:a479dc61e931 119 Ticker Output; // Output loop at 4Hz (Writing and saving)
altb2 0:a479dc61e931 120
altb2 0:a479dc61e931 121 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 122 // data logger:
altb2 0:a479dc61e931 123 data_logger my_logger(35); // number shows # of logged data columns
altb2 0:a479dc61e931 124 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 125 // MAIN MAIN MAIN MAIN MAIN MAIN MAIN MAIN
altb2 0:a479dc61e931 126 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 127 int main() {
altb2 0:a479dc61e931 128 i2c.frequency(400000);
altb2 0:a479dc61e931 129 att_cntrl.P_Roll = Kv; // was 8 // p-controller different attitudes
altb2 0:a479dc61e931 130 att_cntrl.P_Pitch = Kv;
altb2 0:a479dc61e931 131 // --- initialize PWMS
altb2 0:a479dc61e931 132 wait(1);
altb2 0:a479dc61e931 133 uint32_t pwm_init_us = 500;
altb2 0:a479dc61e931 134 PWMOut1.period_ms(5);
altb2 0:a479dc61e931 135 PWMOut1.pulsewidth_us(pwm_init_us);
altb2 0:a479dc61e931 136 PWMOut2.period_ms(5);
altb2 0:a479dc61e931 137 PWMOut2.pulsewidth_us(pwm_init_us);
altb2 0:a479dc61e931 138 PWMOut3.period_ms(5);
altb2 0:a479dc61e931 139 PWMOut3.pulsewidth_us(pwm_init_us);
altb2 0:a479dc61e931 140 PWMOut4.period_ms(5);
altb2 0:a479dc61e931 141 PWMOut4.pulsewidth_us(pwm_init_us);
altb2 0:a479dc61e931 142 copter.calc_copter(myMotor);
altb2 0:a479dc61e931 143 myRC.map_Channels();
altb2 0:a479dc61e931 144 myRC.cou=0;
altb2 0:a479dc61e931 145 myMotor.calc_n2pwm_9k5();
altb2 0:a479dc61e931 146 //myMotor.calc_n2pwm_8k4();
altb2 0:a479dc61e931 147 global_timer.reset();
altb2 0:a479dc61e931 148 global_timer.start();
altb2 0:a479dc61e931 149 wait(1);
altb2 0:a479dc61e931 150 while(!myRC.isAlive)
altb2 0:a479dc61e931 151 {
altb2 0:a479dc61e931 152 wait(.5);
altb2 0:a479dc61e931 153 pc.printf("Failed to receive RC - signal!\r\n");
altb2 0:a479dc61e931 154 }
altb2 0:a479dc61e931 155 pc.printf("RC - signal on!\r\n");
altb2 0:a479dc61e931 156 wait(.5);
altb2 0:a479dc61e931 157 if(1){
altb2 0:a479dc61e931 158 // start main threads:
altb2 0:a479dc61e931 159 thread_mainLoop.start(&updateLoop);
altb2 0:a479dc61e931 160 mainLoop.attach(&sendSignal_mainLoop, TsCntrl);
altb2 0:a479dc61e931 161 thread_read_US_OF_sensors.start(&read_US_OF_sensors);
altb2 0:a479dc61e931 162 US_OF_sensors_loop.attach(&sendSignal_read_US_OF_sensors,Ts50);
altb2 0:a479dc61e931 163 my_logger.start_logging(TsSDLog);
altb2 0:a479dc61e931 164 my_logger.pause_logging();
altb2 0:a479dc61e931 165 }
altb2 0:a479dc61e931 166 }
altb2 0:a479dc61e931 167 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 168 // main loop with state handling and controllers
altb2 0:a479dc61e931 169 // -----------------------------------------------------------------------------
altb2 0:a479dc61e931 170 void updateLoop(void){
altb2 0:a479dc61e931 171 pc.printf("Start Main loop...\r\n");
altb2 0:a479dc61e931 172 bool flightmode_changed = false;
altb2 0:a479dc61e931 173 float old_dt=0;
altb2 0:a479dc61e931 174 uint8_t flm_temp = 1;
altb2 0:a479dc61e931 175 uint8_t flm_loc = 1;
altb2 0:a479dc61e931 176 float des_values[3]; // temporary variable for desired values
altb2 0:a479dc61e931 177 des_values[0]=0.0; // temporary variable for desired values
altb2 0:a479dc61e931 178 des_values[1]=0.0; // temporary variable for desired values
altb2 0:a479dc61e931 179 des_values[2]=0.0; // temporary variable for desired values
altb2 0:a479dc61e931 180 while(1){
altb2 0:a479dc61e931 181 thread_mainLoop.signal_wait(signal_mainLoop);
altb2 0:a479dc61e931 182 old_dt = dt;
altb2 0:a479dc61e931 183 dt = global_timer.read(); // time elapsed in current state
altb2 0:a479dc61e931 184 my_logger.data_vector[24] = dt - old_dt; // time delta for analyses
altb2 0:a479dc61e931 185 if(myRC.flightmode_changed){ // This is triggered bei RC-PWM readout
altb2 0:a479dc61e931 186 flm_temp = myRC.get_flightmode();
altb2 0:a479dc61e931 187 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
altb2 0:a479dc61e931 188 {
altb2 0:a479dc61e931 189 myRC.flightmode_changed = false; // set flag back, but do nothing else!
altb2 0:a479dc61e931 190 }
altb2 0:a479dc61e931 191 else{
altb2 0:a479dc61e931 192 flm_loc = flm_temp;
altb2 0:a479dc61e931 193 pc.printf("Changed FM to %d \r\n",flm_loc);
altb2 0:a479dc61e931 194 my_logger.data_vector[23]=(float)flm_loc;
altb2 0:a479dc61e931 195 myRC.flightmode_changed = false;
altb2 0:a479dc61e931 196 flightmode_changed = true;
altb2 0:a479dc61e931 197 }
altb2 0:a479dc61e931 198 }
altb2 0:a479dc61e931 199 switch(CS) {
altb2 0:a479dc61e931 200 case INIT: // at very beginning
altb2 0:a479dc61e931 201 if (dt > 5.0f && (myRC.PM1[1]<-0.9f) && (myRC.PM1[4]>0.9f)){
altb2 0:a479dc61e931 202 CS = INIT_MOTORS; // switch to FLAT state
altb2 0:a479dc61e931 203 global_timer.reset();
altb2 0:a479dc61e931 204 printf("INIT_MOTORS\r\n");
altb2 0:a479dc61e931 205 }
altb2 0:a479dc61e931 206 break; // CS: INIT
altb2 0:a479dc61e931 207 case INIT_MOTORS: //
altb2 0:a479dc61e931 208 if(dt < 2.0f && flipm){ // after 2 sec armed
altb2 0:a479dc61e931 209 for(uint8_t m=1;m <= copter.nb_motors;m++)
altb2 0:a479dc61e931 210 motorspeed[m]=100.0;
altb2 0:a479dc61e931 211 flipm = false;
altb2 0:a479dc61e931 212 }
altb2 0:a479dc61e931 213 else if(dt < 2.0f){ // after 2 sec armed
altb2 0:a479dc61e931 214 for(uint8_t m=1;m <= copter.nb_motors;m++)
altb2 0:a479dc61e931 215 motorspeed[m]=0.0f;
altb2 0:a479dc61e931 216 flipm = true;
altb2 0:a479dc61e931 217 }
altb2 0:a479dc61e931 218 else{
altb2 0:a479dc61e931 219 CS = WAIT_ARMING;
altb2 0:a479dc61e931 220 global_timer.reset();
altb2 0:a479dc61e931 221 pc.printf("Motors initialized\r\n");
altb2 0:a479dc61e931 222 }
altb2 0:a479dc61e931 223 break; // CS: INIT_MOTORS
altb2 0:a479dc61e931 224 case WAIT_ARMING: //
altb2 0:a479dc61e931 225 if(flm_loc == RTL) // HACK!: update scaled Parameters
altb2 0:a479dc61e931 226 {
altb2 0:a479dc61e931 227 /*rate_cntrl_Roll.scale_PID_param(map_pwm_2_scale(myRC.pwms[10]));
altb2 0:a479dc61e931 228 rate_cntrl_Pitch.scale_PID_param(map_pwm_2_scale(myRC.pwms[10]));
altb2 0:a479dc61e931 229 att_cntrl.P_Roll = map_pwm_2_scale(myRC.pwms[11]) * Kv;
altb2 0:a479dc61e931 230 att_cntrl.P_Pitch = map_pwm_2_scale(myRC.pwms[11]) * Kv;*/
altb2 0:a479dc61e931 231 my_logger.continue_logging();
altb2 0:a479dc61e931 232 }
altb2 0:a479dc61e931 233 if((myRC.PM1[1] < -.9) && (myRC.PM1[4]>0.9f) ){ // use CH1 (Roll) and CH4 (Yaw) to arm (stick to middle)
altb2 0:a479dc61e931 234 CS = ARMING;
altb2 0:a479dc61e931 235 my_logger.continue_logging();
altb2 0:a479dc61e931 236 pc.printf("Rate Cntrl P: %2.3f\r\n",rate_cntrl_Roll.get_P_gain());
altb2 0:a479dc61e931 237 pc.printf("Att Cntrl P: %2.3f\r\n",att_cntrl.P_Roll);
altb2 0:a479dc61e931 238 global_timer.reset();
altb2 0:a479dc61e931 239 }
altb2 0:a479dc61e931 240 break; // CS: WAIT_ARMING
altb2 0:a479dc61e931 241 case ARMING: //
altb2 0:a479dc61e931 242 if((myRC.PM1[1] >= -.9) || (myRC.PM1[4]<.9f)){
altb2 0:a479dc61e931 243 CS = WAIT_ARMING;
altb2 0:a479dc61e931 244 global_timer.reset();
altb2 0:a479dc61e931 245 }
altb2 0:a479dc61e931 246 else if(dt > 1.0f){ // after 1 sec armed
altb2 0:a479dc61e931 247 CS = START_MOTORS;
altb2 0:a479dc61e931 248 global_timer.reset();
altb2 0:a479dc61e931 249 pc.printf("Armed, start motors at %3.0f rad/s \r\n",copter.static_thrust_n*0.1f);
altb2 0:a479dc61e931 250 }
altb2 0:a479dc61e931 251 break; // CS: ARMING
altb2 0:a479dc61e931 252 case START_MOTORS: //
altb2 0:a479dc61e931 253 for(uint8_t m=1;m <= copter.nb_motors;m++)
altb2 0:a479dc61e931 254 motorspeed[m]=copter.static_thrust_n*0.1f;
altb2 0:a479dc61e931 255 if (dt > 1.5f){
altb2 0:a479dc61e931 256 CS = FLIGHT;
altb2 0:a479dc61e931 257 global_timer.reset();
altb2 0:a479dc61e931 258 Mxy_limit = copter.max_Mxy;
altb2 0:a479dc61e931 259 rate_cntrl_Roll.reset(0.0);
altb2 0:a479dc61e931 260 rate_cntrl_Roll.set_limits(-Mxy_limit,Mxy_limit);
altb2 0:a479dc61e931 261 rate_cntrl_Pitch.reset(0.0);
altb2 0:a479dc61e931 262 rate_cntrl_Pitch.set_limits(-Mxy_limit,Mxy_limit);
altb2 0:a479dc61e931 263 // rate_cntrl_Roll_rolloff.reset(0.0);
altb2 0:a479dc61e931 264 // rate_cntrl_Pitch_rolloff.reset(0.0);
altb2 0:a479dc61e931 265 rate_cntrl_Yaw.reset(0.0);
altb2 0:a479dc61e931 266 rate_cntrl_Yaw.set_limits(-copter.max_Mz,copter.max_Mz);
altb2 0:a479dc61e931 267 pos_cntrl_z.reset(0.0);
altb2 0:a479dc61e931 268 z_des = 0.0;
altb2 0:a479dc61e931 269 }
altb2 0:a479dc61e931 270 break; // CS: START_MOTORS
altb2 0:a479dc61e931 271 case FLIGHT:
altb2 0:a479dc61e931 272 // 2nd SWITCH-CASE
altb2 0:a479dc61e931 273 switch(flm_loc){
altb2 0:a479dc61e931 274 case STABILIZED: // STABILIZED: Control RP-Angles with Sticks, Yaw-Rate with Sticks, Thrust with Sticks
altb2 0:a479dc61e931 275 des_values[0] = myRC.PM1[1] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle
altb2 0:a479dc61e931 276 des_values[1] = myRC.PM1[2] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle
altb2 0:a479dc61e931 277 des_values[2] = -myRC.PM1[4]* 0.8f;//integrate_CH4_2_yaw_angle(myRC.PM1[4]);; // integrate +- 1 to yaw angle
altb2 0:a479dc61e931 278 F_thrust = PM1_2_F_Thrust(myRC.PM1[3]); // desired thrust
altb2 0:a479dc61e931 279 RPY_controller(des_values, 1,1,Mxyz);
altb2 0:a479dc61e931 280 break; // flm_loc: STABILIZED
altb2 0:a479dc61e931 281 case ACRO: // ACRO: Control Rates with sticks, Thrust with Sticks
altb2 0:a479dc61e931 282 des_values[0] = myRC.PM1[1]*2.5f; // +- 1 correspond to +-1 rad/s desired rate
altb2 0:a479dc61e931 283 des_values[1] = myRC.PM1[2]*2.5f; // +- 1 correspond to +-1 rad/s desired rate
altb2 0:a479dc61e931 284 des_values[2] = -myRC.PM1[4]*1.5f; // +- 1 correspond to +-1 rad/s desired rate
altb2 0:a479dc61e931 285 F_thrust = PM1_2_F_Thrust(myRC.PM1[3]); // desired thrust
altb2 0:a479dc61e931 286 RPY_controller(des_values, 0,1,Mxyz);
altb2 0:a479dc61e931 287 break; // flm_loc: ACRO
altb2 0:a479dc61e931 288 case ALT_HOLD: // Altitde Hold (with US Sensor)
altb2 0:a479dc61e931 289 if(flightmode_changed){
altb2 0:a479dc61e931 290 cur_pos[2] = ahrs.xyzUS[2];
altb2 0:a479dc61e931 291 pos_cntrl_z.reset(F_thrust);
altb2 0:a479dc61e931 292 flightmode_changed = false;
altb2 0:a479dc61e931 293 // pc.printf("Initialized z:%1.3f\r\n",cur_pos[2]);
altb2 0:a479dc61e931 294 }
altb2 0:a479dc61e931 295 des_values[0] = myRC.PM1[1] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle
altb2 0:a479dc61e931 296 des_values[1] = myRC.PM1[2] * 0.5f; // +- 1 correspond to +-0.5 rad desired angle
altb2 0:a479dc61e931 297 des_values[2] = -myRC.PM1[4]* 0.8f; //integrate_CH4_2_yaw_angle(myRC.PM1[4]);; // integrate +- 1 to yaw angle
altb2 0:a479dc61e931 298 // cur_pos[2] += 0.8 * TsCntrl * deadzone(myRC.PM1[3],-0.15,0.15);
altb2 0:a479dc61e931 299 // cur_pos[2] = limit_minmax(cur_pos[2],-1.0,0.8);
altb2 0:a479dc61e931 300 //F_thrust = pos_cntrl_z(cur_pos[2] - ahrs.xyzUS[2]);
altb2 0:a479dc61e931 301 F_thrust = pos_cntrl_z(0.8f - ahrs.xyzLIDAR[2]); // use fixed value here and use analog Sensor
altb2 0:a479dc61e931 302 RPY_controller(des_values, 1,1,Mxyz);
altb2 0:a479dc61e931 303 break; // flm_loc: ALT_HOLD
altb2 0:a479dc61e931 304 case VEL_OF_Z_POS: // This is the velocity mode with optical flow sensor
altb2 0:a479dc61e931 305 if(flightmode_changed){
altb2 0:a479dc61e931 306 vel_cntrl_x.reset(0.0);
altb2 0:a479dc61e931 307 vel_cntrl_y.reset(0.0);
altb2 0:a479dc61e931 308 // pos_cntrl_z.reset(F_thrust); // carfull!!! only valid if this state comes from ALT_HOLD
altb2 0:a479dc61e931 309 flightmode_changed = false;
altb2 0:a479dc61e931 310 }
altb2 0:a479dc61e931 311 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
altb2 0:a479dc61e931 312 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!!!
altb2 0:a479dc61e931 313 des_values[2] = -myRC.PM1[4]* 0.8f;
altb2 0:a479dc61e931 314 F_thrust = pos_cntrl_z(0.8f - ahrs.xyzLIDAR[2]); // use fixed value here and use analog Sensor
altb2 0:a479dc61e931 315 RPY_controller(des_values, 1,1,Mxyz);
altb2 0:a479dc61e931 316 break; // flm_loc: VEL_OF_Z_POS
altb2 0:a479dc61e931 317 default: break;
altb2 0:a479dc61e931 318 } // end of switch(flm_loc)
altb2 0:a479dc61e931 319 // ... continue FLIGHT case
altb2 0:a479dc61e931 320 motor_mix(Mxyz,F_thrust,motorspeed); // MAp TOrques to Motor speeds
altb2 0:a479dc61e931 321 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)
altb2 0:a479dc61e931 322 disarm_flag=true;
altb2 0:a479dc61e931 323 global_timer.reset();
altb2 0:a479dc61e931 324 }
altb2 0:a479dc61e931 325 if(disarm_flag == true && dt > 0.8f){
altb2 0:a479dc61e931 326 disarm_flag=false;
altb2 0:a479dc61e931 327 global_timer.reset();
altb2 0:a479dc61e931 328 CS = MOTOR_STOPPED;
altb2 0:a479dc61e931 329 my_logger.pause_logging();
altb2 0:a479dc61e931 330 pc.printf("UAV disarmed\r\n");
altb2 0:a479dc61e931 331 }
altb2 0:a479dc61e931 332 break; // case FLIGHT
altb2 0:a479dc61e931 333 case MOTOR_STOPPED:
altb2 0:a479dc61e931 334 for(uint8_t m=1;m <= copter.nb_motors;m++)
altb2 0:a479dc61e931 335 motorspeed[m]=0.0f;
altb2 0:a479dc61e931 336 if(dt>1){
altb2 0:a479dc61e931 337 global_timer.reset();
altb2 0:a479dc61e931 338 CS = WAIT_ARMING;
altb2 0:a479dc61e931 339 } // case MOTOR_STOPPED
altb2 0:a479dc61e931 340 break;
altb2 0:a479dc61e931 341 default: break;
altb2 0:a479dc61e931 342 } // end of switch(CS)
altb2 0:a479dc61e931 343 // ------- Datalogger
altb2 0:a479dc61e931 344 my_logger.data_vector[15] = Mxyz[0];
altb2 0:a479dc61e931 345 my_logger.data_vector[16] = Mxyz[1];
altb2 0:a479dc61e931 346 my_logger.data_vector[17] = Mxyz[2];
altb2 0:a479dc61e931 347 my_logger.data_vector[18]=F_thrust;
altb2 0:a479dc61e931 348 my_logger.data_vector[19]=motorspeed[1];
altb2 0:a479dc61e931 349 my_logger.data_vector[20]=motorspeed[2];
altb2 0:a479dc61e931 350 my_logger.data_vector[21]=motorspeed[3];
altb2 0:a479dc61e931 351 my_logger.data_vector[22]=motorspeed[4];
altb2 0:a479dc61e931 352 my_logger.data_vector[25]=des_values[0];
altb2 0:a479dc61e931 353 my_logger.data_vector[26]=des_values[1];
altb2 0:a479dc61e931 354 my_logger.data_vector[27]=des_values[2];
altb2 0:a479dc61e931 355 // pc.printf("%3.3f %3.3f %3.3f \r\n",des_values[0],des_values[1],des_values[2]);
altb2 0:a479dc61e931 356 } // of while(1) (the thread)
altb2 0:a479dc61e931 357 }
altb2 0:a479dc61e931 358 //-----------------------------------------------------------------------------
altb2 0:a479dc61e931 359 // Read Ultrasonic range finder Sensors (via PWM) and Optcal Flow Sensors
altb2 0:a479dc61e931 360 // WARNING: US-Sensors interfere each other, thus the sensorw are read sequentially
altb2 0:a479dc61e931 361 void read_US_OF_sensors(void){
altb2 0:a479dc61e931 362 while(1)
altb2 0:a479dc61e931 363 {
altb2 0:a479dc61e931 364 thread_read_US_OF_sensors.signal_wait(signal_read_US_OF_sensors);
altb2 0:a479dc61e931 365 /*
altb2 0:a479dc61e931 366 rf.read_and_filter(rf.current_sensor++);
altb2 0:a479dc61e931 367 if(rf.current_sensor >= rf.n_sensors) // loop over 4 Sensors from step to step.(only 1/4 sampling rate)
altb2 0:a479dc61e931 368 rf.current_sensor = 0;
altb2 0:a479dc61e931 369 //for(uint8_t k=0;k<rf.n_sensors;k++)printf("S%d: %1.3f ",k,rf.val[k]);printf("\r\n");
altb2 0:a479dc61e931 370 ahrs.xyzUS[0] = 1.0f-(rf.val[2]+rf.val[3])/2; // mind x positive! If Sensors measure larger distance, it is towards -x !!!
altb2 0:a479dc61e931 371 // distance 1m is the origin!
altb2 0:a479dc61e931 372 ahrs.xyzUS[1] = rf.val[1]; // larger position of sensor 1 means +y (correct direction)
altb2 0:a479dc61e931 373 ahrs.xyzUS[2] = rf.val[0]; // z-sensor
altb2 0:a479dc61e931 374 ahrs.rxryrzUS[2] = (rf.val[3]-rf.val[2])/0.4; // Yaw axis
altb2 0:a479dc61e931 375 my_logger.data_vector[18]=rf.val[0];
altb2 0:a479dc61e931 376 */
altb2 0:a479dc61e931 377 ahrs.xyzAS[2] = 2.311f *exp(-6.5514f*ai1.read())+0.2017f;
altb2 0:a479dc61e931 378 ahrs.xyzLIDAR[2] = tfmini()*0.001f;
altb2 0:a479dc61e931 379 my_logger.data_vector[14]=ahrs.xyzLIDAR[2];
altb2 0:a479dc61e931 380 //my_logger.data_vector[22]=ahrs.xyzAS[2];
altb2 0:a479dc61e931 381 if(optical_flow.update_integral()) {
altb2 0:a479dc61e931 382 ahrs.v_xyzOF[0]= vel_cntrl_x_rolloff(-optical_flow.avg_flowx()*0.001f + ahrs.raw_gy2gy(ahrs.imu.gyroY) * ahrs.xyzLIDAR[2]);
altb2 0:a479dc61e931 383 ahrs.v_xyzOF[1]= vel_cntrl_y_rolloff( optical_flow.avg_flowy()*0.001f - ahrs.raw_gx2gx(ahrs.imu.gyroX) * ahrs.xyzLIDAR[2]);
altb2 0:a479dc61e931 384 ahrs.xyzOF[2]= 0.0f;//((float)optical_flow.ground_distance())*0.001f;
altb2 0:a479dc61e931 385 my_logger.data_vector[10]=ahrs.v_xyzOF[0];
altb2 0:a479dc61e931 386 my_logger.data_vector[11]=ahrs.v_xyzOF[1];
altb2 0:a479dc61e931 387 my_logger.data_vector[13]=(float)optical_flow.avg_scaled_quality();
altb2 0:a479dc61e931 388 my_logger.data_vector[28]=optical_flow.gyro_x_rate();
altb2 0:a479dc61e931 389 my_logger.data_vector[29]=optical_flow.gyro_y_rate();
altb2 0:a479dc61e931 390 my_logger.data_vector[30]=optical_flow.gyro_z_rate();
altb2 0:a479dc61e931 391 }
altb2 0:a479dc61e931 392 }
altb2 0:a479dc61e931 393 }
altb2 0:a479dc61e931 394
altb2 0:a479dc61e931 395
altb2 0:a479dc61e931 396 /* ****************************************************************************
altb2 0:a479dc61e931 397 void RPY_controller(float *des_values, uint8_t rate_and_att,float* Mxyz){
altb2 0:a479dc61e931 398 Roll/Pitch/Yaw Controller,
altb2 0:a479dc61e931 399 INPUT:
altb2 0:a479dc61e931 400 des_values desired angle ( mode stabilized aso), rotation rates (acro)
altb2 0:a479dc61e931 401 rate_and_att logic for flight mode (1 = cascade control)
altb2 0:a479dc61e931 402 *Mxyz resulting values
altb2 0:a479dc61e931 403 ***************************************************************************** */
altb2 0:a479dc61e931 404 void RPY_controller(float *des_values, uint8_t rate_and_att,uint8_t filtertype,float* Mxyz){
altb2 0:a479dc61e931 405 float rate_des[4];
altb2 0:a479dc61e931 406 //--------------------------------
altb2 0:a479dc61e931 407 if(rate_and_att ==1){ // this is for Mode STABILIZED (and Loiter aso)
altb2 0:a479dc61e931 408 rate_des[1] = att_cntrl.P_Roll * (des_values[0] - ahrs.getRoll(filtertype));
altb2 0:a479dc61e931 409 rate_des[2] = att_cntrl.P_Pitch * (des_values[1] - ahrs.getPitch(filtertype));
altb2 0:a479dc61e931 410 rate_des[3] = des_values[2];//att_cntrl.P_Yaw * (des_values[2] - ahrs.getYawRadians());
altb2 0:a479dc61e931 411 }
altb2 0:a479dc61e931 412 else{ // this is for Mode ACRO
altb2 0:a479dc61e931 413 rate_des[1] = des_values[0];
altb2 0:a479dc61e931 414 rate_des[2] = des_values[1];
altb2 0:a479dc61e931 415 rate_des[3] = des_values[2];
altb2 0:a479dc61e931 416 }
altb2 0:a479dc61e931 417 // Mxyz[0]= rate_cntrl_Roll_rolloff( rate_cntrl_Roll(rate_des[1] - ahrs.raw_gx2gx(ahrs.imu.gyroX)) );
altb2 0:a479dc61e931 418 // Mxyz[1]= rate_cntrl_Pitch_rolloff( rate_cntrl_Pitch(rate_des[2] - ahrs.raw_gy2gy(ahrs.imu.gyroY)) );
altb2 0:a479dc61e931 419 float gx = ahrs.raw_gx2gx(ahrs.imu.gyroX);
altb2 0:a479dc61e931 420 float gy = ahrs.raw_gy2gy(ahrs.imu.gyroY);
altb2 0:a479dc61e931 421 Mxyz[0]= rate_cntrl_Roll(rate_des[1] - gx, gx);
altb2 0:a479dc61e931 422 Mxyz[1]= rate_cntrl_Pitch(rate_des[2] - gy, gy);
altb2 0:a479dc61e931 423 Mxyz[2]= rate_cntrl_Yaw(rate_des[3] - ahrs.raw_gz2gz(ahrs.imu.gyroZ));
altb2 0:a479dc61e931 424 }
altb2 0:a479dc61e931 425 // *****************************************************************************
altb2 0:a479dc61e931 426 // *****************************************************************************
altb2 0:a479dc61e931 427 float limit_minmax(float val,float mi,float ma){
altb2 0:a479dc61e931 428 if(val < mi)
altb2 0:a479dc61e931 429 val = mi;
altb2 0:a479dc61e931 430 if(val > ma)
altb2 0:a479dc61e931 431 val = ma;
altb2 0:a479dc61e931 432 return val;
altb2 0:a479dc61e931 433 }
altb2 0:a479dc61e931 434
altb2 0:a479dc61e931 435 // *****************************************************************************
altb2 0:a479dc61e931 436 // Dead zone, from lo ... hi
altb2 0:a479dc61e931 437 float deadzone(float x,float lo,float hi){
altb2 0:a479dc61e931 438 return (x>hi)*(x-hi)+(x<lo)*(x-lo);
altb2 0:a479dc61e931 439 }
altb2 0:a479dc61e931 440
altb2 0:a479dc61e931 441 // *****************************************************************************
altb2 0:a479dc61e931 442 // nonlinear characteristics for thrust
altb2 0:a479dc61e931 443 float PM1_2_F_Thrust(float x)
altb2 0:a479dc61e931 444 {
altb2 0:a479dc61e931 445 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);
altb2 0:a479dc61e931 446 }
altb2 0:a479dc61e931 447
altb2 0:a479dc61e931 448
altb2 0:a479dc61e931 449 void sendSignal_mainLoop(void){
altb2 0:a479dc61e931 450 thread_mainLoop.signal_set(signal_mainLoop);
altb2 0:a479dc61e931 451 }
altb2 0:a479dc61e931 452 void sendSignal_read_US_OF_sensors(void){
altb2 0:a479dc61e931 453 thread_read_US_OF_sensors.signal_set(signal_read_US_OF_sensors);
altb2 0:a479dc61e931 454 }
altb2 0:a479dc61e931 455
altb2 0:a479dc61e931 456 void sendSignal_UAV_state(void){
altb2 0:a479dc61e931 457 thread_UAV_state.signal_set(signal_UAV_state);
altb2 0:a479dc61e931 458 }
altb2 0:a479dc61e931 459 void sendSignal_Output(void){
altb2 0:a479dc61e931 460 thread_Output.signal_set(signal_Output);
altb2 0:a479dc61e931 461 }
altb2 0:a479dc61e931 462
altb2 0:a479dc61e931 463 /*
altb2 0:a479dc61e931 464 Thread thread_mainLoop(osPriorityNormal, 4096);
altb2 0:a479dc61e931 465 Thread thread_AHRS(osPriorityBelowNormal, 4096);
altb2 0:a479dc61e931 466 Thread thread_UAV_state(osPriorityLow, 4096);
altb2 0:a479dc61e931 467 Thread thread_Output(osPriorityIdle, 4096);
altb2 0:a479dc61e931 468
altb2 0:a479dc61e931 469 Signal signal_mainLoop;
altb2 0:a479dc61e931 470 Signal signal_AHRS;
altb2 0:a479dc61e931 471 Signal signal_UAV_state;
altb2 0:a479dc61e931 472 Signal signal_Output;
altb2 0:a479dc61e931 473
altb2 0:a479dc61e931 474 Ticker AHRS; // Madgwick filter for AHRS
altb2 0:a479dc61e931 475 Ticker mainLoop; // Main loop at 200Hz
altb2 0:a479dc61e931 476 Ticker UAV_state; // check if UAV crashed / flying / landed etc.
altb2 0:a479dc61e931 477 Ticker Output; // Output loop at 10Hz (Writing and saving
altb2 0:a479dc61e931 478 */