1 | void configure_for_INT2_PID_stabi_ctrl(const Word INT2_interval_ms)
|
2 | {
|
3 | Word Gyro_Output_Rate_Hz;
|
4 | Bool on_off;
|
5 |
|
6 | on_off = (INT2_interval_ms > 0);
|
7 |
|
8 | memset((void *) &MPU9150_Registers, 0, sizeof (MPU9150_Registers));
|
9 |
|
10 | //OPTION we can play with Bandwidth
|
11 | MPU9150_Registers._0x1A_Config.EXT_SYNC_SET = no;
|
12 | MPU9150_Registers._0x1A_Config.DLPF_CFG = DLPF_20_kHz;
|
13 |
|
14 | if (MPU9150_Registers._0x1A_Config.DLPF_CFG > DLPF_off)
|
15 | {
|
16 | Gyro_Output_Rate_Hz = 1000; // if filter is on - 1 kHz
|
17 | }
|
18 | else
|
19 | {
|
20 | Gyro_Output_Rate_Hz = 8000; // without filter - 8 kHz
|
21 | }
|
22 |
|
23 | MPU9150_Registers._0x19_Sample_rate_divider = (Gyro_Output_Rate_Hz / (1000.0/INT2_interval_ms) - 1);
|
24 |
|
25 | //OPTION we can play with full scale range
|
26 | MPU9150_Registers._0x1B_Gyro_config.FS_SEL = _2000_Grad_pro_Sekunde;
|
27 | switch (MPU9150_Registers._0x1B_Gyro_config.FS_SEL)
|
28 | {
|
29 | case _250_Grad_pro_Sekunde : MPU9150_Data.OMEGA_scale_factor = 131.0; break;
|
30 | case _500_Grad_pro_Sekunde : MPU9150_Data.OMEGA_scale_factor = 65.5; break;
|
31 | case _1000_Grad_pro_Sekunde : MPU9150_Data.OMEGA_scale_factor = 32.8; break;
|
32 | case _2000_Grad_pro_Sekunde : MPU9150_Data.OMEGA_scale_factor = 16.4; break;
|
33 | };
|
34 |
|
35 | MPU9150_Registers._0x1C_Accel_config.AFS_SEL = _scale_16g;
|
36 | switch (MPU9150_Registers._0x1C_Accel_config.AFS_SEL)
|
37 | {
|
38 | case _scale_2g : MPU9150_Data.ACC_scale_factor = 16384.0; break;
|
39 | case _scale_6g : MPU9150_Data.ACC_scale_factor = 8192.0; break;
|
40 | case _scale_8g : MPU9150_Data.ACC_scale_factor = 4096.0; break;
|
41 | case _scale_16g : MPU9150_Data.ACC_scale_factor = 2048.0; break;
|
42 | };
|
43 |
|
44 | MPU9150_Registers._0x1C_Accel_config.ACCEL_HPF = no;
|
45 |
|
46 | MPU9150_Registers._0x24_I2C_master_control.WAIT_FOR_ES = no;
|
47 | MPU9150_Registers._0x24_I2C_master_control.I2C_MST_CLK = _400kHz;
|
48 |
|
49 | // I2C Slave 0 AK8975C Magnetometer read out data
|
50 | MPU9150_Registers._0x25_Slave0._7bit_address = AK8975C_7bit_address;
|
51 | MPU9150_Registers._0x25_Slave0._RW = I2C_read;
|
52 | MPU9150_Registers._0x26_Slave0.Reg_address = AK8975C_Status1_address;
|
53 | MPU9150_Registers._0x27_Slave0.Lenght = 8;
|
54 | MPU9150_Registers._0x27_Slave0.Enable = yes;
|
55 |
|
56 | // I2C Slave 1 AK8975C Magnetometer write/start new meassurement
|
57 | MPU9150_Registers._0x28_Slave1._7bit_address = AK8975C_7bit_address;
|
58 | MPU9150_Registers._0x28_Slave1._RW = I2C_write;
|
59 | MPU9150_Registers._0x29_Slave1.Reg_address = AK8975C_cntrl2_address;
|
60 | MPU9150_Registers._0x64_Slave1_Data_Out = single_meass_mode;
|
61 | MPU9150_Registers._0x2A_Slave1.Lenght = 1;
|
62 | MPU9150_Registers._0x2A_Slave1.Enable = yes;
|
63 |
|
64 | MPU9150_Registers._0x38_INT_enable.DATA_RDY_EN = yes;
|
65 | MPU9150_Registers._0x38_INT_enable.I2C_MST_INT_EN = yes;
|
66 |
|
67 | MPU9150_Registers._0x6A_User_control.I2C_MST_EN = 1;
|
68 |
|
69 | if (on_off) // turn on
|
70 | {
|
71 | MPU9150_Registers._0x6B_Power_management_1.CLKSEL = PLL_with_X_axis;
|
72 | MPU9150_Registers._0x6B_Power_management_1.SLEEP = no;
|
73 | }
|
74 | else // turn off
|
75 | {
|
76 | MPU9150_Registers._0x6B_Power_management_1.CLKSEL = PLL_Stop;
|
77 | MPU9150_Registers._0x6B_Power_management_1.SLEEP = yes;
|
78 | }
|
79 | MPU9150_Registers._0x6B_Power_management_1.CYCLE = no;
|
80 |
|
81 | burst_write_MPU9150(0x19, (Byte*)&MPU9150_Registers._0x19_Sample_rate_divider, 27); // von 0x19 - 0x34
|
82 | burst_write_MPU9150(0x37, (Byte*)&MPU9150_Registers._0x37_INT_pin_configuration, 2); // 0x37 und 0x38
|
83 | burst_write_MPU9150(0x63, (Byte*)&MPU9150_Registers._0x63_Slave0_Data_Out, 10); // 0x63 und 0x6C
|
84 |
|
85 | //INT2
|
86 | INTCON2bits.INT2EP = 1; // interrupt on negative edge
|
87 | IFS1bits.INT2IF = clear; // Reset INT1 interrupt flag
|
88 | IPC7bits.INT2IP = priority_Int2;
|
89 | IEC1bits.INT2IE = on_off; // Enable INT1 Interrupt Service Routine
|
90 |
|
91 | INT2_dt = 1;
|
92 | set_PID_factors_XY (35.00, 00.00, 9.00); // PD regler
|
93 | set_PID_factors_Z (08.40, 04.00, 00.00); // PI regler
|
94 | set_PID_timing_XYZ (00.90, INT2_interval_ms); // T, PR1
|
95 |
|
96 | }
|
