Chris Elsholz / Mbed 2 deprecated Quadrocopter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed TextLCD

Fork of Quadrocopter by Marco Friedmann

messen/messen.cpp@12:4a4dad7a3432, 2017-08-16 (annotated)

Committer:
MarcoF89
Date:
Wed Aug 16 09:45:50 2017 +0000
Revision:
12:4a4dad7a3432
Parent:
7:a54c97795013
Child:
13:5f0a2103c707
Taste zwei beim einschalten gedr?ckt halten --->

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarcoF89 4:3eaf38e4809f 1 #include "mbed.h"
MarcoF89 4:3eaf38e4809f 2 #include "stdio.h"
MarcoF89 4:3eaf38e4809f 3
MarcoF89 12:4a4dad7a3432 4
MarcoF89 6:27a09e8bebfb 5 int16_t high, low;
MarcoF89 4:3eaf38e4809f 6
MarcoF89 4:3eaf38e4809f 7 extern Serial pc(SERIAL_TX, SERIAL_RX);
MarcoF89 4:3eaf38e4809f 8 extern SPI spi(PE_6,PE_5,PE_2); //mosi,miso,sclk
MarcoF89 4:3eaf38e4809f 9 extern DigitalOut ncs(PE_4); //ssel
MarcoF89 4:3eaf38e4809f 10
MarcoF89 7:a54c97795013 11 extern AnalogIn potis_1 (PF_3);
MarcoF89 7:a54c97795013 12 extern AnalogIn potis_2 (PF_10);
MarcoF89 7:a54c97795013 13 extern AnalogIn potis_3 (PF_4);
MarcoF89 7:a54c97795013 14 extern AnalogIn potis_4 (PF_5);
MarcoF89 7:a54c97795013 15
MarcoF89 12:4a4dad7a3432 16 extern PwmOut Motor1 (PC_8); // Schwarz QBRAIN: rot
MarcoF89 12:4a4dad7a3432 17 extern PwmOut Motor2 (PC_9); // Weiß orange
MarcoF89 12:4a4dad7a3432 18 extern PwmOut Motor3 (PC_6); // Grau weiß
MarcoF89 12:4a4dad7a3432 19 extern PwmOut Motor4 (PB_9); // Blau braun
MarcoF89 12:4a4dad7a3432 20 // Gelb und Orange Vcc +5V
MarcoF89 12:4a4dad7a3432 21 // Gnd Rot
MarcoF89 12:4a4dad7a3432 22
MarcoF89 12:4a4dad7a3432 23
MarcoF89 7:a54c97795013 24
MarcoF89 12:4a4dad7a3432 25
MarcoF89 12:4a4dad7a3432 26 static int n1, n2, n3, n4;
MarcoF89 7:a54c97795013 27
chriselsholz 5:584acd257531 28 /**********************/
chriselsholz 5:584acd257531 29 /*Initialisieren**Gyro*/
chriselsholz 5:584acd257531 30 /**********************/
MarcoF89 12:4a4dad7a3432 31 void initialisierung_gyro()
chriselsholz 5:584acd257531 32 {
chriselsholz 5:584acd257531 33 spi.format(8,0);
chriselsholz 5:584acd257531 34 spi.frequency(1000000);
chriselsholz 5:584acd257531 35
chriselsholz 5:584acd257531 36 ncs = 0;
chriselsholz 5:584acd257531 37 spi.write(0x6B); // Register 107
chriselsholz 5:584acd257531 38 spi.write(0x80); //Reset // Standby off
chriselsholz 5:584acd257531 39 ncs = 1;
chriselsholz 5:584acd257531 40 wait_ms(1000);
chriselsholz 5:584acd257531 41
chriselsholz 5:584acd257531 42
chriselsholz 5:584acd257531 43 ncs=0;
chriselsholz 5:584acd257531 44 spi.write(0x1A); //CONFIG write // DLPF_CFG // Register 26
chriselsholz 5:584acd257531 45 spi.write(0x06); //Bandwidth: 250Hz// Delay: 0.97ms// Fs: 8kHz
chriselsholz 5:584acd257531 46 ncs = 1;
chriselsholz 5:584acd257531 47 wait_ms(1);
chriselsholz 5:584acd257531 48
chriselsholz 5:584acd257531 49 ncs=0;
chriselsholz 5:584acd257531 50 spi.write(0x1B); //Gyro_CONFIG write
chriselsholz 5:584acd257531 51 spi.write(0x18); //Max. Skalenwert//00=+250dps;08=+500dps;10=+1000dps;18=+2000dps
chriselsholz 5:584acd257531 52 ncs = 1;
chriselsholz 5:584acd257531 53 wait_ms(1);
chriselsholz 5:584acd257531 54
chriselsholz 5:584acd257531 55 ncs = 0;
chriselsholz 5:584acd257531 56 spi.write(0x17); // Register 23
MarcoF89 6:27a09e8bebfb 57 spi.write(0x00); // Offset High Byte Z
chriselsholz 5:584acd257531 58 ncs = 1;
chriselsholz 5:584acd257531 59 wait_ms(1);
chriselsholz 5:584acd257531 60
chriselsholz 5:584acd257531 61 ncs = 0;
chriselsholz 5:584acd257531 62 spi.write(0x18); // Register 24
MarcoF89 6:27a09e8bebfb 63 spi.write(0x17); // Offset Low Byte Z
chriselsholz 5:584acd257531 64 ncs = 1;
chriselsholz 5:584acd257531 65 wait_ms(1000);
chriselsholz 5:584acd257531 66 }
chriselsholz 5:584acd257531 67
chriselsholz 5:584acd257531 68 /**********************/
chriselsholz 5:584acd257531 69 /*Initialisieren Acc */
chriselsholz 5:584acd257531 70 /**********************/
chriselsholz 5:584acd257531 71
chriselsholz 5:584acd257531 72 int16_t initialisierung_acc ()
chriselsholz 5:584acd257531 73 {
chriselsholz 5:584acd257531 74 int i,faktor = 0x00;
chriselsholz 5:584acd257531 75 for(i=0;i<=2;i++)
chriselsholz 5:584acd257531 76 {
chriselsholz 5:584acd257531 77 ncs=0;
chriselsholz 5:584acd257531 78 spi.write(0x1c); //ACC_CONFIG write
chriselsholz 5:584acd257531 79 spi.write(faktor); //Skalierung 00=2g;08=4g;10=8g;18=16g
chriselsholz 5:584acd257531 80 ncs=1; //Teilung 16384;8192;4096;2048
chriselsholz 5:584acd257531 81 wait_us(0.1);
chriselsholz 5:584acd257531 82
chriselsholz 5:584acd257531 83 ncs=0;
chriselsholz 5:584acd257531 84 spi.write(0x1d); //ACC_CONFIG_2 08=460Hz;09=184Hz;0a=92Hz
MarcoF89 6:27a09e8bebfb 85 spi.write(0x0e); //TP-Filter 0b=41Hz;0c=20Hz;0d=10Hz;0e=5Hz
chriselsholz 5:584acd257531 86 ncs=1;
chriselsholz 5:584acd257531 87 wait_us(0.1);
chriselsholz 5:584acd257531 88 }
chriselsholz 5:584acd257531 89 switch (faktor)
chriselsholz 5:584acd257531 90 {
chriselsholz 5:584acd257531 91 case 0x00: return 16384; break;
chriselsholz 5:584acd257531 92 case 0x08: return 8192; break;
chriselsholz 5:584acd257531 93 case 0x10: return 4096; break;
chriselsholz 5:584acd257531 94 case 0x18: return 2048; break;
chriselsholz 5:584acd257531 95 }
chriselsholz 5:584acd257531 96 }
chriselsholz 5:584acd257531 97
chriselsholz 5:584acd257531 98
chriselsholz 5:584acd257531 99 /***************/
chriselsholz 5:584acd257531 100 /*Messen Gyro Z*/
chriselsholz 5:584acd257531 101 /***************/
MarcoF89 4:3eaf38e4809f 102
MarcoF89 6:27a09e8bebfb 103 int16_t aktuell_gyro_z()
MarcoF89 6:27a09e8bebfb 104 {
MarcoF89 4:3eaf38e4809f 105 ncs = 0;
MarcoF89 4:3eaf38e4809f 106 spi.write(0xc7); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 107 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 108 ncs = 1;
MarcoF89 6:27a09e8bebfb 109 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 110
MarcoF89 6:27a09e8bebfb 111 ncs = 0;
MarcoF89 6:27a09e8bebfb 112 spi.write(0xc8); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 113 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 114 ncs = 1;
MarcoF89 6:27a09e8bebfb 115 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 116 return (low | high << 8);
MarcoF89 6:27a09e8bebfb 117 }
MarcoF89 6:27a09e8bebfb 118
MarcoF89 6:27a09e8bebfb 119 /***************/
MarcoF89 6:27a09e8bebfb 120 /*Messen Gyro X*/
MarcoF89 6:27a09e8bebfb 121 /***************/
MarcoF89 6:27a09e8bebfb 122
MarcoF89 6:27a09e8bebfb 123 int16_t aktuell_gyro_x()
MarcoF89 6:27a09e8bebfb 124 {
MarcoF89 6:27a09e8bebfb 125 ncs = 0;
MarcoF89 6:27a09e8bebfb 126 spi.write(0xc3); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 127 high = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 128 ncs = 1;
MarcoF89 4:3eaf38e4809f 129 wait_us(1);
MarcoF89 4:3eaf38e4809f 130
MarcoF89 4:3eaf38e4809f 131 ncs = 0;
MarcoF89 6:27a09e8bebfb 132 spi.write(0xc4); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 133 low = spi.write(0x0);
MarcoF89 4:3eaf38e4809f 134 ncs = 1;
MarcoF89 6:27a09e8bebfb 135 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 136 return low | high << 8;;
MarcoF89 6:27a09e8bebfb 137 }
MarcoF89 6:27a09e8bebfb 138
MarcoF89 6:27a09e8bebfb 139 /***************/
MarcoF89 6:27a09e8bebfb 140 /*Messen Gyro Y*/
MarcoF89 6:27a09e8bebfb 141 /***************/
MarcoF89 6:27a09e8bebfb 142
MarcoF89 6:27a09e8bebfb 143 int16_t aktuell_gyro_y()
MarcoF89 6:27a09e8bebfb 144 {
MarcoF89 6:27a09e8bebfb 145 ncs = 0;
MarcoF89 6:27a09e8bebfb 146 spi.write(0xc5); //Z_OUT_H
MarcoF89 6:27a09e8bebfb 147 high = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 148 ncs = 1;
MarcoF89 6:27a09e8bebfb 149 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 150
MarcoF89 6:27a09e8bebfb 151 ncs = 0;
MarcoF89 6:27a09e8bebfb 152 spi.write(0xc6); //Z_OUT_L
MarcoF89 6:27a09e8bebfb 153 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 154 ncs = 1;
MarcoF89 6:27a09e8bebfb 155 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 156 return low | high << 8;
MarcoF89 4:3eaf38e4809f 157 }
MarcoF89 4:3eaf38e4809f 158
chriselsholz 5:584acd257531 159 /************/
chriselsholz 5:584acd257531 160 /*Messen Acc*/
chriselsholz 5:584acd257531 161 /************/
MarcoF89 4:3eaf38e4809f 162
chriselsholz 5:584acd257531 163 int16_t aktuell_acc_x ()
chriselsholz 5:584acd257531 164 {
MarcoF89 4:3eaf38e4809f 165 ncs=0;
chriselsholz 5:584acd257531 166 spi.write(0xbb);
MarcoF89 6:27a09e8bebfb 167 high = spi.write(0x0);
chriselsholz 5:584acd257531 168 ncs=1;
chriselsholz 5:584acd257531 169 wait_us(0.1);
chriselsholz 5:584acd257531 170
chriselsholz 5:584acd257531 171 ncs=0;
chriselsholz 5:584acd257531 172 spi.write(0xbc);
MarcoF89 6:27a09e8bebfb 173 low = spi.write(0x0);
chriselsholz 5:584acd257531 174 ncs=1;
MarcoF89 6:27a09e8bebfb 175 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 176 return low | high<<8;
chriselsholz 5:584acd257531 177 }
MarcoF89 4:3eaf38e4809f 178
chriselsholz 5:584acd257531 179 int16_t aktuell_acc_y ()
chriselsholz 5:584acd257531 180 {
chriselsholz 5:584acd257531 181 ncs=0;
chriselsholz 5:584acd257531 182 spi.write(0xbd);
MarcoF89 6:27a09e8bebfb 183 high = spi.write(0x0);
chriselsholz 5:584acd257531 184 ncs=1;
chriselsholz 5:584acd257531 185 wait_us(0.1);
chriselsholz 5:584acd257531 186
chriselsholz 5:584acd257531 187 ncs=0;
chriselsholz 5:584acd257531 188 spi.write(0xbe);
MarcoF89 6:27a09e8bebfb 189 low = spi.write(0x0);
chriselsholz 5:584acd257531 190 ncs=1;
MarcoF89 6:27a09e8bebfb 191 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 192 return low | high<<8;
chriselsholz 5:584acd257531 193 }
MarcoF89 4:3eaf38e4809f 194
chriselsholz 5:584acd257531 195 int16_t aktuell_acc_z ()
chriselsholz 5:584acd257531 196 {
chriselsholz 5:584acd257531 197 ncs=0;
chriselsholz 5:584acd257531 198 spi.write(0xbf);
MarcoF89 6:27a09e8bebfb 199 high = spi.write(0x0);
chriselsholz 5:584acd257531 200 ncs=1;
chriselsholz 5:584acd257531 201 wait_us(0.1);
chriselsholz 5:584acd257531 202
chriselsholz 5:584acd257531 203 ncs=0;
chriselsholz 5:584acd257531 204 spi.write(0xc0);
MarcoF89 6:27a09e8bebfb 205 low = spi.write(0x0);
MarcoF89 6:27a09e8bebfb 206 ncs=1;
MarcoF89 6:27a09e8bebfb 207 wait_us(0.1);
MarcoF89 6:27a09e8bebfb 208 return low | high<<8;
chriselsholz 5:584acd257531 209 }
MarcoF89 12:4a4dad7a3432 210
MarcoF89 12:4a4dad7a3432 211