├── .gitignore ├── Configer ├── configer │ ├── stdafx.h │ ├── configer.aps │ ├── configer.cpp │ ├── configer.h │ ├── configer.rc │ ├── resource.h │ ├── stdafx.cpp │ ├── targetver.h │ ├── configerDlg.h │ ├── configerDlg.cpp │ ├── res │ │ ├── configer.ico │ │ └── configer.rc2 │ ├── configer.vcxproj.filters │ ├── ReadMe.txt │ └── configer.vcxproj └── configer.sln ├── RemoteControl ├── Untitled Sketch.fzz ├── buttn.h ├── stick.h ├── connect.h ├── leds.h ├── lcd.h └── RemoteControl.ino ├── Aircraft ├── barometer.h ├── power.h ├── sonar.h ├── compass.h ├── config.h ├── connect.h ├── AdjustMotor │ └── AdjustMotor.ino ├── pid.h ├── ypr.h ├── control.h └── Aircraft.ino ├── libraries ├── Mirf │ ├── MirfSpiDriver.cpp │ ├── MirfSpiDriver.h │ ├── MirfHardwareSpiDriver.cpp │ ├── MirfHardwareSpiDriver.h │ ├── keywords │ ├── examples │ │ ├── reg_read │ │ │ └── reg_read.pde │ │ ├── ping_server │ │ │ └── ping_server.pde │ │ ├── ping_client │ │ │ └── ping_client.pde │ │ └── ping_server_interupt │ │ │ └── ping_server_interupt.pde │ ├── Mirf.h │ ├── nRF24L01.h │ └── Mirf.cpp ├── NewPing │ ├── keywords.txt │ ├── NewPing.h │ └── NewPing.cpp ├── ShiftRegLCD │ ├── examples │ │ ├── SerialDisplay │ │ │ └── SerialDisplay.pde │ │ ├── CustomChar │ │ │ └── CustomChar.pde │ │ ├── CustomChar2 │ │ │ └── CustomChar2.pde │ │ ├── HelloWorld │ │ │ └── HelloWorld.pde │ │ └── HelloWorld2 │ │ │ └── HelloWorld2.pde │ ├── keywords.txt │ ├── ShiftRegLCD.h │ └── ShiftRegLCD.cpp ├── NetDefine │ └── NetDefine.h ├── HMC5883L │ ├── Examples │ │ └── HMC5883L_raw │ │ │ └── HMC5883L_raw.ino │ ├── HMC5883L.h │ └── HMC5883L.cpp ├── BMP085 │ ├── Examples │ │ └── BMP085_basic │ │ │ └── BMP085_basic.ino │ ├── BMP085.h │ └── BMP085.cpp ├── MPU6050 │ ├── Examples │ │ ├── MPU6050_raw │ │ │ └── MPU6050_raw.ino │ │ └── MPU6050_DMP6 │ │ │ └── Processing │ │ │ └── MPUTeapot.pde │ └── helper_3dmath.h └── I2Cdev │ └── I2Cdev.h └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | -------------------------------------------------------------------------------- /Configer/configer/stdafx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/stdafx.h -------------------------------------------------------------------------------- /Configer/configer/configer.aps: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/configer.aps -------------------------------------------------------------------------------- /Configer/configer/configer.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/configer.cpp -------------------------------------------------------------------------------- /Configer/configer/configer.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/configer.h -------------------------------------------------------------------------------- /Configer/configer/configer.rc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/configer.rc -------------------------------------------------------------------------------- /Configer/configer/resource.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/resource.h -------------------------------------------------------------------------------- /Configer/configer/stdafx.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/stdafx.cpp -------------------------------------------------------------------------------- /Configer/configer/targetver.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/targetver.h -------------------------------------------------------------------------------- /Configer/configer/configerDlg.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/configerDlg.h -------------------------------------------------------------------------------- /Configer/configer/configerDlg.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/configerDlg.cpp -------------------------------------------------------------------------------- /Configer/configer/res/configer.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/res/configer.ico -------------------------------------------------------------------------------- /Configer/configer/res/configer.rc2: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/Configer/configer/res/configer.rc2 -------------------------------------------------------------------------------- /RemoteControl/Untitled Sketch.fzz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/zhang-heng/Pterosauria/HEAD/RemoteControl/Untitled Sketch.fzz -------------------------------------------------------------------------------- /Aircraft/barometer.h: -------------------------------------------------------------------------------- 1 | //气压传感器(BMP180) 2 | class Cbarometer 3 | { 4 | public : 5 | Cbarometer(){ 6 | } 7 | float GetPoint(){ 8 | } 9 | }; 10 | -------------------------------------------------------------------------------- /libraries/Mirf/MirfSpiDriver.cpp: -------------------------------------------------------------------------------- 1 | #include "MirfSpiDriver.h" 2 | 3 | uint8_t MirfSpiDriver::transfer(uint8_t data){ 4 | return 0; 5 | } 6 | 7 | void MirfSpiDriver::begin(){ 8 | } 9 | 10 | void MirfSpiDriver::end(){ 11 | } 12 | -------------------------------------------------------------------------------- /Aircraft/power.h: -------------------------------------------------------------------------------- 1 | class Cpower 2 | { 3 | public: 4 | Cpower(int pin){ 5 | m_pin = pin; 6 | } 7 | float GetVoltage(){ 8 | v = analogRead(m_pin) * 4 * 5 / 1024; 9 | return map(v, 9, 12.6, 0, 100); 10 | } 11 | private: 12 | float v ; 13 | int m_pin; 14 | }; 15 | -------------------------------------------------------------------------------- /Aircraft/sonar.h: -------------------------------------------------------------------------------- 1 | class Csonar 2 | { 3 | private: 4 | NewPing * m_sonar; 5 | public: 6 | Csonar(){ 7 | int trigPin = A6; 8 | int echoPin = A7; 9 | m_sonar = new NewPing(trigPin, echoPin); 10 | } 11 | int GetDistance(){ 12 | unsigned int uS = m_sonar->ping(); 13 | return uS / US_ROUNDTRIP_CM; 14 | } 15 | }; 16 | -------------------------------------------------------------------------------- /libraries/Mirf/MirfSpiDriver.h: -------------------------------------------------------------------------------- 1 | #ifndef __MIRF_SPI_DRIVER 2 | #define __MIRF_SPI_DRIVER 3 | 4 | extern "C" { 5 | #include 6 | #include 7 | } 8 | 9 | class MirfSpiDriver { 10 | public: 11 | virtual uint8_t transfer(uint8_t data); 12 | 13 | virtual void begin(); 14 | virtual void end(); 15 | }; 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /libraries/Mirf/MirfHardwareSpiDriver.cpp: -------------------------------------------------------------------------------- 1 | #include "MirfHardwareSpiDriver.h" 2 | uint8_t MirfHardwareSpiDriver::transfer(uint8_t data){ 3 | return SPI.transfer(data); 4 | } 5 | 6 | void MirfHardwareSpiDriver::begin(){ 7 | SPI.begin(); 8 | SPI.setDataMode(SPI_MODE0); 9 | SPI.setClockDivider(SPI_2XCLOCK_MASK); 10 | } 11 | 12 | void MirfHardwareSpiDriver::end(){ 13 | } 14 | 15 | MirfHardwareSpiDriver MirfHardwareSpi; 16 | -------------------------------------------------------------------------------- /libraries/Mirf/MirfHardwareSpiDriver.h: -------------------------------------------------------------------------------- 1 | #include "MirfSpiDriver.h" 2 | 3 | #ifndef __MIRF_HARDWARE_SPI_DRIVER 4 | #define __MIRF_HARDWARE_SPI_DRIVER 5 | 6 | #include 7 | 8 | class MirfHardwareSpiDriver : public MirfSpiDriver { 9 | 10 | public: 11 | virtual uint8_t transfer(uint8_t data); 12 | virtual void begin(); 13 | virtual void end(); 14 | }; 15 | 16 | extern MirfHardwareSpiDriver MirfHardwareSpi; 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /libraries/NewPing/keywords.txt: -------------------------------------------------------------------------------- 1 | ################################### 2 | # Syntax Coloring Map For NewPing 3 | ################################### 4 | 5 | ################################### 6 | # Datatypes (KEYWORD1) 7 | ################################### 8 | 9 | NewPing KEYWORD1 10 | 11 | ################################### 12 | # Methods and Functions (KEYWORD2) 13 | ################################### 14 | 15 | ping KEYWORD2 16 | ping_in KEYWORD2 17 | ping_cm KEYWORD2 18 | ping_median KEYWORD2 19 | ping_timer KEYWORD2 20 | check_timer KEYWORD2 21 | timer_us KEYWORD2 22 | timer_ms KEYWORD2 23 | timer_stop KEYWORD2 24 | convert_in KEYWORD2 25 | convert_cm KEYWORD2 26 | 27 | ################################### 28 | # Constants (LITERAL1) 29 | ################################### 30 | -------------------------------------------------------------------------------- /libraries/Mirf/keywords: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For Mirf 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ####################################### 10 | # Methods and Functions (KEYWORD2) 11 | ####################################### 12 | 13 | init KEYWORD2 14 | config KEYWORD2 15 | send KEYWORD2 16 | setRADDR KEYWORD2 17 | setTADDR KEYWORD2 18 | dataReady KEYWORD2 19 | getData KEYWORD2 20 | getStatus KEYWORD2 21 | 22 | ####################################### 23 | # Instances (KEYWORD2) 24 | ####################################### 25 | 26 | Mirf KEYWORD2 27 | 28 | ####################################### 29 | # Constants (LITERAL1) 30 | ####################################### 31 | 32 | -------------------------------------------------------------------------------- /RemoteControl/buttn.h: -------------------------------------------------------------------------------- 1 | typedef struct BUTTONS 2 | { 3 | bool a1; 4 | bool a2; 5 | bool b1; 6 | bool b2; 7 | bool b3; 8 | }Buttons; 9 | 10 | class Cbuttn 11 | { 12 | public : 13 | Cbuttn (int latchPin, int dataPin, int clockPin){ 14 | m_LatchPin = latchPin; 15 | m_DataPin = dataPin; 16 | m_ClockPin = clockPin; 17 | pinMode(m_LatchPin, OUTPUT); 18 | pinMode(m_DataPin, INPUT); 19 | pinMode(m_ClockPin, OUTPUT); 20 | } 21 | 22 | void Read(Buttons * b){ 23 | digitalWrite(m_LatchPin, LOW); 24 | byte c = shiftIn(m_DataPin, m_ClockPin, MSBFIRST); 25 | digitalWrite(m_LatchPin, HIGH); 26 | b->a1 = c & 0x04; 27 | b->a2 = c & 0x02; 28 | b->b1 = !(c & 0x20); 29 | b->b2 = !(c & 0x08); 30 | b->b3 = !(c & 0x10); 31 | } 32 | 33 | private: 34 | int m_LatchPin; 35 | int m_DataPin; 36 | int m_ClockPin; 37 | }; 38 | -------------------------------------------------------------------------------- /libraries/Mirf/examples/reg_read/reg_read.pde: -------------------------------------------------------------------------------- 1 | /** 2 | * Pins: 3 | * Hardware SPI: 4 | * MISO -> 12 5 | * MOSI -> 11 6 | * SCK -> 13 7 | * 8 | * Configurable: 9 | * CE -> 8 10 | * CSN -> 7 11 | */ 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | void setup() { 19 | Serial.begin(9600); 20 | Serial.println( "Starting wireless..." ); 21 | 22 | // Setup 23 | Mirf.spi = &MirfHardwareSpi; 24 | Mirf.init(); 25 | Mirf.setRADDR((byte *)"clie1"); 26 | Mirf.payload = sizeof(unsigned long); 27 | Mirf.config(); 28 | 29 | // Read and print RF_SETUP 30 | byte rf_setup = 0; 31 | Mirf.readRegister( RF_SETUP, &rf_setup, sizeof(rf_setup) ); 32 | Serial.print( "rf_setup = " ); 33 | Serial.println( rf_setup, BIN ); 34 | 35 | Serial.println( "Wireless initialized!" ); 36 | } 37 | 38 | void loop() {} 39 | -------------------------------------------------------------------------------- /Aircraft/compass.h: -------------------------------------------------------------------------------- 1 | //电子罗盘(GY-271 HMC5883L) 2 | class Ccompass 3 | { 4 | private : 5 | HMC5883L mag; 6 | int16_t mx, my, mz; 7 | ulong m_lastTime; 8 | float m_point; 9 | 10 | public : 11 | Ccompass(){ 12 | Wire.begin(); 13 | Serial.println("HMC5883L: Initializing I2C devices..."); 14 | mag.initialize(); 15 | Serial.print("HMC5883L: Testing device connections..."); 16 | Serial.print(mag.testConnection() ? "successful\n" : "failed\n"); 17 | m_point = 0; 18 | m_lastTime = millis(); 19 | } 20 | 21 | float GetPoint(){ 22 | ulong currentTime = millis(); 23 | if(currentTime - m_lastTime > 20){ 24 | m_lastTime = currentTime; 25 | mag.getHeading(&mx, &my, &mz); 26 | float heading = atan2(my, mx); 27 | if(heading < 0) heading += 2 * M_PI; 28 | m_point = (heading * 180/M_PI); 29 | } 30 | return m_point; 31 | } 32 | 33 | }; 34 | -------------------------------------------------------------------------------- /Aircraft/config.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef struct STRUCT_CONFIG 4 | { 5 | double PitchP; 6 | double PitchI; 7 | double PitchD; 8 | 9 | double RollP; 10 | double RollI; 11 | double RollD; 12 | 13 | double YawP; 14 | double YawI; 15 | double YawD; 16 | 17 | double ElevationP; 18 | double ElevationI; 19 | double ElevationD; 20 | 21 | double AdjustPitch; 22 | double AdjustRoll; 23 | double AdjustYaw; 24 | double AdjustElevation; 25 | } StructConfig, *pStructConfig; 26 | 27 | #define ADDR_CONFIG 0x30 28 | 29 | void WriteRom(StructConfig* p){ 30 | if (p) eeprom_write_block((void*)p, (void*)ADDR_CONFIG, sizeof(StructConfig)); 31 | } 32 | 33 | void ReadRom(StructConfig* p){ 34 | if (p){ 35 | eeprom_read_block((void*)p, (void*)ADDR_CONFIG, sizeof(StructConfig)); 36 | for (int i = 0; i < sizeof(StructConfig) / 4; i++) 37 | if (isnan(*((double*)p + i))) 38 | *((double*)p + i) = 0; 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /Configer/configer.sln: -------------------------------------------------------------------------------- 1 | 2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2012 4 | Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "configer", "configer\configer.vcxproj", "{4E5BA5CD-93E9-40CB-B56F-07C14A97D424}" 5 | EndProject 6 | Global 7 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 8 | Debug|Win32 = Debug|Win32 9 | Release|Win32 = Release|Win32 10 | EndGlobalSection 11 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 12 | {4E5BA5CD-93E9-40CB-B56F-07C14A97D424}.Debug|Win32.ActiveCfg = Debug|Win32 13 | {4E5BA5CD-93E9-40CB-B56F-07C14A97D424}.Debug|Win32.Build.0 = Debug|Win32 14 | {4E5BA5CD-93E9-40CB-B56F-07C14A97D424}.Release|Win32.ActiveCfg = Release|Win32 15 | {4E5BA5CD-93E9-40CB-B56F-07C14A97D424}.Release|Win32.Build.0 = Release|Win32 16 | EndGlobalSection 17 | GlobalSection(SolutionProperties) = preSolution 18 | HideSolutionNode = FALSE 19 | EndGlobalSection 20 | EndGlobal 21 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/examples/SerialDisplay/SerialDisplay.pde: -------------------------------------------------------------------------------- 1 | // Displays text sent over the serial port (e.g. from the Serial Monitor) on 2 | // an attached LCD, using 2-wire connection to the LCD. 3 | 4 | #include 5 | 6 | const byte dataPin = 10; // SR Data from Arduino pin 10 7 | const byte clockPin = 11; // SR Clock from Arduino pin 11 8 | 9 | ShiftRegLCD srlcd(dataPin, clockPin, TWO_WIRE); 10 | 11 | void setup() 12 | { 13 | Serial.begin(9600); 14 | 15 | // Turn on LCD backlight (if used) 16 | srlcd.backlightOn(); 17 | } 18 | 19 | void loop() 20 | { 21 | // when characters arrive over the serial port... 22 | if (Serial.available()) { 23 | // wait a bit for the entire message to arrive 24 | delay(100); 25 | // clear the screen 26 | srlcd.clear(); 27 | // read all the available characters 28 | while (Serial.available() > 0) { 29 | // display each character to the LCD 30 | srlcd.write(Serial.read()); 31 | } 32 | } 33 | } 34 | 35 | -------------------------------------------------------------------------------- /Aircraft/connect.h: -------------------------------------------------------------------------------- 1 | class Cconnect 2 | { 3 | public : 4 | NetStruct netbuffer; 5 | Cconnect(int cePin, int csnPin){ 6 | //* MISO -> 12 7 | //* MOSI -> 11 8 | //* SCK -> 13 9 | 10 | Serial.print("Initializing nRF2401l"); 11 | Mirf.cePin = cePin; 12 | Mirf.csnPin = csnPin; 13 | Mirf.spi = &MirfHardwareSpi; 14 | Serial.print("."); 15 | Mirf.init(); 16 | Serial.print("."); 17 | Mirf.setRADDR((byte *)"1aircraft"); 18 | Mirf.channel = 1; 19 | Mirf.payload = sizeof(NetStruct); 20 | Mirf.config(); 21 | Serial.print("."); 22 | Mirf.setTADDR((byte *)"1controler"); 23 | Serial.print("finish! \n"); 24 | } 25 | 26 | void Read(NetStruct *pmsg){ 27 | if(!Mirf.isSending() && Mirf.dataReady()){ 28 | Mirf.getData((byte*)pmsg); 29 | } 30 | else{ 31 | memset(pmsg,0,sizeof(NetStruct)); 32 | pmsg->type = TYPE_UNKNOW; 33 | } 34 | } 35 | 36 | void Send(NetStruct *pmsg){ 37 | Mirf.send((byte*) pmsg); 38 | } 39 | }; 40 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For ShiftRegLCD 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | 9 | ShiftRegLCD KEYWORD1 10 | 11 | ####################################### 12 | # Methods and Functions (KEYWORD2) 13 | ####################################### 14 | 15 | backlightOn KEYWORD2 16 | backlightOff KEYWORD2 17 | clear KEYWORD2 18 | home KEYWORD2 19 | print KEYWORD2 20 | noDisplay KEYWORD2 21 | display KEYWORD2 22 | noBlink KEYWORD2 23 | blink KEYWORD2 24 | noCursor KEYWORD2 25 | cursor KEYWORD2 26 | scrollDisplayLeft KEYWORD2 27 | scrollDisplayRight KEYWORD2 28 | printLeft KEYWORD2 29 | printRight KEYWORD2 30 | shiftLeft KEYWORD2 31 | shiftRight KEYWORD2 32 | shiftIncrement KEYWORD2 33 | shiftDecrement KEYWORD2 34 | createChar KEYWORD2 35 | setCursor KEYWORD2 36 | 37 | ####################################### 38 | # Constants (LITERAL1) 39 | ####################################### 40 | TWO_WIRE LITERAL1 41 | 42 | -------------------------------------------------------------------------------- /Aircraft/AdjustMotor/AdjustMotor.ino: -------------------------------------------------------------------------------- 1 | #include 2 | int ServoPins[4]; 3 | int ServoMin[4]; 4 | Servo Servos[4]; 5 | 6 | void setup() { 7 | ServoPins[0] = 5; 8 | ServoPins[1] = 9; 9 | ServoPins[2] = 6; 10 | ServoPins[3] = 10; 11 | ServoMin[0] = 750; 12 | ServoMin[1] = 750; 13 | ServoMin[2] = 750; 14 | ServoMin[3] = 750; 15 | for (int i = 0; i < 4; i++) { 16 | Servos[i].attach(ServoPins[i]); 17 | Servos[i].writeMicroseconds(700); 18 | } 19 | delay(5000); 20 | Serial.begin(115200); 21 | } 22 | 23 | void loop() { 24 | switch (Serial.read()) { 25 | case '1': 26 | ServoMin[0]++;break; 27 | case 'q': 28 | ServoMin[0]--;;break; 29 | case '2': 30 | ServoMin[1]++;;break; 31 | case 'w': 32 | ServoMin[1]--;;break; 33 | case '3': 34 | ServoMin[2]++;;break; 35 | case 'e': 36 | ServoMin[2]--;;break; 37 | case '4': 38 | ServoMin[3]++;;break; 39 | case 'r': 40 | ServoMin[3]--;;break; 41 | } 42 | for (int i = 0; i < 4; i++) { 43 | Servos[i].writeMicroseconds(ServoMin[i]); 44 | Serial.print(ServoMin[i]); 45 | Serial.print("\t"); 46 | } 47 | Serial.print("\n"); 48 | delay(10); 49 | } 50 | -------------------------------------------------------------------------------- /libraries/NetDefine/NetDefine.h: -------------------------------------------------------------------------------- 1 | #define ulong unsigned long 2 | 3 | enum NetType 4 | { 5 | TYPE_UNKNOW, //无效 6 | 7 | TYPE_TIME_SYN, //对时 8 | TYPE_POWER, //电量 9 | 10 | TYPE_MOTOR_A, //电机A速度 11 | TYPE_MOTOR_B, //电机B速度 12 | TYPE_MOTOR_C, //电机C速度 13 | TYPE_MOTOR_D, //电机D速度 14 | 15 | TYPE_PITCH_DESIRED, //期望俯仰 16 | TYPE_PITCH_CURRENT, //当前俯仰 17 | TYPE_PITCH_P, 18 | TYPE_PITCH_I, 19 | TYPE_PITCH_D, 20 | 21 | TYPE_ROLL_DESIRED, //期望横滚 22 | TYPE_ROLL_CURRENT, //当前横滚 23 | TYPE_ROLL_P, 24 | TYPE_ROLL_I, 25 | TYPE_ROLL_D, 26 | 27 | TYPE_YAW_DESIRED, //期望航向 28 | TYPE_YAW_CURRENT, //当前航向 29 | TYPE_YAW_P, 30 | TYPE_YAW_I, 31 | TYPE_YAW_D, 32 | 33 | TYPE_ELEVATION_DESIRED, //期望航向 34 | TYPE_ELEVATION_CURRENT, //当前海拔 35 | TYPE_ELEVATION_P, 36 | TYPE_ELEVATION_I, 37 | TYPE_ELEVATION_D, 38 | 39 | TYPE_FLY, //解锁飞行 40 | 41 | TYPE_SELF_STATIONARY, //自稳 42 | TYPE_LAND, //降落 43 | 44 | TYPE_INIT_BALANCE, //初始化平衡 45 | 46 | TYPE_READ_CONFIG, //读取配置 47 | TYPE_WRITE_CONFIG, //写配置 48 | 49 | }; 50 | 51 | typedef struct NET_STRUCT 52 | { 53 | ulong seq; 54 | ulong time; 55 | ulong type; 56 | ulong value; 57 | } 58 | NetStruct,*pNetStruct; 59 | -------------------------------------------------------------------------------- /RemoteControl/stick.h: -------------------------------------------------------------------------------- 1 | typedef struct COORDINATE 2 | { 3 | float lx; 4 | float ly; 5 | float rx; 6 | float ry; 7 | } 8 | Coordinate, *pCoordinate; 9 | 10 | class Cstick 11 | { 12 | public : 13 | Cstick(int leftXPin, int leftYPin, int rightXPin, int rightYPin){ 14 | m_LeftXPin = leftXPin; 15 | m_LeftYPin = leftYPin; 16 | m_RightXPin = rightXPin; 17 | m_RightYPin = rightYPin; 18 | m_InitLeftX = analogRead(m_LeftXPin); 19 | m_InitLeftY = analogRead(m_LeftYPin); 20 | m_InitRightY = analogRead(m_RightYPin); 21 | m_InitRightX = analogRead(m_RightXPin); 22 | } 23 | 24 | void Read(Coordinate * c){ 25 | c->lx = -adjust(analogRead(m_LeftXPin) , m_InitLeftX); 26 | c->ly = adjust(analogRead(m_LeftYPin) , m_InitLeftY); 27 | c->rx = -adjust(analogRead(m_RightXPin) , m_InitRightX); 28 | c->ry = adjust(analogRead(m_RightYPin) , m_InitRightY); 29 | } 30 | 31 | private: 32 | int m_LeftXPin; 33 | int m_LeftYPin; 34 | int m_RightXPin; 35 | int m_RightYPin; 36 | 37 | int m_InitLeftX; 38 | int m_InitLeftY; 39 | int m_InitRightX; 40 | int m_InitRightY; 41 | 42 | int adjust(int v, int initV){ 43 | int val = v - initV; 44 | if (abs(val)<20) return 0; 45 | return map(val, -initV, 1024- initV, -100, 100); 46 | return val; 47 | } 48 | }; 49 | -------------------------------------------------------------------------------- /Aircraft/pid.h: -------------------------------------------------------------------------------- 1 | class Cpid 2 | { 3 | private: 4 | double SetPoint; //期望角 5 | double SumError; //误差累计 6 | double LastError; //Error[-1] 7 | double PrevError; //Error[-2] 8 | double Proportion; //比例常数 Proportional Const 9 | double Integral; //积分常数 Integral Const 10 | double Derivative; //微分常数 Derivative Const 11 | 12 | public : 13 | Cpid(double p, double i, double d, double point){ 14 | SumError = 0; 15 | LastError = 0; 16 | PrevError = 0; 17 | Proportion = p; 18 | Integral = i; 19 | Derivative = d; 20 | SetPoint = point; 21 | } 22 | 23 | double PIDCalc(double NextPoint){ 24 | double dError, Error; 25 | Error = SetPoint - NextPoint; // 偏差 26 | SumError += Error; // 积分 27 | dError = Error - LastError; // 当前微分 28 | PrevError = LastError; 29 | LastError = Error; 30 | if(SumError>1000) SumError = 1000; 31 | if(SumError<-1000) SumError = -1000; 32 | Serial.print(SumError); 33 | return (Proportion * Error // 比例项 34 | + Integral * SumError * 0.1 // 积分项 35 | + Derivative * dError* 10); // 微分项 36 | } 37 | 38 | void ReSetPID(double p, double i, double d){ 39 | Proportion = p; 40 | Integral = i; 41 | Derivative = d; 42 | } 43 | 44 | void ReSetPoint(double p){ 45 | SetPoint = p; 46 | } 47 | }; 48 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/examples/CustomChar/CustomChar.pde: -------------------------------------------------------------------------------- 1 | // Example creating custom characters 0-7 on a HD44700 compatible LCD 2 | // Using 2-wire connection to LCD 3 | 4 | #include 5 | 6 | const byte dataPin = 10; // SR Data from Arduino pin 10 7 | const byte clockPin = 11; // SR Clock from Arduino pin 11 8 | 9 | ShiftRegLCD srlcd(dataPin, clockPin, TWO_WIRE); 10 | 11 | void setup() 12 | { 13 | // Turn on backlight (if used) 14 | srlcd.backlightOn(); 15 | 16 | uint8_t bell[8] = {0x4,0xe,0xe,0xe,0x1f,0x0,0x4}; 17 | uint8_t note[8] = {0x2,0x3,0x2,0xe,0x1e,0xc,0x0}; 18 | uint8_t clock[8] = {0x0,0xe,0x15,0x17,0x11,0xe,0x0}; 19 | uint8_t heart[8] = {0x0,0xa,0x1f,0x1f,0xe,0x4,0x0}; 20 | uint8_t duck[8] = {0x0,0xc,0x1d,0xf,0xf,0x6,0x0}; 21 | uint8_t check[8] = {0x0,0x1,0x3,0x16,0x1c,0x8,0x0}; 22 | uint8_t cross[8] = {0x0,0x1b,0xe,0x4,0xe,0x1b,0x0}; 23 | uint8_t retarrow[8] = {0x1,0x1,0x5,0x9,0x1f,0x8,0x4}; 24 | 25 | srlcd.createChar(0, bell); 26 | srlcd.createChar(1, note); 27 | srlcd.createChar(2, clock); 28 | srlcd.createChar(3, heart); 29 | srlcd.createChar(4, duck); 30 | srlcd.createChar(5, check); 31 | srlcd.createChar(6, cross); 32 | srlcd.createChar(7, retarrow); 33 | 34 | for (uint8_t i=8;i>0;i--) srlcd.write(i); 35 | 36 | srlcd.print(" CUSTOM CHARACTERS"); 37 | } 38 | 39 | void loop() 40 | { 41 | } 42 | 43 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/examples/CustomChar2/CustomChar2.pde: -------------------------------------------------------------------------------- 1 | // Example creating custom characters 0-7 on a HD44700 compatible LCD 2 | // Using 2-wire connection to LCD 3 | 4 | #include 5 | 6 | const byte dataPin = 10; // SR Data from Arduino pin 10 7 | const byte clockPin = 11; // SR Clock from Arduino pin 11 8 | 9 | ShiftRegLCD srlcd(dataPin, clockPin, TWO_WIRE); 10 | 11 | void setup() 12 | { 13 | // Turn on backlight (if used) 14 | srlcd.backlightOn(); 15 | 16 | // very simple characters, could be used for a bar-graph 17 | uint8_t customchar[][8] ={ { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F }, 18 | { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x1F }, 19 | { 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x1F, 0x1F }, 20 | { 0x00, 0x00, 0x00, 0x00, 0x1F, 0x1F, 0x1F, 0x1F }, 21 | { 0x00, 0x00, 0x00, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F }, 22 | { 0x00, 0x00, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F }, 23 | { 0x00, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F }, 24 | { 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F, 0x1F } }; 25 | 26 | 27 | // Program the characters into the LCD 28 | 29 | for (uint8_t c = 0;c<8;c++) { 30 | srlcd.createChar(c, customchar[c]); 31 | } 32 | 33 | for (uint8_t i=0;i<8;i++) srlcd.write(i); 34 | srlcd.print("CUSTOM CHARS"); 35 | for (uint8_t i=8;i>0;i--) srlcd.write(i-1); 36 | } 37 | 38 | void loop() 39 | { 40 | } 41 | 42 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/examples/HelloWorld/HelloWorld.pde: -------------------------------------------------------------------------------- 1 | /* 2-wire connection demo to a HD44780-compatible LCD via shiftregister with: 2 | Data (and Enable) on Arduino pin 10 3 | Clock on Arduino pin 11 4 | 5 | CONNECTION DESCRIPTION 6 | 7 | ShiftRegLCD 8 | 9 | Requires 2 or 3 pins from the Arduino. 10 | Arduino outputs to shift register data, clock and (optional) LCD Enable inputs. 11 | 12 | SR output: 13 | Bit #0 - N/C - not connected. Must be 0 in 2-wire mode. 14 | Bit #1 - LCD backlight control bit, or unused. 15 | Do not connect directly! Use a driver/ transistor! 16 | Bit #2 - connects to RS (Register Select) on the LCD 17 | Bits #3-6 - connects to LCD data inputs D4 - D7. 18 | Bit #7 - 2-wire mode: To cathode of the diode in the diode-resistor AND "gate" 19 | 3-wire mode: Unused 20 | 21 | LCD RW-pin hardwired to LOW (only writing to LCD). 22 | Busy Flag (BF, data bit D7) is not read. 23 | 24 | 25 | USAGE: 26 | 27 | ShiftRegLCD LCDobjectvariablename(Datapin, Clockpin, Enablepin or TWO_WIRE [, Lines [, Font]]]) 28 | 29 | where: 30 | Lines : Optional. Sets display nr of (logical) lines (1 or 2). 31 | Font : Optional. Sets small (normal 5x8 pix) or large (5x10 pix) font. 32 | Enablepin: Can be replaced by constant TWO_WIRE, if using only 2 wires. 33 | */ 34 | 35 | #include 36 | 37 | const byte dataPin = 10; // SR Data from Arduino pin 10 38 | const byte clockPin = 11; // SR Clock from Arduino pin 11 39 | 40 | ShiftRegLCD srlcd(dataPin, clockPin, TWO_WIRE); 41 | 42 | void setup() 43 | { 44 | // Turn on backlight (if used) 45 | srlcd.backlightOn(); 46 | 47 | // Print a message to the LCD. 48 | srlcd.print("HELLO, WORLD!"); 49 | } 50 | 51 | void loop() 52 | { 53 | } 54 | 55 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/examples/HelloWorld2/HelloWorld2.pde: -------------------------------------------------------------------------------- 1 | /* 3-wire connection demo to a HD44780-compatible LCD via shiftregister with: 2 | Data on Arduino pin 10 3 | Clock on Arduino pin 11 4 | LCD Enable on Arduino pin 12 5 | 6 | CONNECTION DESCRIPTION 7 | 8 | ShiftRegLCD 9 | 10 | Requires 2 or 3 pins from the Arduino. 11 | Arduino outputs to shift register data, clock and (optional) LCD Enable inputs. 12 | 13 | SR output: 14 | Bit #0 - N/C - not connected. Must be 0 in 2-wire mode. 15 | Bit #1 - LCD backlight control bit, or unused. 16 | Do not connect directly! Use a driver/ transistor! 17 | Bit #2 - connects to RS (Register Select) on the LCD 18 | Bits #3-6 - connects to LCD data inputs D4 - D7. 19 | Bit #7 - 2-wire mode: To cathode of the diode in the diode-resistor AND "gate" 20 | 3-wire mode: Unused 21 | 22 | LCD RW-pin hardwired to LOW (only writing to LCD). 23 | Busy Flag (BF, data bit D7) is not read. 24 | 25 | 26 | USAGE: 27 | 28 | ShiftRegLCD LCDobjectvariablename(Datapin, Clockpin, Enablepin or TWO_WIRE [, Lines [, Font]]]) 29 | 30 | where: 31 | Lines : Optional. Sets display nr of (logical) lines (1 or 2). 32 | Font : Optional. Sets small (normal 5x8 pix) or large (5x10 pix) font. 33 | Enablepin: Can be replaced by constant TWO_WIRE, if using only 2 wires. 34 | */ 35 | 36 | #include 37 | 38 | const byte dataPin = 10; // SR Data from Arduino pin 10 39 | const byte clockPin = 11; // SR Clock from Arduino pin 11 40 | const byte enablePin = 12; // LCD enable from Arduino pin 12 41 | 42 | ShiftRegLCD srlcd(dataPin, clockPin, enablePin); 43 | 44 | void setup() 45 | { 46 | // Turn on backlight (if used) 47 | srlcd.backlightOn(); 48 | 49 | // Print a message to the LCD. 50 | srlcd.print("HELLO, WORLD!"); 51 | } 52 | 53 | void loop() 54 | { 55 | } 56 | 57 | -------------------------------------------------------------------------------- /Configer/configer/configer.vcxproj.filters: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | {4FC737F1-C7A5-4376-A066-2A32D752A2FF} 6 | cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx 7 | 8 | 9 | {93995380-89BD-4b04-88EB-625FBE52EBFB} 10 | h;hpp;hxx;hm;inl;inc;xsd 11 | 12 | 13 | {67DA6AB6-F800-4c08-8B7A-83BB121AAD01} 14 | rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 头文件 23 | 24 | 25 | 头文件 26 | 27 | 28 | 头文件 29 | 30 | 31 | 头文件 32 | 33 | 34 | 头文件 35 | 36 | 37 | 38 | 39 | 源文件 40 | 41 | 42 | 源文件 43 | 44 | 45 | 源文件 46 | 47 | 48 | 49 | 50 | 资源文件 51 | 52 | 53 | 54 | 55 | 资源文件 56 | 57 | 58 | 59 | 60 | 资源文件 61 | 62 | 63 | -------------------------------------------------------------------------------- /RemoteControl/connect.h: -------------------------------------------------------------------------------- 1 | class Cconnect 2 | { 3 | public : 4 | NetStruct netBuff; 5 | 6 | ulong m_LastTime; 7 | ulong m_SuccessCount; 8 | ulong m_SendCount; 9 | ulong m_RecvCount; 10 | 11 | Cconnect(int cePin, int csnPin){ 12 | //* MISO -> 12 13 | //* MOSI -> 11 14 | //* SCK -> 13 15 | 16 | Mirf.cePin = cePin; 17 | Mirf.csnPin = csnPin; 18 | Mirf.spi = &MirfHardwareSpi; 19 | Mirf.init(); 20 | Mirf.setRADDR((byte *)"1controler"); 21 | Mirf.channel = 1; 22 | Mirf.payload = sizeof(NetStruct); 23 | Mirf.config(); 24 | Mirf.setTADDR((byte *)"1aircraft"); 25 | 26 | m_LastTime = millis(); 27 | m_SendCount = 0; 28 | m_RecvCount = 0; 29 | } 30 | 31 | NetStruct sendbuff; 32 | NetStruct recvbuff; 33 | 34 | //发送操作指令 35 | bool CommandByType(NetType t){ 36 | return SetValueByType( t, 0); 37 | } 38 | 39 | //调整参数 40 | bool SetValueByType(NetType t, float vIn){ 41 | ulong v = 0; 42 | return GetValue( t, *(ulong*)&vIn, v); 43 | } 44 | 45 | //仅获取 46 | bool GetValueByType(NetType t, float &vOut){ 47 | return GetValue( t, 0 ,*(ulong*)&vOut); 48 | } 49 | 50 | //修改并获取 51 | bool GetValueByType(NetType t, float vIn ,float &vOut){ 52 | return GetValue( t, *(ulong*)&vIn ,*(ulong*)&vOut); 53 | } 54 | 55 | bool GetValue(NetType t, ulong vIn ,ulong &vOut){ 56 | m_SendCount++; 57 | if (Mirf.dataReady()) Mirf.getData((byte *) &sendbuff); 58 | sendbuff.seq++; 59 | sendbuff.time = millis(); 60 | sendbuff.type = t; 61 | sendbuff.value = vIn; 62 | Mirf.send((byte *)&sendbuff); 63 | while(Mirf.isSending()){ 64 | } 65 | while(!Mirf.dataReady()){ 66 | if ( ( millis() - sendbuff.time ) > 10) { 67 | return false; 68 | } 69 | } 70 | Mirf.getData((byte *) &recvbuff); 71 | m_RecvCount++; 72 | if(recvbuff.seq != sendbuff.seq) 73 | return false; 74 | vOut = recvbuff.value; 75 | return true; 76 | } 77 | 78 | int GetSuccessCount(){ 79 | if(millis() - m_LastTime > 500){ 80 | m_SuccessCount = m_RecvCount*100/ m_SendCount; 81 | m_LastTime = millis(); 82 | m_SendCount = 0; 83 | m_RecvCount = 0; 84 | } 85 | return m_SuccessCount; 86 | } 87 | }; 88 | -------------------------------------------------------------------------------- /libraries/Mirf/examples/ping_server/ping_server.pde: -------------------------------------------------------------------------------- 1 | /** 2 | * An Mirf example which copies back the data it recives. 3 | * 4 | * Pins: 5 | * Hardware SPI: 6 | * MISO -> 12 7 | * MOSI -> 11 8 | * SCK -> 13 9 | * 10 | * Configurable: 11 | * CE -> 8 12 | * CSN -> 7 13 | * 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | void setup(){ 22 | Serial.begin(9600); 23 | 24 | /* 25 | * Set the SPI Driver. 26 | */ 27 | 28 | Mirf.spi = &MirfHardwareSpi; 29 | 30 | /* 31 | * Setup pins / SPI. 32 | */ 33 | 34 | Mirf.init(); 35 | 36 | /* 37 | * Configure reciving address. 38 | */ 39 | 40 | Mirf.setRADDR((byte *)"serv1"); 41 | 42 | /* 43 | * Set the payload length to sizeof(unsigned long) the 44 | * return type of millis(). 45 | * 46 | * NB: payload on client and server must be the same. 47 | */ 48 | 49 | Mirf.payload = sizeof(unsigned long); 50 | 51 | /* 52 | * Write channel and payload config then power up reciver. 53 | */ 54 | 55 | Mirf.config(); 56 | 57 | Serial.println("Listening..."); 58 | } 59 | 60 | void loop(){ 61 | /* 62 | * A buffer to store the data. 63 | */ 64 | 65 | byte data[Mirf.payload]; 66 | 67 | /* 68 | * If a packet has been recived. 69 | * 70 | * isSending also restores listening mode when it 71 | * transitions from true to false. 72 | */ 73 | 74 | if(!Mirf.isSending() && Mirf.dataReady()){ 75 | Serial.println("Got packet"); 76 | 77 | /* 78 | * Get load the packet into the buffer. 79 | */ 80 | 81 | Mirf.getData(data); 82 | 83 | /* 84 | * Set the send address. 85 | */ 86 | 87 | 88 | Mirf.setTADDR((byte *)"clie1"); 89 | 90 | /* 91 | * Send the data back to the client. 92 | */ 93 | 94 | Mirf.send(data); 95 | 96 | /* 97 | * Wait untill sending has finished 98 | * 99 | * NB: isSending returns the chip to receving after returning true. 100 | */ 101 | 102 | Serial.println("Reply sent."); 103 | } 104 | } 105 | -------------------------------------------------------------------------------- /RemoteControl/leds.h: -------------------------------------------------------------------------------- 1 | class Cleds 2 | { 3 | public: 4 | int m_LatchPin; 5 | int m_DataPin; 6 | int m_ClockPin; 7 | 8 | short HeaderMask; 9 | short LeftMask[5]; 10 | short RightMask[5]; 11 | 12 | short m_Value; 13 | 14 | long m_LastTime ; 15 | bool m_Light; 16 | 17 | void Write(short v){ 18 | char* c = (char*)&v; 19 | digitalWrite(m_LatchPin, LOW); 20 | shiftOut(m_DataPin, m_ClockPin, MSBFIRST, c[0]); 21 | shiftOut(m_DataPin, m_ClockPin, MSBFIRST, c[1]); 22 | digitalWrite(m_LatchPin, HIGH); 23 | } 24 | 25 | Cleds(int latchPin, int dataPin, int clockPin){ 26 | m_Light = false; 27 | m_Value =0; 28 | HeaderMask = 0x0004; 29 | 30 | LeftMask[0] =0x8000; 31 | LeftMask[1] =0x4000; 32 | LeftMask[2] =0x2000; 33 | LeftMask[3] =0x1000; 34 | LeftMask[4] =0x0800; 35 | 36 | RightMask[0] =0x0080; 37 | RightMask[1] =0x0040; 38 | RightMask[2] =0x0020; 39 | RightMask[3] =0x0010; 40 | RightMask[4] =0x0008; 41 | m_LatchPin = latchPin; 42 | m_DataPin = dataPin; 43 | m_ClockPin = clockPin; 44 | pinMode(m_LatchPin, OUTPUT); 45 | pinMode(m_DataPin, OUTPUT); 46 | pinMode(m_ClockPin, OUTPUT); 47 | } 48 | 49 | void NetBlink(){ 50 | m_Value^= HeaderMask; 51 | Handle(); 52 | } 53 | 54 | void NetOff(){ 55 | m_Value &=~HeaderMask; 56 | Handle(); 57 | } 58 | 59 | void PitchBlink(){ 60 | m_Value^= LeftMask[0]; 61 | Handle(); 62 | } 63 | 64 | void PitchOff(){ 65 | m_Value &=~LeftMask[0]; 66 | Handle(); 67 | } 68 | 69 | void RollBlink(){ 70 | m_Value^= LeftMask[1]; 71 | Handle(); 72 | } 73 | 74 | void RollOff(){ 75 | m_Value &=~LeftMask[1]; 76 | Handle(); 77 | } 78 | 79 | void EleBlink(){ 80 | m_Value^= RightMask[0]; 81 | Handle(); 82 | } 83 | 84 | void EleOff(){ 85 | m_Value &=~RightMask[0]; 86 | Handle(); 87 | } 88 | 89 | void YawBlink(){ 90 | m_Value^= RightMask[1]; 91 | Handle(); 92 | } 93 | 94 | void YawOff(){ 95 | m_Value &=~RightMask[1]; 96 | Handle(); 97 | } 98 | 99 | void Handle(){ 100 | Write(m_Value); 101 | } 102 | 103 | void NetOn(){ 104 | m_Value |=HeaderMask ; 105 | Handle(); 106 | } 107 | }; 108 | -------------------------------------------------------------------------------- /libraries/Mirf/examples/ping_client/ping_client.pde: -------------------------------------------------------------------------------- 1 | /** 2 | * A Mirf example to test the latency between two Ardunio. 3 | * 4 | * Pins: 5 | * Hardware SPI: 6 | * MISO -> 12 7 | * MOSI -> 11 8 | * SCK -> 13 9 | * 10 | * Configurable: 11 | * CE -> 8 12 | * CSN -> 7 13 | * 14 | * Note: To see best case latency comment out all Serial.println 15 | * statements not displaying the result and load 16 | * 'ping_server_interupt' on the server. 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | void setup(){ 25 | Serial.begin(9600); 26 | /* 27 | * Setup pins / SPI. 28 | */ 29 | 30 | /* To change CE / CSN Pins: 31 | * 32 | * Mirf.csnPin = 9; 33 | * Mirf.cePin = 7; 34 | */ 35 | /* 36 | Mirf.cePin = 7; 37 | Mirf.csnPin = 8; 38 | */ 39 | Mirf.spi = &MirfHardwareSpi; 40 | Mirf.init(); 41 | 42 | /* 43 | * Configure reciving address. 44 | */ 45 | 46 | Mirf.setRADDR((byte *)"clie1"); 47 | 48 | /* 49 | * Set the payload length to sizeof(unsigned long) the 50 | * return type of millis(). 51 | * 52 | * NB: payload on client and server must be the same. 53 | */ 54 | 55 | Mirf.payload = sizeof(unsigned long); 56 | 57 | /* 58 | * Write channel and payload config then power up reciver. 59 | */ 60 | 61 | /* 62 | * To change channel: 63 | * 64 | * Mirf.channel = 10; 65 | * 66 | * NB: Make sure channel is legal in your area. 67 | */ 68 | 69 | Mirf.config(); 70 | 71 | Serial.println("Beginning ... "); 72 | } 73 | 74 | void loop(){ 75 | unsigned long time = millis(); 76 | 77 | Mirf.setTADDR((byte *)"serv1"); 78 | 79 | Mirf.send((byte *)&time); 80 | 81 | while(Mirf.isSending()){ 82 | } 83 | Serial.println("Finished sending"); 84 | delay(10); 85 | while(!Mirf.dataReady()){ 86 | //Serial.println("Waiting"); 87 | if ( ( millis() - time ) > 1000 ) { 88 | Serial.println("Timeout on response from server!"); 89 | return; 90 | } 91 | } 92 | 93 | Mirf.getData((byte *) &time); 94 | 95 | Serial.print("Ping: "); 96 | Serial.println((millis() - time)); 97 | 98 | delay(1000); 99 | } 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /libraries/Mirf/examples/ping_server_interupt/ping_server_interupt.pde: -------------------------------------------------------------------------------- 1 | /** 2 | * An Mirf example which copies back the data it recives. 3 | * While wating the arduino goes to sleep and will be woken up 4 | * by the interupt pin of the mirf. 5 | * 6 | * Warning: Due to the sleep mode the Serial output donsn't work. 7 | * 8 | * Pins: 9 | * Hardware SPI: 10 | * MISO -> 12 11 | * MOSI -> 11 12 | * SCK -> 13 13 | * 14 | * Configurable: 15 | * CE -> 8 16 | * CSN -> 7 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | void wakeupFunction(){ 26 | } 27 | 28 | void toSleep(){ 29 | attachInterrupt(0,wakeupFunction,LOW); 30 | sleep_mode(); 31 | detachInterrupt(0); 32 | } 33 | 34 | void setup(){ 35 | Serial.begin(9600); 36 | 37 | /* 38 | * Set the SPI Driver. 39 | */ 40 | 41 | Mirf.spi = &MirfHardwareSpi; 42 | 43 | /* 44 | * Setup pins / SPI. 45 | */ 46 | 47 | Mirf.init(); 48 | 49 | /* 50 | * Configure reciving address. 51 | */ 52 | 53 | Mirf.setRADDR((byte *)"serv1"); 54 | 55 | /* 56 | * Set the payload length to sizeof(unsigned long) the 57 | * return type of millis(). 58 | * 59 | * NB: payload on client and server must be the same. 60 | */ 61 | 62 | Mirf.payload = sizeof(unsigned long); 63 | 64 | /* 65 | * Write channel and payload config then power up reciver. 66 | */ 67 | 68 | Mirf.config(); 69 | 70 | /* 71 | * Configure seep mode to save power. 72 | */ 73 | 74 | set_sleep_mode(SLEEP_MODE_PWR_DOWN); 75 | sleep_enable(); 76 | 77 | Serial.println("Listening..."); 78 | } 79 | 80 | void loop(){ 81 | /* 82 | * A buffer to store the data. 83 | */ 84 | 85 | byte data[Mirf.payload]; 86 | 87 | /* 88 | * If a packet has been recived. 89 | */ 90 | if(!Mirf.isSending() && Mirf.dataReady()){ 91 | 92 | /* 93 | * Get load the packet into the buffer. 94 | */ 95 | 96 | Mirf.getData(data); 97 | 98 | /* 99 | * Set the send address. 100 | */ 101 | 102 | Mirf.setTADDR((byte *)"clie1"); 103 | 104 | /* 105 | * Send the data back to the client. 106 | */ 107 | 108 | Mirf.send(data); 109 | }else{ 110 | /* No data - night night. */ 111 | toSleep(); 112 | } 113 | } 114 | -------------------------------------------------------------------------------- /Configer/configer/ReadMe.txt: -------------------------------------------------------------------------------- 1 | ================================================================================ 2 | MICROSOFT 基础类库 : configer 项目概述 3 | =============================================================================== 4 | 5 | 应用程序向导已为您创建了此 configer 应用程序。此应用程序不仅演示 Microsoft 基础类的基本使用方法，还可作为您编写应用程序的起点。 6 | 7 | 本文件概要介绍组成 configer 应用程序的每个文件的内容。 8 | 9 | configer.vcxproj 10 | 这是使用应用程序向导生成的 VC++ 项目的主项目文件，其中包含生成该文件的 Visual C++ 的版本信息，以及有关使用应用程序向导选择的平台、配置和项目功能的信息。 11 | 12 | configer.vcxproj.filters 13 | 这是使用“应用程序向导”生成的 VC++ 项目筛选器文件。它包含有关项目文件与筛选器之间的关联信息。在 IDE 中，通过这种关联，在特定节点下以分组形式显示具有相似扩展名的文件。例如，“.cpp”文件与“源文件”筛选器关联。 14 | 15 | configer.h 16 | 这是应用程序的主头文件。 17 | 其中包括其他项目特定的标头（包括 Resource.h），并声明 CconfigerApp 应用程序类。 18 | 19 | configer.cpp 20 | 这是包含应用程序类 CconfigerApp 的主应用程序源文件。 21 | 22 | configer.rc 23 | 这是程序使用的所有 Microsoft Windows 资源的列表。它包括 RES 子目录中存储的图标、位图和光标。此文件可以直接在 Microsoft Visual C++ 中进行编辑。项目资源包含在 2052 中。 24 | 25 | res\configer.ico 26 | 这是用作应用程序图标的图标文件。此图标包括在主资源文件 configer.rc 中。 27 | 28 | res\configer.rc2 29 | 此文件包含不在 Microsoft Visual C++ 中进行编辑的资源。您应该将不可由资源编辑器编辑的所有资源放在此文件中。 30 | 31 | 32 | ///////////////////////////////////////////////////////////////////////////// 33 | 34 | 应用程序向导创建一个对话框类： 35 | 36 | configerDlg.h、configerDlg.cpp - 对话框 37 | 这些文件包含 CconfigerDlg 类。此类定义应用程序的主对话框的行为。对话框模板包含在 configer.rc 中，该文件可以在 Microsoft Visual C++ 中编辑。 38 | 39 | ///////////////////////////////////////////////////////////////////////////// 40 | 41 | 其他功能： 42 | 43 | ActiveX 控件 44 | 该应用程序包含对使用 ActiveX 控件的支持。 45 | 46 | 打印和打印预览支持 47 | 应用程序向导通过从 MFC 库调用 CView 类中的成员函数生成代码，来处理打印、打印设置和打印预览命令。 48 | 49 | ///////////////////////////////////////////////////////////////////////////// 50 | 51 | 其他标准文件: 52 | 53 | StdAfx.h, StdAfx.cpp 54 | 这些文件用于生成名为 configer.pch 的预编译头 (PCH) 文件和名为 StdAfx.obj 的预编译类型文件。 55 | 56 | Resource.h 57 | 这是标准头文件，可用于定义新的资源 ID。Microsoft Visual C++ 将读取并更新此文件。 58 | 59 | configer.manifest 60 | Windows XP 使用应用程序清单文件来描述特定版本的并行程序集的应用程序依赖项。加载程序使用这些信息来从程序集缓存中加载相应的程序集，并保护其不被应用程序访问。应用程序清单可能会包含在内，以作为与应用程序可执行文件安装在同一文件夹中的外部 .manifest 文件进行重新分发，它还可能以资源的形式包含在可执行文件中。 61 | ///////////////////////////////////////////////////////////////////////////// 62 | 63 | 其他注释: 64 | 65 | 应用程序向导使用“TODO:”来指示应添加或自定义的源代码部分。 66 | 67 | 如果应用程序使用共享 DLL 中的 MFC，您将需要重新分发 MFC DLL。如果应用程序所使用的语言与操作系统的区域设置不同，则还需要重新分发相应的本地化资源 mfc110XXX.DLL。 68 | 有关上述话题的更多信息，请参见 MSDN 文档中有关重新分发 Visual C++ 应用程序的部分。 69 | 70 | ///////////////////////////////////////////////////////////////////////////// 71 | -------------------------------------------------------------------------------- /RemoteControl/lcd.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | enum TypePic 4 | { 5 | PIC_PITCH = 0, 6 | PIC_ROLL, 7 | PIC_YAW, 8 | PIC_ELEVATION, 9 | PIC_POWER, 10 | PIC_SIGNAL, 11 | }; 12 | 13 | class Clcd 14 | { 15 | public : 16 | ShiftRegLCD *srlcd; 17 | int m_LatchPin; 18 | int m_DataPin; 19 | int m_ClockPin; 20 | 21 | Clcd (int latchPin, int dataPin, int clockPin){ 22 | m_LatchPin = latchPin; 23 | m_DataPin = dataPin; 24 | m_ClockPin = clockPin; 25 | pinMode(m_LatchPin, OUTPUT); 26 | pinMode(m_DataPin, OUTPUT); 27 | pinMode(m_ClockPin, OUTPUT); 28 | 29 | srlcd = new ShiftRegLCD(m_DataPin, m_ClockPin, m_LatchPin, 2); 30 | 31 | } 32 | 33 | void Show(float p, float r, float y, float e){ 34 | ShowP((int)p); 35 | ShowI((int)r); 36 | ShowY((int)y); 37 | ShowE((int)e); 38 | } 39 | 40 | //0 - 100 41 | void ShowPower(int v){ 42 | int p = map (v, 0, 100, 0, 7); 43 | uint8_t power[8] = {0x0, 0x08, 0x1c, 0x1c, 0x1c, 0x1c, 0x1c, 0x0}; 44 | for (int j = 0; j <= p; j++) power[7 - j] |= 0x1; 45 | srlcd->createChar(PIC_POWER, power); 46 | srlcd->setCursor(0, 0); 47 | srlcd->write(PIC_POWER); 48 | } 49 | 50 | void SetSignal(int v){ 51 | int p = map (v, 0, 100, 0, 7); 52 | uint8_t signal[8] = {0x0, 0x14, 0x14, 0x18, 0x10, 0x10, 0x10, 0x0}; 53 | for (int j = 0 ; j <= p; j++) signal[7 - j] |= 0x1; 54 | srlcd->createChar(PIC_SIGNAL, signal); 55 | srlcd->setCursor(1, 0); 56 | srlcd->write(PIC_SIGNAL); 57 | } 58 | 59 | private: 60 | void ShowP(int v){ 61 | uint8_t pic[8] = {0x0, 0x4, 0xe, 0x4, 0x4, 0x4, 0x4, 0x0}; 62 | v<0?pic[7]|=0x01:pic[0]|=0x01; 63 | srlcd->createChar(PIC_PITCH, pic); 64 | srlcd->setCursor(0, 1); 65 | srlcd->write(PIC_PITCH); 66 | srlcd->print(abs(v)); 67 | Interval(v); 68 | } 69 | 70 | void ShowI(int v){ 71 | uint8_t pic[8] = {0x0, 0x0, 0x0, 0x4, 0x1f, 0x0, 0x0, 0x0}; 72 | v<0?pic[7]|=0x01:pic[0]|=0x01; 73 | srlcd->createChar(PIC_ROLL, pic); 74 | srlcd->setCursor(4, 1); 75 | srlcd->write(PIC_ROLL); 76 | srlcd->print(abs(v)); 77 | Interval(v); 78 | } 79 | 80 | void ShowY(int v){ 81 | uint8_t pic[8] = {0x0, 0xe, 0x5, 0x4, 0x4, 0x14, 0xe, 0x0}; 82 | v<0?pic[7]|=0x01:pic[0]|=0x01; 83 | srlcd->createChar(PIC_YAW, pic); 84 | srlcd->setCursor(8, 1); 85 | srlcd->write(PIC_YAW); 86 | srlcd->print(abs(v)); 87 | Interval(v); 88 | } 89 | 90 | void ShowE(int v){ 91 | uint8_t pic[8] = {0x0, 0x1b, 0x0, 0x4, 0x4, 0x0, 0x1b, 0x0}; 92 | v<0?pic[7]|=0x01:pic[0]|=0x01; 93 | srlcd->createChar(PIC_ELEVATION, pic); 94 | srlcd->setCursor(12, 1); 95 | srlcd->write(PIC_ELEVATION); 96 | srlcd->print(abs(v)); 97 | Interval(v); 98 | } 99 | 100 | void Interval(int v){ 101 | if (v/100) srlcd->print(""); 102 | else if (v/10) srlcd->print(" "); 103 | else srlcd->print(" "); 104 | } 105 | }; 106 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/ShiftRegLCD.h: -------------------------------------------------------------------------------- 1 | #ifndef ShiftRegLCD_h 2 | #define ShiftRegLCD_h 3 | 4 | #include 5 | #include "Print.h" 6 | 7 | // commands 8 | #define LCD_CLEARDISPLAY 0x01 9 | #define LCD_RETURNHOME 0x02 10 | #define LCD_ENTRYMODESET 0x04 11 | #define LCD_DISPLAYCONTROL 0x08 12 | #define LCD_CURSORSHIFT 0x10 13 | #define LCD_FUNCTIONSET 0x20 14 | #define LCD_SETCGRAMADDR 0x40 15 | #define LCD_SETDDRAMADDR 0x80 16 | 17 | // flags for display entry mode 18 | #define LCD_ENTRYRIGHT 0x00 19 | #define LCD_ENTRYLEFT 0x02 20 | #define LCD_ENTRYSHIFTINCREMENT 0x01 21 | #define LCD_ENTRYSHIFTDECREMENT 0x00 22 | 23 | // flags for display on/off control 24 | #define LCD_DISPLAYON 0x04 25 | #define LCD_DISPLAYOFF 0x00 26 | #define LCD_CURSORON 0x02 27 | #define LCD_CURSOROFF 0x00 28 | #define LCD_BLINKON 0x01 29 | #define LCD_BLINKOFF 0x00 30 | 31 | // flags for display/cursor shift 32 | #define LCD_DISPLAYMOVE 0x08 33 | #define LCD_CURSORMOVE 0x00 34 | #define LCD_MOVERIGHT 0x04 35 | #define LCD_MOVELEFT 0x00 36 | 37 | // flags for function set 38 | #define LCD_8BITMODE 0x10 39 | #define LCD_4BITMODE 0x00 40 | #define LCD_2LINE 0x08 41 | #define LCD_1LINE 0x00 42 | #define LCD_5x10DOTS 0x04 43 | #define LCD_5x8DOTS 0x00 44 | 45 | // two-wire indicator constant 46 | #define TWO_WIRE 204 47 | 48 | // Shift register bits connected to LCD control pins 49 | #define SR_RS_BIT 0x04 50 | #define SR_EN_BIT 0x80 51 | 52 | // Shift register LCD backlight control bit 53 | #define SR_BACKLIGHT 0x02 54 | 55 | 56 | 57 | class ShiftRegLCD : public Print { 58 | public: 59 | ShiftRegLCD(uint8_t srdata, uint8_t srclockd, uint8_t enable); 60 | ShiftRegLCD(uint8_t srdata, uint8_t srclockd, uint8_t enable, uint8_t lines); 61 | ShiftRegLCD(uint8_t srdata, uint8_t srclockd, uint8_t enable, uint8_t lines, uint8_t font); 62 | void backlightOn(); 63 | void backlightOff(); 64 | void clear(); 65 | void home(); 66 | void noDisplay(); 67 | void display(); 68 | void noBlink(); 69 | void blink(); 70 | void noCursor(); 71 | void cursor(); 72 | void scrollDisplayLeft(); 73 | void scrollDisplayRight(); 74 | void printLeft(); 75 | void printRight(); 76 | void shiftLeft(); 77 | void shiftRight(); 78 | void shiftIncrement(); 79 | void shiftDecrement(); 80 | void createChar(uint8_t, uint8_t[]); 81 | void setCursor(uint8_t, uint8_t); 82 | 83 | #if defined(ARDUINO) && ARDUINO >= 100 84 | virtual size_t write(uint8_t); 85 | #else 86 | virtual void write(uint8_t); 87 | #endif 88 | 89 | void command(uint8_t); 90 | private: 91 | void init(uint8_t srdata, uint8_t srclock, uint8_t enable, uint8_t lines, uint8_t font); 92 | void send(uint8_t, uint8_t); 93 | void init4bits(uint8_t); 94 | uint8_t _srdata_pin; 95 | uint8_t _srclock_pin; 96 | uint8_t _enable_pin; 97 | uint8_t _two_wire; 98 | 99 | uint8_t _displayfunction; 100 | uint8_t _displaycontrol; 101 | uint8_t _displaymode; 102 | uint8_t _numlines; 103 | uint8_t _backlight; 104 | 105 | }; 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /libraries/Mirf/Mirf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (c) 2007 Stefan Engelke 3 | 4 | Permission is hereby granted, free of charge, to any person 5 | obtaining a copy of this software and associated documentation 6 | files (the "Software"), to deal in the Software without 7 | restriction, including without limitation the rights to use, copy, 8 | modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be 13 | included in all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 17 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 19 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 20 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | DEALINGS IN THE SOFTWARE. 23 | 24 | $Id$ 25 | */ 26 | 27 | #ifndef _MIRF_H_ 28 | #define _MIRF_H_ 29 | 30 | #include 31 | 32 | #include "nRF24L01.h" 33 | #include "MirfSpiDriver.h" 34 | 35 | // Nrf24l settings 36 | 37 | #define mirf_ADDR_LEN 5 38 | #define mirf_CONFIG ((1< 3 | 4 | Permission is hereby granted, free of charge, to any person 5 | obtaining a copy of this software and associated documentation 6 | files (the "Software"), to deal in the Software without 7 | restriction, including without limitation the rights to use, copy, 8 | modify, merge, publish, distribute, sublicense, and/or sell copies 9 | of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be 13 | included in all copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 16 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 17 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 18 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 19 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 20 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 22 | DEALINGS IN THE SOFTWARE. 23 | 24 | $Id$ 25 | */ 26 | 27 | /* Memory Map */ 28 | #define CONFIG 0x00 29 | #define EN_AA 0x01 30 | #define EN_RXADDR 0x02 31 | #define SETUP_AW 0x03 32 | #define SETUP_RETR 0x04 33 | #define RF_CH 0x05 34 | #define RF_SETUP 0x06 35 | #define STATUS 0x07 36 | #define OBSERVE_TX 0x08 37 | #define CD 0x09 38 | #define RX_ADDR_P0 0x0A 39 | #define RX_ADDR_P1 0x0B 40 | #define RX_ADDR_P2 0x0C 41 | #define RX_ADDR_P3 0x0D 42 | #define RX_ADDR_P4 0x0E 43 | #define RX_ADDR_P5 0x0F 44 | #define TX_ADDR 0x10 45 | #define RX_PW_P0 0x11 46 | #define RX_PW_P1 0x12 47 | #define RX_PW_P2 0x13 48 | #define RX_PW_P3 0x14 49 | #define RX_PW_P4 0x15 50 | #define RX_PW_P5 0x16 51 | #define FIFO_STATUS 0x17 52 | 53 | /* Bit Mnemonics */ 54 | #define MASK_RX_DR 6 55 | #define MASK_TX_DS 5 56 | #define MASK_MAX_RT 4 57 | #define EN_CRC 3 58 | #define CRCO 2 59 | #define PWR_UP 1 60 | #define PRIM_RX 0 61 | #define ENAA_P5 5 62 | #define ENAA_P4 4 63 | #define ENAA_P3 3 64 | #define ENAA_P2 2 65 | #define ENAA_P1 1 66 | #define ENAA_P0 0 67 | #define ERX_P5 5 68 | #define ERX_P4 4 69 | #define ERX_P3 3 70 | #define ERX_P2 2 71 | #define ERX_P1 1 72 | #define ERX_P0 0 73 | #define AW 0 74 | #define ARD 4 75 | #define ARC 0 76 | #define PLL_LOCK 4 77 | #define RF_DR 3 78 | #define RF_PWR 1 79 | #define LNA_HCURR 0 80 | #define RX_DR 6 81 | #define TX_DS 5 82 | #define MAX_RT 4 83 | #define RX_P_NO 1 84 | #define TX_FULL 0 85 | #define PLOS_CNT 4 86 | #define ARC_CNT 0 87 | #define TX_REUSE 6 88 | #define FIFO_FULL 5 89 | #define TX_EMPTY 4 90 | #define RX_FULL 1 91 | #define RX_EMPTY 0 92 | 93 | /* Instruction Mnemonics */ 94 | #define R_REGISTER 0x00 95 | #define W_REGISTER 0x20 96 | #define REGISTER_MASK 0x1F 97 | #define R_RX_PAYLOAD 0x61 98 | #define W_TX_PAYLOAD 0xA0 99 | #define FLUSH_TX 0xE1 100 | #define FLUSH_RX 0xE2 101 | #define REUSE_TX_PL 0xE3 102 | #define NOP 0xFF 103 | -------------------------------------------------------------------------------- /libraries/HMC5883L/Examples/HMC5883L_raw/HMC5883L_raw.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for HMC5883L class 2 | // 10/7/2011 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-04 - Added Heading Calculation in degrees 7 | // 2011-10-07 - initial release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2011 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 34 | // is used in I2Cdev.h 35 | #include "Wire.h" 36 | 37 | // I2Cdev and HMC5883L must be installed as libraries, or else the .cpp/.h files 38 | // for both classes must be in the include path of your project 39 | #include "I2Cdev.h" 40 | #include "HMC5883L.h" 41 | 42 | // class default I2C address is 0x1E 43 | // specific I2C addresses may be passed as a parameter here 44 | // this device only supports one I2C address (0x1E) 45 | HMC5883L mag; 46 | 47 | int16_t mx, my, mz; 48 | 49 | #define LED_PIN 13 50 | bool blinkState = false; 51 | 52 | void setup() { 53 | // join I2C bus (I2Cdev library doesn't do this automatically) 54 | Wire.begin(); 55 | 56 | // initialize serial communication 57 | // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but 58 | // it's really up to you depending on your project) 59 | Serial.begin(115200); 60 | 61 | // initialize device 62 | Serial.println("Initializing I2C devices..."); 63 | mag.initialize(); 64 | 65 | // verify connection 66 | Serial.println("Testing device connections..."); 67 | Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed"); 68 | 69 | // configure Arduino LED for 70 | pinMode(LED_PIN, OUTPUT); 71 | } 72 | 73 | void loop() { 74 | // read raw heading measurements from device 75 | mag.getHeading(&mx, &my, &mz); 76 | 77 | // display tab-separated gyro x/y/z values 78 | Serial.print("mag:\t"); 79 | Serial.print(mx); Serial.print("\t"); 80 | Serial.print(my); Serial.print("\t"); 81 | Serial.print(mz); Serial.print("\t"); 82 | 83 | // To calculate heading in degrees. 0 degree indicates North 84 | float heading = atan2(my, mx); 85 | if(heading < 0) 86 | heading += 2 * M_PI; 87 | Serial.print("heading:\t"); 88 | Serial.println(heading * 180/M_PI); 89 | 90 | // blink LED to indicate activity 91 | blinkState = !blinkState; 92 | digitalWrite(LED_PIN, blinkState); 93 | } 94 | -------------------------------------------------------------------------------- /libraries/BMP085/Examples/BMP085_basic/BMP085_basic.ino: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - BMP085 basic Arduino example sketch 2 | // Based on register information stored in the I2Cdevlib internal database 3 | // 2012-06-28 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2012-06-28 - initial release, dynamically built 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 34 | // is used in I2Cdev.h 35 | #include "Wire.h" 36 | 37 | // I2Cdev and BMP085 must be installed as libraries, or else the .cpp/.h files 38 | // for both classes must be in the include path of your project 39 | #include "I2Cdev.h" 40 | #include "BMP085.h" 41 | 42 | // class default I2C address is 0x77 43 | // specific I2C addresses may be passed as a parameter here 44 | // (though the BMP085 supports only one address) 45 | BMP085 barometer; 46 | 47 | float temperature; 48 | float pressure; 49 | float altitude; 50 | int32_t lastMicros; 51 | 52 | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) 53 | bool blinkState = false; 54 | 55 | void setup() { 56 | // join I2C bus (I2Cdev library doesn't do this automatically) 57 | Wire.begin(); 58 | 59 | // initialize serial communication 60 | // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but 61 | // it's really up to you depending on your project) 62 | Serial.begin(115200); 63 | delay(5000); 64 | 65 | // initialize device 66 | Serial.println("Initializing I2C devices..."); 67 | barometer.initialize(); 68 | 69 | // verify connection 70 | Serial.println("Testing device connections..."); 71 | Serial.println(barometer.testConnection() ? "BMP085 connection successful" : "BMP085 connection failed"); 72 | 73 | // configure LED pin for activity indication 74 | pinMode(LED_PIN, OUTPUT); 75 | } 76 | 77 | void loop() { 78 | // request temperature 79 | barometer.setControl(BMP085_MODE_TEMPERATURE); 80 | 81 | // wait appropriate time for conversion (4.5ms delay) 82 | lastMicros = micros(); 83 | while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds()); 84 | 85 | // read calibrated temperature value in degrees Celsius 86 | temperature = barometer.getTemperatureC(); 87 | 88 | // request pressure (3x oversampling mode, high detail, 23.5ms delay) 89 | barometer.setControl(BMP085_MODE_PRESSURE_3); 90 | while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds()); 91 | 92 | // read calibrated pressure value in Pascals (Pa) 93 | pressure = barometer.getPressure(); 94 | 95 | // calculate absolute altitude in meters based on known pressure 96 | // (may pass a second "sea level pressure" parameter here, 97 | // otherwise uses the standard value of 101325 Pa) 98 | altitude = barometer.getAltitude(pressure); 99 | 100 | // display measured values if appropriate 101 | Serial.print("T/P/A\t"); 102 | Serial.print(temperature); Serial.print("\t"); 103 | Serial.print(pressure); Serial.print("\t"); 104 | Serial.print(altitude); 105 | Serial.println(""); 106 | 107 | // blink LED to indicate activity 108 | blinkState = !blinkState; 109 | digitalWrite(LED_PIN, blinkState); 110 | 111 | // delay 100 msec to allow visually parsing blink and any serial output 112 | delay(100); 113 | } 114 | -------------------------------------------------------------------------------- /libraries/BMP085/BMP085.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - BMP085 I2C device class 2 | // Based on register information stored in the I2Cdevlib internal database 3 | // 2012-06-28 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2012-06-28 - initial release, dynamically built 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _BMP085_H_ 34 | #define _BMP085_H_ 35 | 36 | #include "I2Cdev.h" 37 | 38 | #define BMP085_ADDRESS 0x77 39 | #define BMP085_DEFAULT_ADDRESS BMP085_ADDRESS 40 | 41 | #define BMP085_RA_AC1_H 0xAA /* AC1_H */ 42 | #define BMP085_RA_AC1_L 0xAB /* AC1_L */ 43 | #define BMP085_RA_AC2_H 0xAC /* AC2_H */ 44 | #define BMP085_RA_AC2_L 0xAD /* AC2_L */ 45 | #define BMP085_RA_AC3_H 0xAE /* AC3_H */ 46 | #define BMP085_RA_AC3_L 0xAF /* AC3_L */ 47 | #define BMP085_RA_AC4_H 0xB0 /* AC4_H */ 48 | #define BMP085_RA_AC4_L 0xB1 /* AC4_L */ 49 | #define BMP085_RA_AC5_H 0xB2 /* AC5_H */ 50 | #define BMP085_RA_AC5_L 0xB3 /* AC5_L */ 51 | #define BMP085_RA_AC6_H 0xB4 /* AC6_H */ 52 | #define BMP085_RA_AC6_L 0xB5 /* AC6_L */ 53 | #define BMP085_RA_B1_H 0xB6 /* B1_H */ 54 | #define BMP085_RA_B1_L 0xB7 /* B1_L */ 55 | #define BMP085_RA_B2_H 0xB8 /* B2_H */ 56 | #define BMP085_RA_B2_L 0xB9 /* B2_L */ 57 | #define BMP085_RA_MB_H 0xBA /* MB_H */ 58 | #define BMP085_RA_MB_L 0xBB /* MB_L */ 59 | #define BMP085_RA_MC_H 0xBC /* MC_H */ 60 | #define BMP085_RA_MC_L 0xBD /* MC_L */ 61 | #define BMP085_RA_MD_H 0xBE /* MD_H */ 62 | #define BMP085_RA_MD_L 0xBF /* MD_L */ 63 | #define BMP085_RA_CONTROL 0xF4 /* CONTROL */ 64 | #define BMP085_RA_MSB 0xF6 /* MSB */ 65 | #define BMP085_RA_LSB 0xF7 /* LSB */ 66 | #define BMP085_RA_XLSB 0xF8 /* XLSB */ 67 | 68 | #define BMP085_MODE_TEMPERATURE 0x2E 69 | #define BMP085_MODE_PRESSURE_0 0x34 70 | #define BMP085_MODE_PRESSURE_1 0x74 71 | #define BMP085_MODE_PRESSURE_2 0xB4 72 | #define BMP085_MODE_PRESSURE_3 0xF4 73 | 74 | class BMP085 { 75 | public: 76 | BMP085(); 77 | BMP085(uint8_t address); 78 | 79 | void initialize(); 80 | bool testConnection(); 81 | 82 | /* calibration register methods */ 83 | int16_t getAC1(); 84 | int16_t getAC2(); 85 | int16_t getAC3(); 86 | uint16_t getAC4(); 87 | uint16_t getAC5(); 88 | uint16_t getAC6(); 89 | int16_t getB1(); 90 | int16_t getB2(); 91 | int16_t getMB(); 92 | int16_t getMC(); 93 | int16_t getMD(); 94 | 95 | /* CONTROL register methods */ 96 | uint8_t getControl(); 97 | void setControl(uint8_t value); 98 | 99 | /* MEASURE register methods */ 100 | uint16_t getMeasurement2(); // 16-bit data 101 | uint32_t getMeasurement3(); // 24-bit data 102 | uint8_t getMeasureDelayMilliseconds(uint8_t mode=0); 103 | uint16_t getMeasureDelayMicroseconds(uint8_t mode=0); 104 | 105 | // convenience methods 106 | void loadCalibration(); 107 | uint16_t getRawTemperature(); 108 | float getTemperatureC(); 109 | float getTemperatureF(); 110 | uint32_t getRawPressure(); 111 | float getPressure(); 112 | float getAltitude(float pressure, float seaLevelPressure=101325); 113 | 114 | private: 115 | uint8_t devAddr; 116 | uint8_t buffer[2]; 117 | 118 | bool calibrationLoaded; 119 | int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; 120 | uint16_t ac4, ac5, ac6; 121 | int32_t b5; 122 | uint8_t measureMode; 123 | }; 124 | 125 | #endif /* _BMP085_H_ */ 126 | -------------------------------------------------------------------------------- /libraries/HMC5883L/HMC5883L.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - HMC5883L I2C device class header file 2 | // Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B) 3 | // 6/12/2012 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2012-06-12 - fixed swapped Y/Z axes 8 | // 2011-08-22 - small Doxygen comment fixes 9 | // 2011-07-31 - initial release 10 | 11 | /* ============================================ 12 | I2Cdev device library code is placed under the MIT license 13 | Copyright (c) 2011 Jeff Rowberg 14 | 15 | Permission is hereby granted, free of charge, to any person obtaining a copy 16 | of this software and associated documentation files (the "Software"), to deal 17 | in the Software without restriction, including without limitation the rights 18 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | copies of the Software, and to permit persons to whom the Software is 20 | furnished to do so, subject to the following conditions: 21 | 22 | The above copyright notice and this permission notice shall be included in 23 | all copies or substantial portions of the Software. 24 | 25 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 26 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 28 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 30 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 31 | THE SOFTWARE. 32 | =============================================== 33 | */ 34 | 35 | #ifndef _HMC5883L_H_ 36 | #define _HMC5883L_H_ 37 | 38 | #include "I2Cdev.h" 39 | 40 | #define HMC5883L_ADDRESS 0x1E // this device only has one address 41 | #define HMC5883L_DEFAULT_ADDRESS 0x1E 42 | 43 | #define HMC5883L_RA_CONFIG_A 0x00 44 | #define HMC5883L_RA_CONFIG_B 0x01 45 | #define HMC5883L_RA_MODE 0x02 46 | #define HMC5883L_RA_DATAX_H 0x03 47 | #define HMC5883L_RA_DATAX_L 0x04 48 | #define HMC5883L_RA_DATAZ_H 0x05 49 | #define HMC5883L_RA_DATAZ_L 0x06 50 | #define HMC5883L_RA_DATAY_H 0x07 51 | #define HMC5883L_RA_DATAY_L 0x08 52 | #define HMC5883L_RA_STATUS 0x09 53 | #define HMC5883L_RA_ID_A 0x0A 54 | #define HMC5883L_RA_ID_B 0x0B 55 | #define HMC5883L_RA_ID_C 0x0C 56 | 57 | #define HMC5883L_CRA_AVERAGE_BIT 6 58 | #define HMC5883L_CRA_AVERAGE_LENGTH 2 59 | #define HMC5883L_CRA_RATE_BIT 4 60 | #define HMC5883L_CRA_RATE_LENGTH 3 61 | #define HMC5883L_CRA_BIAS_BIT 1 62 | #define HMC5883L_CRA_BIAS_LENGTH 2 63 | 64 | #define HMC5883L_AVERAGING_1 0x00 65 | #define HMC5883L_AVERAGING_2 0x01 66 | #define HMC5883L_AVERAGING_4 0x02 67 | #define HMC5883L_AVERAGING_8 0x03 68 | 69 | #define HMC5883L_RATE_0P75 0x00 70 | #define HMC5883L_RATE_1P5 0x01 71 | #define HMC5883L_RATE_3 0x02 72 | #define HMC5883L_RATE_7P5 0x03 73 | #define HMC5883L_RATE_15 0x04 74 | #define HMC5883L_RATE_30 0x05 75 | #define HMC5883L_RATE_75 0x06 76 | 77 | #define HMC5883L_BIAS_NORMAL 0x00 78 | #define HMC5883L_BIAS_POSITIVE 0x01 79 | #define HMC5883L_BIAS_NEGATIVE 0x02 80 | 81 | #define HMC5883L_CRB_GAIN_BIT 7 82 | #define HMC5883L_CRB_GAIN_LENGTH 3 83 | 84 | #define HMC5883L_GAIN_1370 0x00 85 | #define HMC5883L_GAIN_1090 0x01 86 | #define HMC5883L_GAIN_820 0x02 87 | #define HMC5883L_GAIN_660 0x03 88 | #define HMC5883L_GAIN_440 0x04 89 | #define HMC5883L_GAIN_390 0x05 90 | #define HMC5883L_GAIN_330 0x06 91 | #define HMC5883L_GAIN_220 0x07 92 | 93 | #define HMC5883L_MODEREG_BIT 1 94 | #define HMC5883L_MODEREG_LENGTH 2 95 | 96 | #define HMC5883L_MODE_CONTINUOUS 0x00 97 | #define HMC5883L_MODE_SINGLE 0x01 98 | #define HMC5883L_MODE_IDLE 0x02 99 | 100 | #define HMC5883L_STATUS_LOCK_BIT 1 101 | #define HMC5883L_STATUS_READY_BIT 0 102 | 103 | class HMC5883L { 104 | public: 105 | HMC5883L(); 106 | HMC5883L(uint8_t address); 107 | 108 | void initialize(); 109 | bool testConnection(); 110 | 111 | // CONFIG_A register 112 | uint8_t getSampleAveraging(); 113 | void setSampleAveraging(uint8_t averaging); 114 | uint8_t getDataRate(); 115 | void setDataRate(uint8_t rate); 116 | uint8_t getMeasurementBias(); 117 | void setMeasurementBias(uint8_t bias); 118 | 119 | // CONFIG_B register 120 | uint8_t getGain(); 121 | void setGain(uint8_t gain); 122 | 123 | // MODE register 124 | uint8_t getMode(); 125 | void setMode(uint8_t mode); 126 | 127 | // DATA* registers 128 | void getHeading(int16_t *x, int16_t *y, int16_t *z); 129 | int16_t getHeadingX(); 130 | int16_t getHeadingY(); 131 | int16_t getHeadingZ(); 132 | 133 | // STATUS register 134 | bool getLockStatus(); 135 | bool getReadyStatus(); 136 | 137 | // ID_* registers 138 | uint8_t getIDA(); 139 | uint8_t getIDB(); 140 | uint8_t getIDC(); 141 | 142 | private: 143 | uint8_t devAddr; 144 | uint8_t buffer[6]; 145 | uint8_t mode; 146 | }; 147 | 148 | #endif /* _HMC5883L_H_ */ 149 | -------------------------------------------------------------------------------- /Aircraft/control.h: -------------------------------------------------------------------------------- 1 | #include "pid.h" 2 | #include "ypr.h" 3 | #include "barometer.h" 4 | #include "compass.h" 5 | #include "config.h" 6 | #include "sonar.h" 7 | 8 | #define UNLOCK_SERVO 700//停机 9 | #define MAX_SERVO 2000//最大速率 10 | 11 | #define FRONT 0 12 | #define AFTER 1 13 | #define LEFT 2 14 | #define RIGHT 3 15 | 16 | #define PITCH 0 17 | #define ROLL 1 18 | #define YAW 2 19 | #define ELEVATION 3 20 | 21 | class Ccontrol 22 | { 23 | private: 24 | Cypr m_ypr; //读取MPU6050值,此陀螺仪有效的是倾斜值 25 | Cbarometer m_barometer; //读取气压,暂时不用,精度太差 26 | Ccompass m_compass; //读取HMC5883L值,此磁感获取航向 27 | Csonar m_sonar(); //声呐获取对地高度,相对气压值精准 28 | 29 | //正在着陆 30 | bool m_landing; 31 | // 32 | bool m_flying; 33 | //推力?油门? 34 | float m_power; 35 | //姿态调整时间 36 | ulong m_lastOptTime; 37 | 38 | public: 39 | //姿态 40 | //配置 41 | StructConfig configPID; 42 | //四轴电调频率 43 | float ServosValue[4]; 44 | //姿态目标 45 | float Targets[4]; 46 | //PID 47 | Cpid *m_PIDs[4]; 48 | 49 | //电调 50 | Servo Servos[4]; 51 | //电调管脚 52 | int ServoPins[4]; 53 | //每个电机的具体值有偏差 54 | int ServoMin[4]; 55 | 56 | Ccontrol(){ 57 | ServoPins[0] = 5; 58 | ServoPins[1] = 9; 59 | ServoPins[2] = 6; 60 | ServoPins[3] = 10; 61 | ServoMin[0] = 1049; 62 | ServoMin[1] = 1049; 63 | ServoMin[2] = 1014; 64 | ServoMin[3] = 796; 65 | for(int i =0;i<4;i++){ 66 | Servos[i].attach(ServoPins[i]); 67 | ServosValue[i] = UNLOCK_SERVO; 68 | Servos[i].writeMicroseconds(UNLOCK_SERVO); 69 | m_PIDs[i] = new Cpid (0,0,0,0); 70 | Targets[i] = 0; 71 | } 72 | m_power = 0; 73 | m_lastOptTime = 0; 74 | m_ypr.Init(); 75 | //获取rom配置 76 | ReadRom(&configPID); 77 | } 78 | 79 | ///////////////////刷新各传感器数据/////////////////////// 80 | float GetPitch(){ 81 | return m_ypr.GetPitchPoint() - configPID.AdjustPitch; 82 | } 83 | float GetRoll(){ 84 | return -(m_ypr.GetRollPoint() - configPID.AdjustRoll); 85 | } 86 | float GetYaw(){ 87 | return m_compass.GetPoint(); 88 | } 89 | float GetElevation(){ 90 | return m_power;//m_compass.GetPoint() - configPID.AdjustElevation; 91 | } 92 | 93 | void FlushSensors(){ 94 | GetPitch(); 95 | GetRoll(); 96 | GetYaw(); 97 | GetElevation(); 98 | } 99 | 100 | //姿态修改 101 | void MotionPitch(float v){ 102 | Targets[PITCH] = map (v, -100, 100, -30, 30);//映射为15度调整 103 | } 104 | 105 | void MotionRoll(float v){ 106 | Targets[ROLL] = map (v, -100, 100, -30, 30);//映射为15度调整 107 | } 108 | 109 | void MotionElevation(float v){ 110 | m_power += v/10; 111 | if (m_power<0) m_power = 0; 112 | } 113 | 114 | void MotionYaw(float v){ 115 | Targets[YAW] = GetYaw() + v/10; 116 | if (Targets[YAW]<0) Targets[YAW] += 360; 117 | if (Targets[YAW]>360) Targets[YAW] -= 360; 118 | } 119 | 120 | //飞行处理,姿态平衡 121 | void Flying(){ 122 | ulong currentTime = millis(); 123 | if(currentTime - m_lastOptTime > 20){ 124 | m_lastOptTime = currentTime; 125 | OptPitch(); 126 | OptRoll(); 127 | OptYaw(); 128 | WriteAllServos(); 129 | //OptElevation(); 130 | } 131 | } 132 | 133 | //自稳 134 | void SelfStationary(){ 135 | Targets[PITCH] = 0; 136 | Targets[ROLL] = 0; 137 | Flying(); 138 | } 139 | 140 | //着陆处理 141 | void Landing(){ 142 | m_power = 0; 143 | Targets[PITCH] = 0; 144 | Targets[ROLL] = 0; 145 | for(int i =0;i<4;i++){ 146 | ServosValue[i] = 0; 147 | Servos[i].writeMicroseconds(UNLOCK_SERVO); 148 | } 149 | } 150 | 151 | //配置写入rom 152 | void SaveConfig(){ 153 | WriteRom(&configPID); 154 | } 155 | 156 | //校准传感器 157 | void BalanceAdjust(){ 158 | float p = m_ypr.GetPitchPoint(); 159 | float r = m_ypr.GetRollPoint(); 160 | float y = m_compass.GetPoint(); 161 | for (int i = 0 ; i < 20; i++){ 162 | p = (p + m_ypr.GetPitchPoint()) / 2; 163 | r = (r + m_ypr.GetRollPoint()) / 2; 164 | y = (y + m_compass.GetPoint()) / 2; 165 | delay(20); 166 | } 167 | configPID.AdjustPitch = p; 168 | configPID.AdjustRoll = r; 169 | configPID.AdjustYaw = y; 170 | } 171 | 172 | //计算调整值 173 | //0.90 0.37 2.60 | 1.00 0.31 2.89 174 | void OptPitch() { 175 | m_PIDs[PITCH]->ReSetPID(configPID.PitchP, configPID.PitchI, configPID.PitchD); 176 | m_PIDs[PITCH]->ReSetPoint(Targets[PITCH]); 177 | float v = GetPitch(); 178 | v = m_PIDs[PITCH]->PIDCalc(v); 179 | ServosValue[FRONT] = m_power - v; 180 | ServosValue[AFTER] = m_power + v; 181 | } 182 | 183 | void OptRoll() { 184 | m_PIDs[ROLL]->ReSetPID(configPID.RollP, configPID.RollI, configPID.RollD); 185 | m_PIDs[ROLL]->ReSetPoint(Targets[ROLL]); 186 | float v = GetRoll(); 187 | v = m_PIDs[ROLL]->PIDCalc(v); 188 | ServosValue[LEFT] = m_power + v; 189 | ServosValue[RIGHT] = m_power - v; 190 | } 191 | 192 | void OptYaw() { 193 | m_PIDs[YAW]->ReSetPID(configPID.YawP, configPID.YawI, configPID.YawD); 194 | m_PIDs[YAW]->ReSetPoint(Targets[YAW] - 180.00); 195 | float v = GetYaw() - 180; 196 | v = m_PIDs[YAW]->PIDCalc(v); 197 | ServosValue[FRONT] += v; 198 | ServosValue[AFTER] += v; 199 | ServosValue[LEFT] -= v; 200 | ServosValue[RIGHT] -= v; 201 | } 202 | 203 | void OptElevation() { 204 | m_PIDs[ELEVATION]->ReSetPID(configPID.ElevationP, configPID.ElevationI, configPID.ElevationD); 205 | m_PIDs[ELEVATION]->ReSetPoint(Targets[ELEVATION]); 206 | float v = GetElevation(); 207 | v = m_PIDs[ELEVATION]->PIDCalc(v); 208 | ServosValue[FRONT] += v; 209 | ServosValue[AFTER] += v; 210 | ServosValue[LEFT] += v; 211 | ServosValue[RIGHT] += v; 212 | } 213 | 214 | void WriteAllServos(){ 215 | for(int i =0;i<4;i++){ 216 | if(ServosValue[i] > m_power + 100) ServosValue[i] = m_power + 100;//力度阈限 217 | if(ServosValue[i] < 0) ServosValue[i] = 0; 218 | if(ServosValue[i] > 1000) ServosValue[i] = 1000; 219 | int Motor = map(ServosValue[i], 0, 1000, ServoMin[i], MAX_SERVO); 220 | Servos[i].writeMicroseconds(Motor); 221 | } 222 | } 223 | }; 224 | -------------------------------------------------------------------------------- /Configer/configer/configer.vcxproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Debug 6 | Win32 7 | 8 | 9 | Release 10 | Win32 11 | 12 | 13 | 14 | {4E5BA5CD-93E9-40CB-B56F-07C14A97D424} 15 | configer 16 | MFCProj 17 | 18 | 19 | 20 | Application 21 | true 22 | v110 23 | MultiByte 24 | Dynamic 25 | 26 | 27 | Application 28 | false 29 | v110 30 | true 31 | MultiByte 32 | Dynamic 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | true 46 | 47 | 48 | false 49 | 50 | 51 | 52 | Use 53 | Level3 54 | Disabled 55 | WIN32;_WINDOWS;_DEBUG;%(PreprocessorDefinitions) 56 | true 57 | 58 | 59 | Windows 60 | true 61 | 62 | 63 | false 64 | true 65 | _DEBUG;%(PreprocessorDefinitions) 66 | 67 | 68 | 0x0804 69 | _DEBUG;%(PreprocessorDefinitions) 70 | $(IntDir);%(AdditionalIncludeDirectories) 71 | 72 | 73 | 74 | 75 | Level3 76 | Use 77 | MaxSpeed 78 | true 79 | true 80 | WIN32;_WINDOWS;NDEBUG;%(PreprocessorDefinitions) 81 | true 82 | 83 | 84 | Windows 85 | true 86 | true 87 | true 88 | 89 | 90 | false 91 | true 92 | NDEBUG;%(PreprocessorDefinitions) 93 | 94 | 95 | 0x0804 96 | NDEBUG;%(PreprocessorDefinitions) 97 | $(IntDir);%(AdditionalIncludeDirectories) 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | Create 115 | Create 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /RemoteControl/RemoteControl.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "stick.h" 10 | #include "connect.h" 11 | #include "lcd.h" 12 | #include "leds.h" 13 | #include "buttn.h" 14 | 15 | Cstick *stick; 16 | Cleds *leds; 17 | Clcd *lcd; 18 | Cbuttn *buttn; 19 | Cconnect *conn; 20 | 21 | void setup(){ 22 | Serial.begin(115200); 23 | stick = new Cstick(3,2,0,1); 24 | conn = new Cconnect (10, 9); 25 | leds = new Cleds(5, 3, 2); 26 | lcd = new Clcd(4, 3, 2); 27 | buttn = new Cbuttn (6, 7, 2); 28 | Serial.println ("init finish"); 29 | } 30 | 31 | float pitch = 0; 32 | float roll = 0; 33 | float yaw = 0; 34 | float ele = 0; 35 | float pitchP = 0; 36 | float pitchI = 0; 37 | float pitchD = 0; 38 | float rollP = 0; 39 | float rollI = 0; 40 | float rollD = 0; 41 | float yawP = 0; 42 | float yawI = 0; 43 | float yawD = 0; 44 | float eleP = 0; 45 | float eleI = 0; 46 | float eleD = 0; 47 | float power = 0; 48 | 49 | ulong motorA =0; 50 | ulong motorB =0; 51 | ulong motorC =0; 52 | ulong motorD =0; 53 | 54 | void loop(){ 55 | Buttons bs = {0, 0, 0, 0, 0}; 56 | buttn->Read(&bs); 57 | 58 | Coordinate cc; 59 | stick->Read(&cc); 60 | 61 | conn->GetValueByType(TYPE_POWER, power); 62 | 63 | conn->GetValueByType(TYPE_PITCH_CURRENT, pitch); 64 | conn->GetValueByType(TYPE_ROLL_CURRENT, roll); 65 | conn->GetValueByType(TYPE_YAW_CURRENT, yaw); 66 | conn->GetValueByType(TYPE_ELEVATION_CURRENT, ele); 67 | 68 | if (bs.b3){ 69 | conn->CommandByType(TYPE_WRITE_CONFIG); 70 | } 71 | if (bs.a1){ 72 | if (bs.a2){//飞行端解锁,飞行,自稳. 73 | leds->NetBlink(); 74 | conn->CommandByType(TYPE_FLY); 75 | if (!cc.lx && !cc.ly && !cc.rx && !cc.ry){ 76 | conn->CommandByType(TYPE_SELF_STATIONARY); 77 | }else{ 78 | Serial.print("stick:"); 79 | Serial.print("\t"); 80 | Serial.print(cc.lx); 81 | Serial.print("\t"); 82 | Serial.print(cc.ly); 83 | Serial.print("\t"); 84 | Serial.print(cc.rx); 85 | Serial.print("\t"); 86 | Serial.print(cc.ry); 87 | Serial.print("\n"); 88 | } 89 | if(cc.lx) { 90 | conn->SetValueByType(TYPE_ROLL_DESIRED, cc.lx); 91 | leds->RollBlink(); 92 | }else leds->RollOff(); 93 | if(cc.ly){ 94 | conn->SetValueByType(TYPE_PITCH_DESIRED, cc.ly); 95 | leds->PitchBlink(); 96 | }else leds->PitchOff(); 97 | if(cc.rx) { 98 | conn->SetValueByType(TYPE_YAW_DESIRED, cc.rx); 99 | leds->YawBlink(); 100 | }else leds->YawOff(); 101 | if(cc.ry) { 102 | conn->SetValueByType(TYPE_ELEVATION_DESIRED, cc.ry); 103 | leds->EleBlink(); 104 | }else leds->EleOff(); 105 | } 106 | else{ 107 | leds->NetOn(); 108 | conn->CommandByType(TYPE_LAND); 109 | } 110 | } 111 | else{ 112 | leds->NetOff(); 113 | conn->CommandByType(TYPE_LAND); 114 | 115 | if (bs.b1){ 116 | conn->CommandByType(TYPE_INIT_BALANCE); 117 | } 118 | conn->CommandByType(TYPE_TIME_SYN); 119 | } 120 | 121 | lcd->Show(pitch, roll, yaw, ele); 122 | lcd->ShowPower(power); 123 | lcd->SetSignal(conn->GetSuccessCount()); 124 | for(int i =0;i<10;i++) 125 | SerialControl(); 126 | } 127 | 128 | void SerialControl(){ 129 | switch(Serial.read()){ 130 | case '1':conn->GetValueByType(TYPE_PITCH_P, 0.01,pitchP);break; 131 | case 'q':conn->GetValueByType(TYPE_PITCH_P, -0.01,pitchP);break; 132 | case '2':conn->GetValueByType(TYPE_PITCH_I, 0.01,pitchI);break; 133 | case 'w':conn->GetValueByType(TYPE_PITCH_I, -0.01,pitchI);break; 134 | case '3':conn->GetValueByType(TYPE_PITCH_D, 0.01,pitchD);break; 135 | case 'e':conn->GetValueByType(TYPE_PITCH_D, -0.01,pitchD);break; 136 | case '#':conn->GetValueByType(TYPE_PITCH_D, 0.1,pitchD);break; 137 | case 'E':conn->GetValueByType(TYPE_PITCH_D, -0.1,pitchD);break; 138 | 139 | case '4':conn->GetValueByType(TYPE_ROLL_P, 0.01,rollP);break; 140 | case 'r':conn->GetValueByType(TYPE_ROLL_P, -0.01,rollP);break; 141 | case '5':conn->GetValueByType(TYPE_ROLL_I, 0.01,rollI);break; 142 | case 't':conn->GetValueByType(TYPE_ROLL_I, -0.01,rollI);break; 143 | case '6':conn->GetValueByType(TYPE_ROLL_D, 0.01,rollD);break; 144 | case 'y':conn->GetValueByType(TYPE_ROLL_D, -0.01,rollD);break; 145 | case '^':conn->GetValueByType(TYPE_ROLL_D, 0.1,rollD);break; 146 | case 'Y':conn->GetValueByType(TYPE_ROLL_D, -0.1,rollD);break; 147 | 148 | case '7':conn->GetValueByType(TYPE_YAW_P, 0.01,yawP);break; 149 | case 'u':conn->GetValueByType(TYPE_YAW_P, -0.01,yawP);break; 150 | case '8':conn->GetValueByType(TYPE_YAW_I, 0.01,yawI);break; 151 | case 'i':conn->GetValueByType(TYPE_YAW_I, -0.01,yawI);break; 152 | case '9':conn->GetValueByType(TYPE_YAW_D, 0.01,yawD);break; 153 | case 'o':conn->GetValueByType(TYPE_YAW_D, -0.01,yawD);break; 154 | case '0':conn->GetValueByType(TYPE_ELEVATION_P, 0.01,eleP);break; 155 | case 'p':conn->GetValueByType(TYPE_ELEVATION_P, -0.01,eleP);break; 156 | case '-':conn->GetValueByType(TYPE_ELEVATION_I, 0.01,eleI);break; 157 | case '[':conn->GetValueByType(TYPE_ELEVATION_I, -0.01,eleI);break; 158 | case '=':conn->GetValueByType(TYPE_ELEVATION_D, 0.01,eleD);break; 159 | case ']':conn->GetValueByType(TYPE_ELEVATION_D, -0.01,eleD);break; 160 | case -1: return; 161 | } 162 | 163 | conn->GetValueByType(TYPE_PITCH_P, 0,pitchP); 164 | conn->GetValueByType(TYPE_PITCH_I, 0,pitchI); 165 | conn->GetValueByType(TYPE_PITCH_D, 0,pitchD); 166 | conn->GetValueByType(TYPE_ROLL_P, 0,rollP); 167 | conn->GetValueByType(TYPE_ROLL_I, 0,rollI); 168 | conn->GetValueByType(TYPE_ROLL_D, 0,rollD); 169 | conn->GetValueByType(TYPE_YAW_P, 0,yawP); 170 | conn->GetValueByType(TYPE_YAW_I, 0,yawI); 171 | conn->GetValueByType(TYPE_YAW_D, 0,yawD); 172 | conn->GetValueByType(TYPE_ELEVATION_P, 0,eleP); 173 | conn->GetValueByType(TYPE_ELEVATION_I, 0,eleI); 174 | conn->GetValueByType(TYPE_ELEVATION_D, 0,eleD); 175 | Serial.print("pitchPID:"); 176 | Serial.print(pitchP);Serial.print("\t"); 177 | Serial.print(pitchI);Serial.print("\t"); 178 | Serial.print(pitchD);Serial.print("\t"); 179 | Serial.print("rollPID:"); 180 | Serial.print(rollP);Serial.print("\t"); 181 | Serial.print(rollI);Serial.print("\t"); 182 | Serial.print(rollD);Serial.print("\t"); 183 | Serial.print("yawPID:"); 184 | Serial.print(yawP);Serial.print("\t"); 185 | Serial.print(yawI);Serial.print("\t"); 186 | Serial.print(yawD);Serial.print("\t"); 187 | Serial.print("elePID:"); 188 | Serial.print(eleP);Serial.print("\t"); 189 | Serial.print(eleI);Serial.print("\t"); 190 | Serial.print(eleD);Serial.print("\n"); 191 | } 192 | -------------------------------------------------------------------------------- /libraries/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class 2 | // 10/7/2011 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added multiple output formats 7 | // - added seamless Fastwire support 8 | // 2011-10-07 - initial release 9 | 10 | /* ============================================ 11 | I2Cdev device library code is placed under the MIT license 12 | Copyright (c) 2011 Jeff Rowberg 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | =============================================== 32 | */ 33 | 34 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 35 | // for both classes must be in the include path of your project 36 | #include "I2Cdev.h" 37 | #include "MPU6050.h" 38 | 39 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 40 | // is used in I2Cdev.h 41 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 42 | #include "Wire.h" 43 | #endif 44 | 45 | // class default I2C address is 0x68 46 | // specific I2C addresses may be passed as a parameter here 47 | // AD0 low = 0x68 (default for InvenSense evaluation board) 48 | // AD0 high = 0x69 49 | MPU6050 accelgyro; 50 | //MPU6050 accelgyro(0x69); // <-- use for AD0 high 51 | 52 | int16_t ax, ay, az; 53 | int16_t gx, gy, gz; 54 | 55 | 56 | 57 | // uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated 58 | // list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read, 59 | // not so easy to parse, and slow(er) over UART. 60 | #define OUTPUT_READABLE_ACCELGYRO 61 | 62 | // uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit 63 | // binary, one right after the other. This is very fast (as fast as possible 64 | // without compression or data loss), and easy to parse, but impossible to read 65 | // for a human. 66 | //#define OUTPUT_BINARY_ACCELGYRO 67 | 68 | 69 | #define LED_PIN 13 70 | bool blinkState = false; 71 | 72 | void setup() { 73 | // join I2C bus (I2Cdev library doesn't do this automatically) 74 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 75 | Wire.begin(); 76 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 77 | Fastwire::setup(400, true); 78 | #endif 79 | 80 | // initialize serial communication 81 | // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but 82 | // it's really up to you depending on your project) 83 | Serial.begin(38400); 84 | 85 | // initialize device 86 | Serial.println("Initializing I2C devices..."); 87 | accelgyro.initialize(); 88 | 89 | // verify connection 90 | Serial.println("Testing device connections..."); 91 | Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 92 | 93 | // use the code below to change accel/gyro offset values 94 | /* 95 | Serial.println("Updating internal sensor offsets..."); 96 | // -76 -2359 1688 0 0 0 97 | Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 98 | Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 99 | Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 100 | Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 101 | Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 102 | Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 103 | Serial.print("\n"); 104 | accelgyro.setXGyroOffset(220); 105 | accelgyro.setYGyroOffset(76); 106 | accelgyro.setZGyroOffset(-85); 107 | Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 108 | Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 109 | Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 110 | Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 111 | Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 112 | Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 113 | Serial.print("\n"); 114 | */ 115 | 116 | // configure Arduino LED for 117 | pinMode(LED_PIN, OUTPUT); 118 | } 119 | 120 | void loop() { 121 | // read raw accel/gyro measurements from device 122 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 123 | 124 | // these methods (and a few others) are also available 125 | //accelgyro.getAcceleration(&ax, &ay, &az); 126 | //accelgyro.getRotation(&gx, &gy, &gz); 127 | 128 | #ifdef OUTPUT_READABLE_ACCELGYRO 129 | // display tab-separated accel/gyro x/y/z values 130 | Serial.print("a/g:\t"); 131 | Serial.print(ax); Serial.print("\t"); 132 | Serial.print(ay); Serial.print("\t"); 133 | Serial.print(az); Serial.print("\t"); 134 | Serial.print(gx); Serial.print("\t"); 135 | Serial.print(gy); Serial.print("\t"); 136 | Serial.println(gz); 137 | #endif 138 | 139 | #ifdef OUTPUT_BINARY_ACCELGYRO 140 | Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF)); 141 | Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF)); 142 | Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF)); 143 | Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF)); 144 | Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF)); 145 | Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF)); 146 | #endif 147 | 148 | // blink LED to indicate activity 149 | blinkState = !blinkState; 150 | digitalWrite(LED_PIN, blinkState); 151 | } 152 | -------------------------------------------------------------------------------- /libraries/MPU6050/helper_3dmath.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper 2 | // 6/5/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-05 - add 3D math helper file to DMP6 example sketch 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | #ifndef _HELPER_3DMATH_H_ 33 | #define _HELPER_3DMATH_H_ 34 | 35 | class Quaternion { 36 | public: 37 | float w; 38 | float x; 39 | float y; 40 | float z; 41 | 42 | Quaternion() { 43 | w = 1.0f; 44 | x = 0.0f; 45 | y = 0.0f; 46 | z = 0.0f; 47 | } 48 | 49 | Quaternion(float nw, float nx, float ny, float nz) { 50 | w = nw; 51 | x = nx; 52 | y = ny; 53 | z = nz; 54 | } 55 | 56 | Quaternion getProduct(Quaternion q) { 57 | // Quaternion multiplication is defined by: 58 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 59 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 60 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 61 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 62 | return Quaternion( 63 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 64 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 65 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 66 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 67 | } 68 | 69 | Quaternion getConjugate() { 70 | return Quaternion(w, -x, -y, -z); 71 | } 72 | 73 | float getMagnitude() { 74 | return sqrt(w*w + x*x + y*y + z*z); 75 | } 76 | 77 | void normalize() { 78 | float m = getMagnitude(); 79 | w /= m; 80 | x /= m; 81 | y /= m; 82 | z /= m; 83 | } 84 | 85 | Quaternion getNormalized() { 86 | Quaternion r(w, x, y, z); 87 | r.normalize(); 88 | return r; 89 | } 90 | }; 91 | 92 | class VectorInt16 { 93 | public: 94 | int16_t x; 95 | int16_t y; 96 | int16_t z; 97 | 98 | VectorInt16() { 99 | x = 0; 100 | y = 0; 101 | z = 0; 102 | } 103 | 104 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 105 | x = nx; 106 | y = ny; 107 | z = nz; 108 | } 109 | 110 | float getMagnitude() { 111 | return sqrt(x*x + y*y + z*z); 112 | } 113 | 114 | void normalize() { 115 | float m = getMagnitude(); 116 | x /= m; 117 | y /= m; 118 | z /= m; 119 | } 120 | 121 | VectorInt16 getNormalized() { 122 | VectorInt16 r(x, y, z); 123 | r.normalize(); 124 | return r; 125 | } 126 | 127 | void rotate(Quaternion *q) { 128 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 129 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 130 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 131 | // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 132 | 133 | // P_out = q * P_in * conj(q) 134 | // - P_out is the output vector 135 | // - q is the orientation quaternion 136 | // - P_in is the input vector (a*aReal) 137 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 138 | Quaternion p(0, x, y, z); 139 | 140 | // quaternion multiplication: q * p, stored back in p 141 | p = q -> getProduct(p); 142 | 143 | // quaternion multiplication: p * conj(q), stored back in p 144 | p = p.getProduct(q -> getConjugate()); 145 | 146 | // p quaternion is now [0, x', y', z'] 147 | x = p.x; 148 | y = p.y; 149 | z = p.z; 150 | } 151 | 152 | VectorInt16 getRotated(Quaternion *q) { 153 | VectorInt16 r(x, y, z); 154 | r.rotate(q); 155 | return r; 156 | } 157 | }; 158 | 159 | class VectorFloat { 160 | public: 161 | float x; 162 | float y; 163 | float z; 164 | 165 | VectorFloat() { 166 | x = 0; 167 | y = 0; 168 | z = 0; 169 | } 170 | 171 | VectorFloat(float nx, float ny, float nz) { 172 | x = nx; 173 | y = ny; 174 | z = nz; 175 | } 176 | 177 | float getMagnitude() { 178 | return sqrt(x*x + y*y + z*z); 179 | } 180 | 181 | void normalize() { 182 | float m = getMagnitude(); 183 | x /= m; 184 | y /= m; 185 | z /= m; 186 | } 187 | 188 | VectorFloat getNormalized() { 189 | VectorFloat r(x, y, z); 190 | r.normalize(); 191 | return r; 192 | } 193 | 194 | void rotate(Quaternion *q) { 195 | Quaternion p(0, x, y, z); 196 | 197 | // quaternion multiplication: q * p, stored back in p 198 | p = q -> getProduct(p); 199 | 200 | // quaternion multiplication: p * conj(q), stored back in p 201 | p = p.getProduct(q -> getConjugate()); 202 | 203 | // p quaternion is now [0, x', y', z'] 204 | x = p.x; 205 | y = p.y; 206 | z = p.z; 207 | } 208 | 209 | VectorFloat getRotated(Quaternion *q) { 210 | VectorFloat r(x, y, z); 211 | r.rotate(q); 212 | return r; 213 | } 214 | }; 215 | 216 | #endif /* _HELPER_3DMATH_H_ */ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | #Pterosauria 2 | 3 | 这是一个基于Arduino开发的四轴飞行器，整套飞行器分为飞控端和遥控端。 4 | 5 | 6 | TodoList: 7 | 8 | 四轴飞行器分为两种基本飞行模式：X模式和十模式，模式的区别体现在做俯仰和横滚的中心轴不一样，暂选择实现十模式。 9 | 10 | 四轴平衡控制：根据三轴角速度、陀螺仪数据， 控制状态分为 平衡态：俯仰(pitch)、横滚(roll)、航向(raw)、高度(elevation)，平衡态主要以锁定以上四种状态的绝对值实现。遥控端的指令分为：悬停、前平移、后平移、左平移、右平移、左旋、右旋、上升、下降。遥控指令与控制状态存在对应关系。表现在悬停即保持基本的四平衡态；前后平移与pitch目标值关联；左右平移与roll目标值关联；左右旋与raw目标值相关；升降与elevation目标值相关。遥控端的指令输入两个joystick对应，左摇杆上下为前后平移、左摇杆左右为左右平移、右摇杆上下为升降平移、右摇杆左右为左右旋。pitch/roll值来源：三轴加速度计、陀螺仪。raw值来源：电子罗盘。elevation值来源：气压传感器。 11 | 12 | 启停过程：以面朝地面的超声波测距来感知是否已悬空和是否已接地。 13 | 14 | 15 | 16 | #飞控端： 17 | 机架及动力部分：网购的成品四轴飞行器机架、无刷电机、电调、正反浆。 18 | 控制部分： 19 | arduino mini pro(Atamga328) 20 | 2.4GHz无线通讯模块(NRF24L01) 21 | 超声波测距模块(HC-SR04) 22 | 气压传感器(BMP180) 23 | 三轴加速度计、陀螺仪(MPU-6050) 24 | 电子罗盘(GY-271 HMC5883L) 25 | 26 | #遥控端： 27 | arduino mini pro(Atamga328) 28 | 2.4GHz无线通讯模块(NRF24L01) 29 | PS2游戏摇杆控制器(Joystick) 30 | 1602液晶显示屏 31 | 自锁、无锁按钮若干 32 | 各色led小灯若干 33 | 34 | 35 | ┌ └ ┐ ┘ ─ │ ├ ┤ ┬ ┴ ┼ 36 | 37 | #Arduino mini: 38 | 39 | ``` 40 | ┌──────┐ 41 | ┌───── USB ──────┤ PC │ 42 | │ └──────┘ 43 | ┌───────────***───────────┐ 44 | │ micro usb │ 45 | │ # USB─TTL 5v # │ 46 | └──┬───┬───┬───┬───┬───┬──┘ 47 | │ │ │ │ │ │ 48 | │ │ │ │ │ 49 | ┌───*───*───*───*───*───*───┐ 50 | │ DTR TXD RXD VCC GND GND │ 51 | ┤*TXD RAW*├ 52 | ┤*RXD GND*├ 53 | ┤*RESET RESET*├ 54 | ┤*GDN VCC*├ 55 | ┤*2 A3*├ 56 | ┤~3 ## Arduino mini ## A2*├ 57 | ┤*4 A1*├ 58 | ┤~5 A0*├ 59 | ┤~6 13*├ 60 | ┤*7 12*├ 61 | │ RES SCK MIS A6 A7 │ 62 | ┤*8 ──*┬──*┬──*┬──*┬──*┬ 11~├ 63 | │ GND│MOS│VCC│ A4│ A5│ │ 64 | ┘~9 ┐*─│┐*─│┐*─│┐*─│┐*─│ 10~└ 65 | └┐ │└┐ │└┐ │└┐ │└┐ │ 66 | 67 | ``` 68 | 69 | #Aircraft: 70 | ``` 71 | LinkForm: 72 | ┌───────┬───┬────┬────┬────┬────┬────┬────┬────┬────┬────┬────┐ 73 | │Arduino│GND│ 2 │ ~3 │ 4 │ ~5 │ ~6 │ 7 │ 8 │ ~9 │~10 │~11 │ 74 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 75 | │MPU6050│GND│ INT│ │ │ │ │ │ │ │ │ │ 76 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 77 | │ RF24 │GND│ │ │ │ │ │ CSN│ CE │ │ │MOSI│ 78 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 79 | │HC-SR04│GND│ │ │ │ │ │ │ │ │ │ │ 80 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 81 | │GY-271 │GND│ │ │ │ │ │ │ │ │ │ │ 82 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 83 | │ BMP180│GND│ │ │ │ │ │ │ │ │ │ │ 84 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 85 | │ Motor │GND│ │ │ │ MW │ MS │ │ │ MA │ MD │ │ 86 | └───────┴───┴────┴────┴────┴────┴────┴────┴────┴────┴────┴────┘ 87 | ┌───────┬───┬────┬────┬────┬────┬────┬────┬────┬────┬────┬────┐ 88 | │Arduino│VCC│ 12 │ 13 │ A0 │ A1 │ A2 │ A3 │ A4 │ A5 │ A6 │ A7 │ 89 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 90 | │MPU6050│VCC│ │ │ │ │ │ │ SDA│ SCL│ │ │ 91 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 92 | │ RF24 │VCC│MISO│ SCK│ │ │ │ │ │ │ │ │ 93 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 94 | │HC-SR04│VCC│ │ │ │ │ │ │ │ │TRIG│ECHO│ 95 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 96 | │GY-271 │VCC│ │ │ │ │ │ │ SDA│ SCL│ │ │ 97 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 98 | │ BMP180│VCC│ │ │ │ │ │ │ SDA│ SCL│ │ │ 99 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 100 | │ Motor │VCC│ │ │ │ │ │ │ │ │ │ │ 101 | └───────┴───┴────┴────┴────┴────┴────┴────┴────┴────┴────┴────┘ 102 | 103 | 104 | MUP6050: 105 | ┌───────────────────────────┐ 106 | VCC ┤*VCC ##MUP6050## │ 107 | GND ┤*GND │ 108 | A5 ┤*SCL │ 109 | A4 ┤*SDA │ 110 | ┤*XDA │ 111 | ┤*XCL │ 112 | ┤*ADO │ 113 | *2 ┤*INT │ 114 | └───────────────────────────┘ 115 | 116 | RF24: 117 | ┌───────────────────────────┐ 118 | ┤*IRQ ##RF24## MISO*├ 12 119 | 11 ┤*MOSI SCK*├ 13 120 | 7 ┤*CSN CE*├ 8 121 | VCC ┤*VCC GND*├ GND 122 | └───────────────────────────┘ 123 | 124 | ``` 125 | 126 | #RemoteControl: 127 | 128 | ``` 129 | LinkForm: 130 | ┌───────┬───┬────┬────┬────┬────┬────┬────┬────┬────┬────┬────┐ 131 | │Arduino│GND│ 2 │ ~3 │ 4 │ ~5 │ ~6 │ 7 │ 8 │ ~9 │~10 │~11 │ 132 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 133 | │ Jstick│GND│ INT│ │ │ │ │ │ │ │ │ │ 134 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 135 | │ Jstick│GND│ INT│ │ │ │ │ │ │ │ │ │ 136 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 137 | │ Jstick│GND│ INT│ │ │ │ │ │ │ │ │ │ 138 | └───────┴───┴────┴────┴────┴────┴────┴────┴────┴────┴────┴────┘ 139 | ┌───────┬───┬────┬────┬────┬────┬────┬────┬────┬────┬────┬────┐ 140 | │Arduino│VCC│ 12 │ 13 │ A0 │ A1 │ A2 │ A3 │ A4 │ A5 │ A6 │ A7 │ 141 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 142 | │ Jstick│GND│ INT│ │ │ │ │ │ │ │ │ │ 143 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 144 | │ Jstick│GND│ INT│ │ │ │ │ │ │ │ │ │ 145 | ├───────┼───┼────┼────┼────┼────┼────┼────┼────┼────┼────┼────┤ 146 | │ Jstick│VCC│ │ │ │ │ │ │ SDA│ SCL│ │ │ 147 | └───────┴───┴────┴────┴────┴────┴────┴────┴────┴────┴────┴────┘ 148 | 149 | Joystick: 150 | ┌───────────────────────────┐ 151 | ┤*VCC ##Joystick## │ 152 | ┤*GND ##Left## │ 153 | ┤*I-X │ 154 | ┤*I-Y │ 155 | ┤*I-Z │ 156 | └───────────────────────────┘ 157 | ┌───────────────────────────┐ 158 | ┤*VCC ##Joystick## │ 159 | ┤*GND ##Right## │ 160 | ┤*I-X │ 161 | ┤*I-Y │ 162 | ┤*I-Z │ 163 | └───────────────────────────┘ 164 | 165 | 1602: 166 | 167 | ┌──────────────────────────┐ ┌──────────────────────────┐ ┌──────────────────────────┐ 168 | ┤*QB ##74HC595## VCC* ├ ┤*QB ##74HC595## VCC* ├ ┤*QB ##74HC595## VCC* ├ 169 | ┤*QC QA* ├ ┤*QC QA* ├ ┤*QC QA* ├ 170 | ┤*QD SI* ├ ┤*QD SI* ├ ┤*QD SI* ├ 171 | ┤*QE OE* ├ ┤*QE OE* ├ ┤*QE OE* ├ 172 | ┤*QF RCK* ├ ┤*QF RCK* ├ ┤*QF RCK* ├ 173 | ┤*QG SCK* ├ ┤*QG SCK* ├ ┤*QG SCK* ├ 174 | ┤*QH SCLR* ├ ┤*QH SCLR* ├ ┤*QH SCLR* ├ 175 | ┤*GND SQH* ├ ┤*GND SQH* ├ ┤*GND SQH* ├ 176 | └──────────────────────────┘ └──────────────────────────┘ └──────────────────────────┘ 177 | 178 | 179 | ┌─┴───┴───┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴──┴───┴──┐ 180 | │VSS VDD VL RS R/W E D0 D1 D2 D3 D4 D5 D6 D7 BLA BLK │ 181 | │ ##1602## │ 182 | └────────────────────────────────────────────────────┘ 183 | 184 | ``` 185 | -------------------------------------------------------------------------------- /Aircraft/Aircraft.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 17 | #include "power.h" 18 | #include "control.h" 19 | #include "connect.h" 20 | 21 | //获取电源电压 22 | Cpower * pPower; 23 | //控制电机 24 | Ccontrol * pControl; 25 | //网络 26 | Cconnect * conn; 27 | 28 | //初始化飞控 29 | void setup() 30 | { 31 | Serial.begin(115200); 32 | conn = new Cconnect(8, 7); 33 | pControl = new Ccontrol(); 34 | pPower = new Cpower(A2); 35 | Serial.println("Initializing finish"); 36 | } 37 | 38 | //网络接收数据包 39 | NetStruct msg; 40 | ulong lastRecvTime =0; 41 | 42 | bool b_flying = false; 43 | 44 | ulong lastSecondHandleTime =0; 45 | int loopCount =0; 46 | //循环体 47 | void loop() 48 | { 49 | //刷新传感器 50 | pControl->FlushSensors(); 51 | ulong currentTime = millis(); 52 | loopCount++; 53 | if(currentTime-lastSecondHandleTime>1000){ 54 | lastSecondHandleTime=currentTime; 55 | Serial.println(loopCount); 56 | loopCount = 0; 57 | DebugShow(); 58 | } 59 | //循环过程中频繁访问网络,以免遥控端延迟丢包. 60 | for (int i = 0; i < 10; i++){ 61 | conn->Read(&msg); 62 | if(msg.type != TYPE_UNKNOW) lastRecvTime = currentTime; 63 | float vf = 0; 64 | int vi = 0; 65 | switch (msg.type){ 66 | case TYPE_TIME_SYN://获取飞控运行时间 67 | msg.value = *(ulong*)& currentTime; 68 | conn->Send(&msg); 69 | break; 70 | case TYPE_POWER://获取电池百分比 71 | vf = pPower->GetVoltage(); 72 | msg.value = *(ulong*)& vf; 73 | conn->Send(&msg); 74 | break; 75 | 76 | case TYPE_FLY://解锁电调处理飞行 77 | b_flying = true; 78 | msg.value = *(ulong*)& vf; 79 | conn->Send(&msg); 80 | break; 81 | case TYPE_SELF_STATIONARY://自稳 82 | b_flying = true; 83 | pControl->SelfStationary(); 84 | vf = 0; 85 | msg.value = *(ulong*)& vf; 86 | conn->Send(&msg); 87 | break; 88 | case TYPE_LAND://着陆 89 | b_flying = false; 90 | conn->Send(&msg); 91 | break; 92 | case TYPE_INIT_BALANCE://平衡校正 93 | vf = 0; 94 | msg.value = *(ulong*)& vf; 95 | conn->Send(&msg); 96 | pControl->BalanceAdjust(); 97 | break; 98 | //期望姿态调整 99 | case TYPE_PITCH_DESIRED://调整俯仰状态 100 | pControl->MotionPitch(*(float*)&msg.value); 101 | conn->Send(&msg); 102 | break; 103 | case TYPE_ROLL_DESIRED://调整横滚状态 104 | pControl->MotionRoll(*(float*)&msg.value); 105 | conn->Send(&msg); 106 | break; 107 | case TYPE_YAW_DESIRED://调整航向状态 108 | pControl->MotionYaw(*(float*)&msg.value); 109 | conn->Send(&msg); 110 | break; 111 | case TYPE_ELEVATION_DESIRED://调整高度状态 112 | pControl->MotionElevation(*(float*)&msg.value); 113 | conn->Send(&msg); 114 | break; 115 | 116 | //当前姿态获取 117 | case TYPE_PITCH_CURRENT://获取当前俯仰状态 118 | vf = pControl->GetPitch(); 119 | msg.value = *(ulong*)& vf; 120 | conn->Send(&msg); 121 | break; 122 | case TYPE_ROLL_CURRENT://获取当前横滚状态 123 | vf = pControl->GetRoll(); 124 | msg.value = *(ulong*)& vf; 125 | conn->Send(&msg); 126 | break; 127 | case TYPE_YAW_CURRENT://获取当前航向状态 128 | vf = pControl->GetYaw(); 129 | msg.value = *(ulong*)& vf; 130 | conn->Send(&msg); 131 | break; 132 | case TYPE_ELEVATION_CURRENT://获取当前高度状态 133 | vf = pControl->GetElevation(); 134 | msg.value = *(ulong*)& vf; 135 | conn->Send(&msg); 136 | break; 137 | 138 | /*************调试参数************/ 139 | case TYPE_READ_CONFIG://读取配置 140 | conn->Send(&msg); 141 | break; 142 | case TYPE_WRITE_CONFIG://写入配置 143 | pControl->SaveConfig(); 144 | conn->Send(&msg); 145 | break; 146 | 147 | case TYPE_MOTOR_A://调整/获取A电调频率 148 | pControl->ServosValue[0] += *(float*)&msg.value; 149 | msg.value = *(ulong*)&pControl->Servos[0]; 150 | conn->Send(&msg); 151 | break; 152 | case TYPE_MOTOR_B://调整/获取B电调频率 153 | pControl->ServosValue[1] += *(float*)&msg.value; 154 | msg.value = *(ulong*)&pControl->Servos[1]; 155 | conn->Send(&msg); 156 | break; 157 | case TYPE_MOTOR_C://调整/获取C电调频率 158 | pControl->ServosValue[2] += *(float*)&msg.value; 159 | msg.value = *(ulong*)&pControl->Servos[2]; 160 | conn->Send(&msg); 161 | break; 162 | case TYPE_MOTOR_D://调整/获取D电调频率 163 | pControl->ServosValue[3] += *(float*)&msg.value; 164 | msg.value = *(ulong*)&pControl->Servos[3]; 165 | conn->Send(&msg); 166 | break; 167 | 168 | case TYPE_PITCH_P: 169 | pControl->configPID.PitchP += *(float*)&msg.value; 170 | msg.value = *(ulong*)& pControl->configPID.PitchP; 171 | conn->Send(&msg); 172 | break; 173 | case TYPE_PITCH_I: 174 | pControl->configPID.PitchI += *(float*)&msg.value; 175 | msg.value = *(ulong*)& pControl->configPID.PitchI; 176 | conn->Send(&msg); 177 | break; 178 | case TYPE_PITCH_D: 179 | pControl->configPID.PitchD += *(float*)&msg.value; 180 | msg.value = *(ulong*)& pControl->configPID.PitchD; 181 | conn->Send(&msg); 182 | break; 183 | case TYPE_ROLL_P: 184 | pControl->configPID.RollP += *(float*)&msg.value; 185 | msg.value = *(ulong*)& pControl->configPID.RollP; 186 | conn->Send(&msg); 187 | break; 188 | case TYPE_ROLL_I: 189 | pControl->configPID.RollI += *(float*)&msg.value; 190 | msg.value = *(ulong*)& pControl->configPID.RollI; 191 | conn->Send(&msg); 192 | break; 193 | case TYPE_ROLL_D: 194 | pControl->configPID.RollD += *(float*)&msg.value; 195 | msg.value = *(ulong*)& pControl->configPID.RollD; 196 | conn->Send(&msg); 197 | break; 198 | case TYPE_YAW_P: 199 | pControl->configPID.YawP += *(float*)&msg.value; 200 | msg.value = *(ulong*)& pControl->configPID.YawP; 201 | conn->Send(&msg); 202 | break; 203 | case TYPE_YAW_I: 204 | pControl->configPID.YawI += *(float*)&msg.value; 205 | msg.value = *(ulong*)& pControl->configPID.YawI; 206 | conn->Send(&msg); 207 | break; 208 | case TYPE_YAW_D: 209 | pControl->configPID.YawD += *(float*)&msg.value; 210 | msg.value = *(ulong*)& pControl->configPID.YawD; 211 | conn->Send(&msg); 212 | break; 213 | case TYPE_ELEVATION_P: 214 | pControl->configPID.ElevationP += *(float*)&msg.value; 215 | msg.value = *(ulong*)& pControl->configPID.ElevationP; 216 | conn->Send(&msg); 217 | break; 218 | case TYPE_ELEVATION_I: 219 | pControl->configPID.ElevationI += *(float*)&msg.value; 220 | msg.value = *(ulong*)& pControl->configPID.ElevationI; 221 | conn->Send(&msg); 222 | break; 223 | case TYPE_ELEVATION_D: 224 | pControl->configPID.ElevationD += *(float*)&msg.value; 225 | msg.value = *(ulong*)& pControl->configPID.ElevationD; 226 | conn->Send(&msg); 227 | break; 228 | default: 229 | break; 230 | } 231 | } 232 | if (b_flying) 233 | pControl->Flying(); 234 | else 235 | pControl->Landing(); 236 | 237 | //1s未收到数据,在飞行状态则进入悬停. 238 | if (currentTime - lastRecvTime > 1000){ 239 | pControl->Landing(); 240 | //pControl->SelfStationary(); 241 | } 242 | } 243 | 244 | void DebugShow(){ 245 | if (!Serial.available()) return; 246 | switch(Serial.read()){ 247 | case '1': 248 | pControl->ServosValue[0]++; 249 | break; 250 | case '2': 251 | pControl->ServosValue[1]++; 252 | break; 253 | case '3': 254 | pControl->ServosValue[2]++; 255 | break; 256 | case '4': 257 | pControl->ServosValue[3]++; 258 | break; 259 | case 'q': 260 | pControl->ServosValue[0]--; 261 | break; 262 | case 'w': 263 | pControl->ServosValue[1]--; 264 | break; 265 | case 'e': 266 | pControl->ServosValue[2]--; 267 | break; 268 | case 'r': 269 | pControl->ServosValue[3]--; 270 | break; 271 | } 272 | Serial.print("prye"); 273 | Serial.print("\t"); 274 | Serial.print(pControl->GetPitch()); 275 | Serial.print("\t"); 276 | Serial.print(pControl->GetRoll()); 277 | Serial.print("\t"); 278 | Serial.print(pControl->GetYaw()); 279 | Serial.print("\t"); 280 | Serial.print(pControl->GetElevation()); 281 | Serial.print("\t"); 282 | Serial.print("servos"); 283 | for(int i =0;i<4;i++){ 284 | Serial.print("\t"); 285 | Serial.print(pControl->ServosValue[i]); 286 | } 287 | Serial.print("targets:"); 288 | for(int i =0;i<4;i++){ 289 | Serial.print(pControl->Targets[i]); 290 | Serial.print("\t"); 291 | } 292 | Serial.print("servos:"); 293 | for(int i =0;i<4;i++){ 294 | pControl->ServosValue[i] = UNLOCK_SERVO; 295 | Serial.print(pControl->ServosValue[i]); 296 | Serial.print("\t"); 297 | } 298 | Serial.print("\n"); 299 | 300 | } 301 | -------------------------------------------------------------------------------- /libraries/Mirf/Mirf.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Mirf 3 | * 4 | * Additional bug fixes and improvements 5 | * 11/03/2011: 6 | * Switched spi library. 7 | * 07/13/2010: 8 | * Added example to read a register 9 | * 11/12/2009: 10 | * Fix dataReady() to work correctly 11 | * Renamed keywords to keywords.txt ( for IDE ) and updated keyword list 12 | * Fixed client example code to timeout after one second and try again 13 | * when no response received from server 14 | * By: Nathan Isburgh 15 | * $Id: mirf.cpp 67 2010-07-13 13:25:53Z nisburgh $ 16 | * 17 | * 18 | * An Ardunio port of: 19 | * http://www.tinkerer.eu/AVRLib/nRF24L01 20 | * 21 | * Significant changes to remove depencence on interupts and auto ack support. 22 | * 23 | * Aaron Shrimpton 24 | * 25 | */ 26 | 27 | /* 28 | Copyright (c) 2007 Stefan Engelke 29 | 30 | Permission is hereby granted, free of charge, to any person 31 | obtaining a copy of this software and associated documentation 32 | files (the "Software"), to deal in the Software without 33 | restriction, including without limitation the rights to use, copy, 34 | modify, merge, publish, distribute, sublicense, and/or sell copies 35 | of the Software, and to permit persons to whom the Software is 36 | furnished to do so, subject to the following conditions: 37 | 38 | The above copyright notice and this permission notice shall be 39 | included in all copies or substantial portions of the Software. 40 | 41 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 42 | EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 43 | MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 44 | NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 45 | HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 46 | WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 47 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 48 | DEALINGS IN THE SOFTWARE. 49 | 50 | $Id: mirf.cpp 67 2010-07-13 13:25:53Z nisburgh $ 51 | */ 52 | 53 | #include "Mirf.h" 54 | // Defines for setting the MiRF registers for transmitting or receiving mode 55 | 56 | Nrf24l Mirf = Nrf24l(); 57 | 58 | Nrf24l::Nrf24l(){ 59 | cePin = 8; 60 | csnPin = 7; 61 | channel = 1; 62 | payload = 16; 63 | spi = NULL; 64 | } 65 | 66 | void Nrf24l::transferSync(uint8_t *dataout,uint8_t *datain,uint8_t len){ 67 | uint8_t i; 68 | for(i = 0;i < len;i++){ 69 | datain[i] = spi->transfer(dataout[i]); 70 | } 71 | } 72 | 73 | void Nrf24l::transmitSync(uint8_t *dataout,uint8_t len){ 74 | uint8_t i; 75 | for(i = 0;i < len;i++){ 76 | spi->transfer(dataout[i]); 77 | } 78 | } 79 | 80 | 81 | void Nrf24l::init() 82 | // Initializes pins to communicate with the MiRF module 83 | // Should be called in the early initializing phase at startup. 84 | { 85 | pinMode(cePin,OUTPUT); 86 | pinMode(csnPin,OUTPUT); 87 | 88 | ceLow(); 89 | csnHi(); 90 | 91 | // Initialize spi module 92 | spi->begin(); 93 | 94 | } 95 | 96 | 97 | void Nrf24l::config() 98 | // Sets the important registers in the MiRF module and powers the module 99 | // in receiving mode 100 | // NB: channel and payload must be set now. 101 | { 102 | // Set RF channel 103 | configRegister(RF_CH,channel); 104 | 105 | // Set length of incoming payload 106 | configRegister(RX_PW_P0, payload); 107 | configRegister(RX_PW_P1, payload); 108 | 109 | // Start receiver 110 | powerUpRx(); 111 | flushRx(); 112 | } 113 | 114 | void Nrf24l::setRADDR(uint8_t * adr) 115 | // Sets the receiving address 116 | { 117 | ceLow(); 118 | writeRegister(RX_ADDR_P1,adr,mirf_ADDR_LEN); 119 | ceHi(); 120 | } 121 | 122 | void Nrf24l::setTADDR(uint8_t * adr) 123 | // Sets the transmitting address 124 | { 125 | /* 126 | * RX_ADDR_P0 must be set to the sending addr for auto ack to work. 127 | */ 128 | 129 | writeRegister(RX_ADDR_P0,adr,mirf_ADDR_LEN); 130 | writeRegister(TX_ADDR,adr,mirf_ADDR_LEN); 131 | } 132 | 133 | extern bool Nrf24l::dataReady() 134 | // Checks if data is available for reading 135 | { 136 | // See note in getData() function - just checking RX_DR isn't good enough 137 | uint8_t status = getStatus(); 138 | 139 | // We can short circuit on RX_DR, but if it's not set, we still need 140 | // to check the FIFO for any pending packets 141 | if ( status & (1 << RX_DR) ) return 1; 142 | return !rxFifoEmpty(); 143 | } 144 | 145 | extern bool Nrf24l::rxFifoEmpty(){ 146 | uint8_t fifoStatus; 147 | 148 | readRegister(FIFO_STATUS,&fifoStatus,sizeof(fifoStatus)); 149 | return (fifoStatus & (1 << RX_EMPTY)); 150 | } 151 | 152 | 153 | 154 | extern void Nrf24l::getData(uint8_t * data) 155 | // Reads payload bytes into data array 156 | { 157 | csnLow(); // Pull down chip select 158 | spi->transfer( R_RX_PAYLOAD ); // Send cmd to read rx payload 159 | transferSync(data,data,payload); // Read payload 160 | csnHi(); // Pull up chip select 161 | // NVI: per product spec, p 67, note c: 162 | // "The RX_DR IRQ is asserted by a new packet arrival event. The procedure 163 | // for handling this interrupt should be: 1) read payload through SPI, 164 | // 2) clear RX_DR IRQ, 3) read FIFO_STATUS to check if there are more 165 | // payloads available in RX FIFO, 4) if there are more data in RX FIFO, 166 | // repeat from step 1)." 167 | // So if we're going to clear RX_DR here, we need to check the RX FIFO 168 | // in the dataReady() function 169 | configRegister(STATUS,(1<transfer(W_REGISTER | (REGISTER_MASK & reg)); 177 | spi->transfer(value); 178 | csnHi(); 179 | } 180 | 181 | void Nrf24l::readRegister(uint8_t reg, uint8_t * value, uint8_t len) 182 | // Reads an array of bytes from the given start position in the MiRF registers. 183 | { 184 | csnLow(); 185 | spi->transfer(R_REGISTER | (REGISTER_MASK & reg)); 186 | transferSync(value,value,len); 187 | csnHi(); 188 | } 189 | 190 | void Nrf24l::writeRegister(uint8_t reg, uint8_t * value, uint8_t len) 191 | // Writes an array of bytes into inte the MiRF registers. 192 | { 193 | csnLow(); 194 | spi->transfer(W_REGISTER | (REGISTER_MASK & reg)); 195 | transmitSync(value,len); 196 | csnHi(); 197 | } 198 | 199 | 200 | void Nrf24l::send(uint8_t * value) 201 | // Sends a data package to the default address. Be sure to send the correct 202 | // amount of bytes as configured as payload on the receiver. 203 | { 204 | uint8_t status; 205 | status = getStatus(); 206 | 207 | while (PTX) { 208 | status = getStatus(); 209 | 210 | if((status & ((1 << TX_DS) | (1 << MAX_RT)))){ 211 | PTX = 0; 212 | break; 213 | } 214 | } // Wait until last paket is send 215 | 216 | ceLow(); 217 | 218 | powerUpTx(); // Set to transmitter mode , Power up 219 | 220 | csnLow(); // Pull down chip select 221 | spi->transfer( FLUSH_TX ); // Write cmd to flush tx fifo 222 | csnHi(); // Pull up chip select 223 | 224 | csnLow(); // Pull down chip select 225 | spi->transfer( W_TX_PAYLOAD ); // Write cmd to write payload 226 | transmitSync(value,payload); // Write payload 227 | csnHi(); // Pull up chip select 228 | 229 | ceHi(); // Start transmission 230 | } 231 | 232 | /** 233 | * isSending. 234 | * 235 | * Test if chip is still sending. 236 | * When sending has finished return chip to listening. 237 | * 238 | */ 239 | 240 | bool Nrf24l::isSending(){ 241 | uint8_t status; 242 | if(PTX){ 243 | status = getStatus(); 244 | 245 | /* 246 | * if sending successful (TX_DS) or max retries exceded (MAX_RT). 247 | */ 248 | 249 | if((status & ((1 << TX_DS) | (1 << MAX_RT)))){ 250 | powerUpRx(); 251 | return false; 252 | } 253 | 254 | return true; 255 | } 256 | return false; 257 | } 258 | 259 | uint8_t Nrf24l::getStatus(){ 260 | uint8_t rv; 261 | readRegister(STATUS,&rv,1); 262 | return rv; 263 | } 264 | 265 | void Nrf24l::powerUpRx(){ 266 | PTX = 0; 267 | ceLow(); 268 | configRegister(CONFIG, mirf_CONFIG | ( (1<transfer( FLUSH_RX ); 276 | csnHi(); 277 | } 278 | 279 | void Nrf24l::powerUpTx(){ 280 | PTX = 1; 281 | configRegister(CONFIG, mirf_CONFIG | ( (1<= 100 107 | #include 108 | #else 109 | #include 110 | #include 111 | #endif 112 | 113 | #include 114 | #include 115 | 116 | // Shoudln't need to changed these values unless you have a specific need to do so. 117 | #define MAX_SENSOR_DISTANCE 500 // Maximum sensor distance can be as high as 500cm, no reason to wait for ping longer than sound takes to travel this distance and back. 118 | #define US_ROUNDTRIP_IN 146 // Microseconds (uS) it takes sound to travel round-trip 1 inch (2 inches total), uses integer to save compiled code space. 119 | #define US_ROUNDTRIP_CM 57 // Microseconds (uS) it takes sound to travel round-trip 1cm (2cm total), uses integer to save compiled code space. 120 | #define DISABLE_ONE_PIN false // Set to "true" to save up to 26 bytes of compiled code space if you're not using one pin sensor connections. 121 | 122 | // Probably shoudln't change these values unless you really know what you're doing. 123 | #define NO_ECHO 0 // Value returned if there's no ping echo within the specified MAX_SENSOR_DISTANCE or max_cm_distance. 124 | #define MAX_SENSOR_DELAY 18000 // Maximum uS it takes for sensor to start the ping (SRF06 is the highest measured, just under 18ms). 125 | #define ECHO_TIMER_FREQ 24 // Frequency to check for a ping echo (every 24uS is about 0.4cm accuracy). 126 | #define PING_MEDIAN_DELAY 29 // Millisecond delay between pings in the ping_median method. 127 | 128 | // Conversion from uS to distance (round result to nearest cm or inch). 129 | #define NewPingConvert(echoTime, conversionFactor) (max((echoTime + conversionFactor / 2) / conversionFactor, (echoTime ? 1 : 0))) 130 | 131 | 132 | class NewPing { 133 | public: 134 | NewPing(uint8_t trigger_pin, uint8_t echo_pin, int max_cm_distance = MAX_SENSOR_DISTANCE); 135 | unsigned int ping(); 136 | unsigned int ping_in(); 137 | unsigned int ping_cm(); 138 | unsigned int ping_median(uint8_t it = 5); 139 | unsigned int convert_in(unsigned int echoTime); 140 | unsigned int convert_cm(unsigned int echoTime); 141 | void ping_timer(void (*userFunc)(void)); 142 | boolean check_timer(); 143 | unsigned long ping_result; 144 | static void timer_us(unsigned int frequency, void (*userFunc)(void)); 145 | static void timer_ms(unsigned long frequency, void (*userFunc)(void)); 146 | static void timer_stop(); 147 | private: 148 | boolean ping_trigger(); 149 | boolean ping_wait_timer(); 150 | uint8_t _triggerBit; 151 | uint8_t _echoBit; 152 | volatile uint8_t *_triggerOutput; 153 | volatile uint8_t *_triggerMode; 154 | volatile uint8_t *_echoInput; 155 | unsigned int _maxEchoTime; 156 | unsigned long _max_time; 157 | static void timer_setup(); 158 | static void timer_ms_cntdwn(); 159 | }; 160 | 161 | 162 | #endif 163 | -------------------------------------------------------------------------------- /libraries/BMP085/BMP085.cpp: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - BMP085 I2C device class 2 | // Based on register information stored in the I2Cdevlib internal database 3 | // 2012-06-28 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2012-06-28 - initial release, dynamically built 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #include "BMP085.h" 34 | 35 | /** 36 | * Default constructor, uses default I2C device address. 37 | * @see BMP085_DEFAULT_ADDRESS 38 | */ 39 | BMP085::BMP085() { 40 | devAddr = BMP085_DEFAULT_ADDRESS; 41 | } 42 | 43 | /** 44 | * Specific address constructor. 45 | * @param address Specific device address 46 | * @see BMP085_DEFAULT_ADDRESS 47 | */ 48 | BMP085::BMP085(uint8_t address) { 49 | devAddr = address; 50 | } 51 | 52 | /** 53 | * Prepare device for normal usage. 54 | */ 55 | void BMP085::initialize() { 56 | // load sensor's calibration constants 57 | loadCalibration(); 58 | } 59 | 60 | /** 61 | * Verify the device is connected and available. 62 | */ 63 | bool BMP085::testConnection() { 64 | // test for a response, though this is very basic 65 | return I2Cdev::readByte(devAddr, BMP085_RA_AC1_H, buffer) == 1; 66 | } 67 | 68 | /* calibration register methods */ 69 | 70 | void BMP085::loadCalibration() { 71 | uint8_t buf2[22]; 72 | I2Cdev::readBytes(devAddr, BMP085_RA_AC1_H, 22, buf2); 73 | ac1 = ((int16_t)buf2[0] << 8) + buf2[1]; 74 | ac2 = ((int16_t)buf2[2] << 8) + buf2[3]; 75 | ac3 = ((int16_t)buf2[4] << 8) + buf2[5]; 76 | ac4 = ((uint16_t)buf2[6] << 8) + buf2[7]; 77 | ac5 = ((uint16_t)buf2[8] << 8) + buf2[9]; 78 | ac6 = ((uint16_t)buf2[10] << 8) + buf2[11]; 79 | b1 = ((int16_t)buf2[12] << 8) + buf2[13]; 80 | b2 = ((int16_t)buf2[14] << 8) + buf2[15]; 81 | mb = ((int16_t)buf2[16] << 8) + buf2[17]; 82 | mc = ((int16_t)buf2[18] << 8) + buf2[19]; 83 | md = ((int16_t)buf2[20] << 8) + buf2[21]; 84 | calibrationLoaded = true; 85 | } 86 | 87 | #ifdef BMP085_INCLUDE_INDIVIDUAL_CALIBRATION_ACCESS 88 | int16_t BMP085::getAC1() { 89 | if (calibrationLoaded) return ac1; 90 | I2Cdev::readBytes(devAddr, BMP085_RA_AC1_H, 2, buffer); 91 | return ((int16_t)buffer[1] << 8) + buffer[0]; 92 | } 93 | 94 | int16_t BMP085::getAC2() { 95 | if (calibrationLoaded) return ac2; 96 | I2Cdev::readBytes(devAddr, BMP085_RA_AC2_H, 2, buffer); 97 | return ((int16_t)buffer[1] << 8) + buffer[0]; 98 | } 99 | 100 | int16_t BMP085::getAC3() { 101 | if (calibrationLoaded) return ac3; 102 | I2Cdev::readBytes(devAddr, BMP085_RA_AC3_H, 2, buffer); 103 | return ((int16_t)buffer[1] << 8) + buffer[0]; 104 | } 105 | 106 | uint16_t BMP085::getAC4() { 107 | if (calibrationLoaded) return ac4; 108 | I2Cdev::readBytes(devAddr, BMP085_RA_AC4_H, 2, buffer); 109 | return ((uint16_t)buffer[1] << 8) + buffer[0]; 110 | } 111 | 112 | uint16_t BMP085::getAC5() { 113 | if (calibrationLoaded) return ac5; 114 | I2Cdev::readBytes(devAddr, BMP085_RA_AC5_H, 2, buffer); 115 | return ((uint16_t)buffer[1] << 8) + buffer[0]; 116 | } 117 | 118 | uint16_t BMP085::getAC6() { 119 | if (calibrationLoaded) return ac6; 120 | I2Cdev::readBytes(devAddr, BMP085_RA_AC6_H, 2, buffer); 121 | return ((uint16_t)buffer[1] << 8) + buffer[0]; 122 | } 123 | 124 | int16_t BMP085::getB1() { 125 | if (calibrationLoaded) return b1; 126 | I2Cdev::readBytes(devAddr, BMP085_RA_B1_H, 2, buffer); 127 | return ((int16_t)buffer[1] << 8) + buffer[0]; 128 | } 129 | 130 | int16_t BMP085::getB2() { 131 | if (calibrationLoaded) return b2; 132 | I2Cdev::readBytes(devAddr, BMP085_RA_B2_H, 2, buffer); 133 | return ((int16_t)buffer[1] << 8) + buffer[0]; 134 | } 135 | 136 | int16_t BMP085::getMB() { 137 | if (calibrationLoaded) return mb; 138 | I2Cdev::readBytes(devAddr, BMP085_RA_MB_H, 2, buffer); 139 | return ((int16_t)buffer[1] << 8) + buffer[0]; 140 | } 141 | 142 | int16_t BMP085::getMC() { 143 | if (calibrationLoaded) return mc; 144 | I2Cdev::readBytes(devAddr, BMP085_RA_MC_H, 2, buffer); 145 | return ((int16_t)buffer[1] << 8) + buffer[0]; 146 | } 147 | 148 | int16_t BMP085::getMD() { 149 | if (calibrationLoaded) return md; 150 | I2Cdev::readBytes(devAddr, BMP085_RA_MD_H, 2, buffer); 151 | return ((int16_t)buffer[1] << 8) + buffer[0]; 152 | } 153 | #endif 154 | 155 | /* control register methods */ 156 | 157 | uint8_t BMP085::getControl() { 158 | I2Cdev::readByte(devAddr, BMP085_RA_CONTROL, buffer); 159 | return buffer[0]; 160 | } 161 | void BMP085::setControl(uint8_t value) { 162 | I2Cdev::writeByte(devAddr, BMP085_RA_CONTROL, value); 163 | measureMode = value; 164 | } 165 | 166 | /* measurement register methods */ 167 | 168 | uint16_t BMP085::getMeasurement2() { 169 | I2Cdev::readBytes(devAddr, BMP085_RA_MSB, 2, buffer); 170 | return ((uint16_t)buffer[0] << 8) + buffer[1]; 171 | } 172 | uint32_t BMP085::getMeasurement3() { 173 | I2Cdev::readBytes(devAddr, BMP085_RA_MSB, 3, buffer); 174 | return ((uint32_t)buffer[0] << 16) + ((uint16_t)buffer[1] << 8) + buffer[2]; 175 | } 176 | uint8_t BMP085::getMeasureDelayMilliseconds(uint8_t mode) { 177 | if (mode == 0) mode = measureMode; 178 | if (measureMode == 0x2E) return 5; 179 | else if (measureMode == 0x34) return 5; 180 | else if (measureMode == 0x74) return 8; 181 | else if (measureMode == 0xB4) return 14; 182 | else if (measureMode == 0xF4) return 26; 183 | return 0; // invalid mode 184 | } 185 | uint16_t BMP085::getMeasureDelayMicroseconds(uint8_t mode) { 186 | if (mode == 0) mode = measureMode; 187 | if (measureMode == 0x2E) return 4500; 188 | else if (measureMode == 0x34) return 4500; 189 | else if (measureMode == 0x74) return 7500; 190 | else if (measureMode == 0xB4) return 13500; 191 | else if (measureMode == 0xF4) return 25500; 192 | return 0; // invalid mode 193 | } 194 | 195 | uint16_t BMP085::getRawTemperature() { 196 | if (measureMode == 0x2E) return getMeasurement2(); 197 | return 0; // wrong measurement mode for temperature request 198 | } 199 | 200 | float BMP085::getTemperatureC() { 201 | /* 202 | Datasheet formula: 203 | UT = raw temperature 204 | X1 = (UT - AC6) * AC5 / 2^15 205 | X2 = MC * 2^11 / (X1 + MD) 206 | B5 = X1 + X2 207 | T = (B5 + 8) / 2^4 208 | */ 209 | int32_t ut = getRawTemperature(); 210 | int32_t x1 = ((ut - (int32_t)ac6) * (int32_t)ac5) >> 15; 211 | int32_t x2 = ((int32_t)mc << 11) / (x1 + md); 212 | b5 = x1 + x2; 213 | return (float)((b5 + 8) >> 4) / 10.0f; 214 | } 215 | 216 | float BMP085::getTemperatureF() { 217 | return getTemperatureC() * 9.0f / 5.0f + 32; 218 | } 219 | 220 | uint32_t BMP085::getRawPressure() { 221 | if (measureMode & 0x34) return getMeasurement3() >> (8 - ((measureMode & 0xC0) >> 6)); 222 | return 0; // wrong measurement mode for pressure request 223 | } 224 | 225 | float BMP085::getPressure() { 226 | /* 227 | Datasheet forumla 228 | UP = raw pressure 229 | B6 = B5 - 4000 230 | X1 = (B2 * (B6 * B6 / 2^12)) / 2^11 231 | X2 = AC2 * B6 / 2^11 232 | X3 = X1 + X2 233 | B3 = ((AC1 * 4 + X3) << oss + 2) / 4 234 | X1 = AC3 * B6 / 2^13 235 | X2 = (B1 * (B6 * B6 / 2^12)) / 2^16 236 | X3 = ((X1 + X2) + 2) / 2^2 237 | B4 = AC4 * (unsigned long)(X3 + 32768) / 2^15 238 | B7 = ((unsigned long)UP - B3) * (50000 >> oss) 239 | if (B7 < 0x80000000) { p = (B7 * 2) / B4 } 240 | else { p = (B7 / B4) * 2 } 241 | X1 = (p / 2^8) * (p / 2^8) 242 | X1 = (X1 * 3038) / 2^16 243 | X2 = (-7357 * p) / 2^16 244 | p = p + (X1 + X2 + 3791) / 2^4 245 | */ 246 | uint32_t up = getRawPressure(); 247 | uint8_t oss = (measureMode & 0xC0) >> 6; 248 | int32_t p; 249 | int32_t b6 = b5 - 4000; 250 | int32_t x1 = ((int32_t)b2 * ((b6 * b6) >> 12)) >> 11; 251 | int32_t x2 = ((int32_t)ac2 * b6) >> 11; 252 | int32_t x3 = x1 + x2; 253 | int32_t b3 = ((((int32_t)ac1 * 4 + x3) << oss) + 2) >> 2; 254 | x1 = ((int32_t)ac3 * b6) >> 13; 255 | x2 = ((int32_t)b1 * ((b6 * b6) >> 12)) >> 16; 256 | x3 = ((x1 + x2) + 2) >> 2; 257 | uint32_t b4 = ((uint32_t)ac4 * (uint32_t)(x3 + 32768)) >> 15; 258 | uint32_t b7 = ((uint32_t)up - b3) * (uint32_t)(50000UL >> oss); 259 | if (b7 < 0x80000000) { 260 | p = (b7 << 1) / b4; 261 | } else { 262 | p = (b7 / b4) << 1; 263 | } 264 | x1 = (p >> 8) * (p >> 8); 265 | x1 = (x1 * 3038) >> 16; 266 | x2 = (-7357 * p) >> 16; 267 | return p + ((x1 + x2 + (int32_t)3791) >> 4); 268 | } 269 | 270 | float BMP085::getAltitude(float pressure, float seaLevelPressure) { 271 | return 44330 * (1.0 - pow(pressure / seaLevelPressure, 0.1903)); 272 | } -------------------------------------------------------------------------------- /libraries/MPU6050/Examples/MPU6050_DMP6/Processing/MPUTeapot.pde: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output 2 | // 6/20/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-20 - initial release 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | import processing.serial.*; 33 | import processing.opengl.*; 34 | import toxi.geom.*; 35 | import toxi.processing.*; 36 | 37 | // NOTE: requires ToxicLibs to be installed in order to run properly. 38 | // 1. Download from http://toxiclibs.org/downloads 39 | // 2. Extract into [userdir]/Processing/libraries 40 | // (location may be different on Mac/Linux) 41 | // 3. Run and bask in awesomeness 42 | 43 | ToxiclibsSupport gfx; 44 | 45 | Serial port; // The serial port 46 | char[] teapotPacket = new char[14]; // InvenSense Teapot packet 47 | int serialCount = 0; // current packet byte position 48 | int aligned = 0; 49 | int interval = 0; 50 | 51 | float[] q = new float[4]; 52 | Quaternion quat = new Quaternion(1, 0, 0, 0); 53 | 54 | float[] gravity = new float[3]; 55 | float[] euler = new float[3]; 56 | float[] ypr = new float[3]; 57 | 58 | void setup() { 59 | // 300px square viewport using OpenGL rendering 60 | size(300, 300, OPENGL); 61 | gfx = new ToxiclibsSupport(this); 62 | 63 | // setup lights and antialiasing 64 | lights(); 65 | smooth(); 66 | 67 | // display serial port list for debugging/clarity 68 | println(Serial.list()); 69 | 70 | // get the first available port (use EITHER this OR the specific port code below) 71 | String portName = Serial.list()[0]; 72 | 73 | // get a specific serial port (use EITHER this OR the first-available code above) 74 | //String portName = "COM4"; 75 | 76 | // open the serial port 77 | port = new Serial(this, portName, 115200); 78 | 79 | // send single character to trigger DMP init/start 80 | // (expected by MPU6050_DMP6 example Arduino sketch) 81 | port.write('r'); 82 | } 83 | 84 | void draw() { 85 | if (millis() - interval > 1000) { 86 | // resend single character to trigger DMP init/start 87 | // in case the MPU is halted/reset while applet is running 88 | port.write('r'); 89 | interval = millis(); 90 | } 91 | 92 | // black background 93 | background(0); 94 | 95 | // translate everything to the middle of the viewport 96 | pushMatrix(); 97 | translate(width / 2, height / 2); 98 | 99 | // 3-step rotation from yaw/pitch/roll angles (gimbal lock!) 100 | // ...and other weirdness I haven't figured out yet 101 | //rotateY(-ypr[0]); 102 | //rotateZ(-ypr[1]); 103 | //rotateX(-ypr[2]); 104 | 105 | // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!) 106 | // (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of 107 | // different coordinate system orientation assumptions between Processing 108 | // and InvenSense DMP) 109 | float[] axis = quat.toAxisAngle(); 110 | rotate(axis[0], -axis[1], axis[3], axis[2]); 111 | 112 | // draw main body in red 113 | fill(255, 0, 0, 200); 114 | box(10, 10, 200); 115 | 116 | // draw front-facing tip in blue 117 | fill(0, 0, 255, 200); 118 | pushMatrix(); 119 | translate(0, 0, -120); 120 | rotateX(PI/2); 121 | drawCylinder(0, 20, 20, 8); 122 | popMatrix(); 123 | 124 | // draw wings and tail fin in green 125 | fill(0, 255, 0, 200); 126 | beginShape(TRIANGLES); 127 | vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer 128 | vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer 129 | vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer 130 | vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer 131 | endShape(); 132 | beginShape(QUADS); 133 | vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); 134 | vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); 135 | vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); 136 | vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); 137 | vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); 138 | vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); 139 | endShape(); 140 | 141 | popMatrix(); 142 | } 143 | 144 | void serialEvent(Serial port) { 145 | interval = millis(); 146 | while (port.available() > 0) { 147 | int ch = port.read(); 148 | print((char)ch); 149 | if (ch == '$') {serialCount = 0;} // this will help with alignment 150 | if (aligned < 4) { 151 | // make sure we are properly aligned on a 14-byte packet 152 | if (serialCount == 0) { 153 | if (ch == '$') aligned++; else aligned = 0; 154 | } else if (serialCount == 1) { 155 | if (ch == 2) aligned++; else aligned = 0; 156 | } else if (serialCount == 12) { 157 | if (ch == '\r') aligned++; else aligned = 0; 158 | } else if (serialCount == 13) { 159 | if (ch == '\n') aligned++; else aligned = 0; 160 | } 161 | //println(ch + " " + aligned + " " + serialCount); 162 | serialCount++; 163 | if (serialCount == 14) serialCount = 0; 164 | } else { 165 | if (serialCount > 0 || ch == '$') { 166 | teapotPacket[serialCount++] = (char)ch; 167 | if (serialCount == 14) { 168 | serialCount = 0; // restart packet byte position 169 | 170 | // get quaternion from data packet 171 | q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; 172 | q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; 173 | q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; 174 | q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; 175 | for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; 176 | 177 | // set our toxilibs quaternion to new data 178 | quat.set(q[0], q[1], q[2], q[3]); 179 | 180 | /* 181 | // below calculations unnecessary for orientation only using toxilibs 182 | 183 | // calculate gravity vector 184 | gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); 185 | gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); 186 | gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; 187 | 188 | // calculate Euler angles 189 | euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); 190 | euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); 191 | euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); 192 | 193 | // calculate yaw/pitch/roll angles 194 | ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); 195 | ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); 196 | ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); 197 | 198 | // output various components for debugging 199 | //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); 200 | //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); 201 | //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); 202 | */ 203 | } 204 | } 205 | } 206 | } 207 | } 208 | 209 | void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) { 210 | float angle = 0; 211 | float angleIncrement = TWO_PI / sides; 212 | beginShape(QUAD_STRIP); 213 | for (int i = 0; i < sides + 1; ++i) { 214 | vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); 215 | vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); 216 | angle += angleIncrement; 217 | } 218 | endShape(); 219 | 220 | // If it is not a cone, draw the circular top cap 221 | if (topRadius != 0) { 222 | angle = 0; 223 | beginShape(TRIANGLE_FAN); 224 | 225 | // Center point 226 | vertex(0, 0, 0); 227 | for (int i = 0; i < sides + 1; i++) { 228 | vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); 229 | angle += angleIncrement; 230 | } 231 | endShape(); 232 | } 233 | 234 | // If it is not a cone, draw the circular bottom cap 235 | if (bottomRadius != 0) { 236 | angle = 0; 237 | beginShape(TRIANGLE_FAN); 238 | 239 | // Center point 240 | vertex(0, tall, 0); 241 | for (int i = 0; i < sides + 1; i++) { 242 | vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); 243 | angle += angleIncrement; 244 | } 245 | endShape(); 246 | } 247 | } 248 | -------------------------------------------------------------------------------- /libraries/NewPing/NewPing.cpp: -------------------------------------------------------------------------------- 1 | // --------------------------------------------------------------------------- 2 | // Created by Tim Eckel - teckel@leethost.com 3 | // Copyright 2012 License: GNU GPL v3 http://www.gnu.org/licenses/gpl-3.0.html 4 | // 5 | // See "NewPing.h" for purpose, syntax, version history, links, and more. 6 | // --------------------------------------------------------------------------- 7 | 8 | #include "NewPing.h" 9 | 10 | 11 | // --------------------------------------------------------------------------- 12 | // NewPing constructor 13 | // --------------------------------------------------------------------------- 14 | 15 | NewPing::NewPing(uint8_t trigger_pin, uint8_t echo_pin, int max_cm_distance) { 16 | _triggerBit = digitalPinToBitMask(trigger_pin); // Get the port register bitmask for the trigger pin. 17 | _echoBit = digitalPinToBitMask(echo_pin); // Get the port register bitmask for the echo pin. 18 | 19 | _triggerOutput = portOutputRegister(digitalPinToPort(trigger_pin)); // Get the output port register for the trigger pin. 20 | _echoInput = portInputRegister(digitalPinToPort(echo_pin)); // Get the input port register for the echo pin. 21 | 22 | _triggerMode = (uint8_t *) portModeRegister(digitalPinToPort(trigger_pin)); // Get the port mode register for the trigger pin. 23 | 24 | _maxEchoTime = min(max_cm_distance, MAX_SENSOR_DISTANCE) * US_ROUNDTRIP_CM + (US_ROUNDTRIP_CM / 2); // Calculate the maximum distance in uS. 25 | 26 | #if DISABLE_ONE_PIN == true 27 | *_triggerMode |= _triggerBit; // Set trigger pin to output. 28 | #endif 29 | } 30 | 31 | 32 | // --------------------------------------------------------------------------- 33 | // Standard ping methods 34 | // --------------------------------------------------------------------------- 35 | 36 | unsigned int NewPing::ping() { 37 | if (!ping_trigger()) return NO_ECHO; // Trigger a ping, if it returns false, return NO_ECHO to the calling function. 38 | while (*_echoInput & _echoBit) // Wait for the ping echo. 39 | if (micros() > _max_time) return NO_ECHO; // Stop the loop and return NO_ECHO (false) if we're beyond the set maximum distance. 40 | return (micros() - (_max_time - _maxEchoTime) - 5); // Calculate ping time, 5uS of overhead. 41 | } 42 | 43 | 44 | unsigned int NewPing::ping_in() { 45 | unsigned int echoTime = NewPing::ping(); // Calls the ping method and returns with the ping echo distance in uS. 46 | return NewPingConvert(echoTime, US_ROUNDTRIP_IN); // Convert uS to inches. 47 | } 48 | 49 | 50 | unsigned int NewPing::ping_cm() { 51 | unsigned int echoTime = NewPing::ping(); // Calls the ping method and returns with the ping echo distance in uS. 52 | return NewPingConvert(echoTime, US_ROUNDTRIP_CM); // Convert uS to centimeters. 53 | } 54 | 55 | 56 | unsigned int NewPing::ping_median(uint8_t it) { 57 | unsigned int uS[it], last; 58 | uint8_t j, i = 0; 59 | uS[0] = NO_ECHO; 60 | while (i < it) { 61 | last = ping(); // Send ping. 62 | if (last == NO_ECHO) { // Ping out of range. 63 | it--; // Skip, don't include as part of median. 64 | last = _maxEchoTime; // Adjust "last" variable so delay is correct length. 65 | } else { // Ping in range, include as part of median. 66 | if (i > 0) { // Don't start sort till second ping. 67 | for (j = i; j > 0 && uS[j - 1] < last; j--) // Insertion sort loop. 68 | uS[j] = uS[j - 1]; // Shift ping array to correct position for sort insertion. 69 | } else j = 0; // First ping is starting point for sort. 70 | uS[j] = last; // Add last ping to array in sorted position. 71 | i++; // Move to next ping. 72 | } 73 | if (i < it) delay(PING_MEDIAN_DELAY - (last >> 10)); // Millisecond delay between pings. 74 | } 75 | return (uS[it >> 1]); // Return the ping distance median. 76 | } 77 | 78 | 79 | // --------------------------------------------------------------------------- 80 | // Standard ping method support functions (not called directly) 81 | // --------------------------------------------------------------------------- 82 | 83 | boolean NewPing::ping_trigger() { 84 | #if DISABLE_ONE_PIN != true 85 | *_triggerMode |= _triggerBit; // Set trigger pin to output. 86 | #endif 87 | *_triggerOutput &= ~_triggerBit; // Set the trigger pin low, should already be low, but this will make sure it is. 88 | delayMicroseconds(4); // Wait for pin to go low, testing shows it needs 4uS to work every time. 89 | *_triggerOutput |= _triggerBit; // Set trigger pin high, this tells the sensor to send out a ping. 90 | delayMicroseconds(10); // Wait long enough for the sensor to realize the trigger pin is high. Sensor specs say to wait 10uS. 91 | *_triggerOutput &= ~_triggerBit; // Set trigger pin back to low. 92 | #if DISABLE_ONE_PIN != true 93 | *_triggerMode &= ~_triggerBit; // Set trigger pin to input (when using one Arduino pin this is technically setting the echo pin to input as both are tied to the same Arduino pin). 94 | #endif 95 | 96 | _max_time = micros() + MAX_SENSOR_DELAY; // Set a timeout for the ping to trigger. 97 | while (*_echoInput & _echoBit && micros() <= _max_time) {} // Wait for echo pin to clear. 98 | while (!(*_echoInput & _echoBit)) // Wait for ping to start. 99 | if (micros() > _max_time) return false; // Something went wrong, abort. 100 | 101 | _max_time = micros() + _maxEchoTime; // Ping started, set the timeout. 102 | return true; // Ping started successfully. 103 | } 104 | 105 | 106 | // --------------------------------------------------------------------------- 107 | // Timer interrupt ping methods (won't work with ATmega8 and ATmega128) 108 | // --------------------------------------------------------------------------- 109 | 110 | void NewPing::ping_timer(void (*userFunc)(void)) { 111 | if (!ping_trigger()) return; // Trigger a ping, if it returns false, return without starting the echo timer. 112 | timer_us(ECHO_TIMER_FREQ, userFunc); // Set ping echo timer check every ECHO_TIMER_FREQ uS. 113 | } 114 | 115 | 116 | boolean NewPing::check_timer() { 117 | if (micros() > _max_time) { // Outside the timeout limit. 118 | timer_stop(); // Disable timer interrupt 119 | return false; // Cancel ping timer. 120 | } 121 | 122 | if (!(*_echoInput & _echoBit)) { // Ping echo received. 123 | timer_stop(); // Disable timer interrupt 124 | ping_result = (micros() - (_max_time - _maxEchoTime) - 13); // Calculate ping time, 13uS of overhead. 125 | return true; // Return ping echo true. 126 | } 127 | 128 | return false; // Return false because there's no ping echo yet. 129 | } 130 | 131 | 132 | // --------------------------------------------------------------------------- 133 | // Timer2/Timer4 interrupt methods (can be used for non-ultrasonic needs) 134 | // --------------------------------------------------------------------------- 135 | 136 | // Variables used for timer functions 137 | void (*intFunc)(); 138 | void (*intFunc2)(); 139 | unsigned long _ms_cnt_reset; 140 | volatile unsigned long _ms_cnt; 141 | 142 | 143 | void NewPing::timer_us(unsigned int frequency, void (*userFunc)(void)) { 144 | timer_setup(); // Configure the timer interrupt. 145 | intFunc = userFunc; // User's function to call when there's a timer event. 146 | 147 | #if defined (__AVR_ATmega32U4__) // Use Timer4 for ATmega32U4 (Teensy/Leonardo). 148 | OCR4C = min((frequency>>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit. 149 | TIMSK4 = (1<>2) - 1, 255); // Every count is 4uS, so divide by 4 (bitwise shift right 2) subtract one, then make sure we don't go over 255 limit. 152 | TIMSK2 |= (1< 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #ifndef _I2CDEV_H_ 47 | #define _I2CDEV_H_ 48 | 49 | // ----------------------------------------------------------------------------- 50 | // I2C interface implementation setting 51 | // ----------------------------------------------------------------------------- 52 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 53 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 54 | 55 | // comment this out if you are using a non-optimal IDE/implementation setting 56 | // but want the compiler to shut up about it 57 | #define I2CDEV_IMPLEMENTATION_WARNINGS 58 | 59 | // ----------------------------------------------------------------------------- 60 | // I2C interface implementation options 61 | // ----------------------------------------------------------------------------- 62 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 63 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 64 | // ^^^ NBWire implementation is still buggy w/some interrupts! 65 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 66 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 67 | 68 | // ----------------------------------------------------------------------------- 69 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 70 | // ----------------------------------------------------------------------------- 71 | //#define I2CDEV_SERIAL_DEBUG 72 | 73 | #ifdef ARDUINO 74 | #if ARDUINO < 100 75 | #include "WProgram.h" 76 | #else 77 | #include "Arduino.h" 78 | #endif 79 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 80 | #include 81 | #endif 82 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 83 | #include 84 | #endif 85 | #endif 86 | 87 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 88 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 89 | 90 | class I2Cdev { 91 | public: 92 | I2Cdev(); 93 | 94 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 95 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 96 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 97 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 98 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 99 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 100 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 101 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 102 | 103 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 104 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 105 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 106 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 107 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 108 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 109 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 110 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 111 | 112 | static uint16_t readTimeout; 113 | }; 114 | 115 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 116 | ////////////////////// 117 | // FastWire 0.24 118 | // This is a library to help faster programs to read I2C devices. 119 | // Copyright(C) 2012 120 | // Francesco Ferrara 121 | ////////////////////// 122 | 123 | /* Master */ 124 | #define TW_START 0x08 125 | #define TW_REP_START 0x10 126 | 127 | /* Master Transmitter */ 128 | #define TW_MT_SLA_ACK 0x18 129 | #define TW_MT_SLA_NACK 0x20 130 | #define TW_MT_DATA_ACK 0x28 131 | #define TW_MT_DATA_NACK 0x30 132 | #define TW_MT_ARB_LOST 0x38 133 | 134 | /* Master Receiver */ 135 | #define TW_MR_ARB_LOST 0x38 136 | #define TW_MR_SLA_ACK 0x40 137 | #define TW_MR_SLA_NACK 0x48 138 | #define TW_MR_DATA_ACK 0x50 139 | #define TW_MR_DATA_NACK 0x58 140 | 141 | #define TW_OK 0 142 | #define TW_ERROR 1 143 | 144 | class Fastwire { 145 | private: 146 | static boolean waitInt(); 147 | 148 | public: 149 | static void setup(int khz, boolean pullup); 150 | static byte beginTransmission(byte device); 151 | static byte write(byte value); 152 | static byte writeBuf(byte device, byte address, byte *data, byte num); 153 | static byte readBuf(byte device, byte address, byte *data, byte num); 154 | static void reset(); 155 | static byte stop(); 156 | }; 157 | #endif 158 | 159 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 160 | // NBWire implementation based heavily on code by Gene Knight 161 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 162 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 163 | 164 | #define NBWIRE_BUFFER_LENGTH 32 165 | 166 | class TwoWire { 167 | private: 168 | static uint8_t rxBuffer[]; 169 | static uint8_t rxBufferIndex; 170 | static uint8_t rxBufferLength; 171 | 172 | static uint8_t txAddress; 173 | static uint8_t txBuffer[]; 174 | static uint8_t txBufferIndex; 175 | static uint8_t txBufferLength; 176 | 177 | // static uint8_t transmitting; 178 | static void (*user_onRequest)(void); 179 | static void (*user_onReceive)(int); 180 | static void onRequestService(void); 181 | static void onReceiveService(uint8_t*, int); 182 | 183 | public: 184 | TwoWire(); 185 | void begin(); 186 | void begin(uint8_t); 187 | void begin(int); 188 | void beginTransmission(uint8_t); 189 | //void beginTransmission(int); 190 | uint8_t endTransmission(uint16_t timeout=0); 191 | void nbendTransmission(void (*function)(int)) ; 192 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 193 | //uint8_t requestFrom(int, int); 194 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 195 | void send(uint8_t); 196 | void send(uint8_t*, uint8_t); 197 | //void send(int); 198 | void send(char*); 199 | uint8_t available(void); 200 | uint8_t receive(void); 201 | void onReceive(void (*)(int)); 202 | void onRequest(void (*)(void)); 203 | }; 204 | 205 | #define TWI_READY 0 206 | #define TWI_MRX 1 207 | #define TWI_MTX 2 208 | #define TWI_SRX 3 209 | #define TWI_STX 4 210 | 211 | #define TW_WRITE 0 212 | #define TW_READ 1 213 | 214 | #define TW_MT_SLA_NACK 0x20 215 | #define TW_MT_DATA_NACK 0x30 216 | 217 | #define CPU_FREQ 16000000L 218 | #define TWI_FREQ 100000L 219 | #define TWI_BUFFER_LENGTH 32 220 | 221 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 222 | 223 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 224 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 225 | #define TW_START 0x08 226 | #define TW_REP_START 0x10 227 | #define TW_MT_SLA_ACK 0x18 228 | #define TW_MT_SLA_NACK 0x20 229 | #define TW_MT_DATA_ACK 0x28 230 | #define TW_MT_DATA_NACK 0x30 231 | #define TW_MT_ARB_LOST 0x38 232 | #define TW_MR_ARB_LOST 0x38 233 | #define TW_MR_SLA_ACK 0x40 234 | #define TW_MR_SLA_NACK 0x48 235 | #define TW_MR_DATA_ACK 0x50 236 | #define TW_MR_DATA_NACK 0x58 237 | #define TW_ST_SLA_ACK 0xA8 238 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 239 | #define TW_ST_DATA_ACK 0xB8 240 | #define TW_ST_DATA_NACK 0xC0 241 | #define TW_ST_LAST_DATA 0xC8 242 | #define TW_SR_SLA_ACK 0x60 243 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 244 | #define TW_SR_GCALL_ACK 0x70 245 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 246 | #define TW_SR_DATA_ACK 0x80 247 | #define TW_SR_DATA_NACK 0x88 248 | #define TW_SR_GCALL_DATA_ACK 0x90 249 | #define TW_SR_GCALL_DATA_NACK 0x98 250 | #define TW_SR_STOP 0xA0 251 | #define TW_NO_INFO 0xF8 252 | #define TW_BUS_ERROR 0x00 253 | 254 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 255 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 256 | 257 | #ifndef sbi // set bit 258 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 259 | #endif // sbi 260 | 261 | #ifndef cbi // clear bit 262 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 263 | #endif // cbi 264 | 265 | extern TwoWire Wire; 266 | 267 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 268 | 269 | #endif /* _I2CDEV_H_ */ 270 | -------------------------------------------------------------------------------- /libraries/ShiftRegLCD/ShiftRegLCD.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | 3 | ShiftRegLCD - Shiftregister-based HD44780 compatible LCD library for Arduino 4 | 5 | Connects a Hitachi HD44780 compatible LCD using 2 or 3 pins from the Arduino, 6 | via an 8-bit shift register (SR from now on). 7 | 8 | 9 | Copyright (C) 2009-2012 raron 10 | 11 | GNU GPLv3 license 12 | 13 | This program is free software: you can redistribute it and/or modify 14 | it under the terms of the GNU General Public License as published by 15 | the Free Software Foundation, either version 3 of the License, or 16 | (at your option) any later version. 17 | 18 | This program is distributed in the hope that it will be useful, 19 | but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | GNU General Public License for more details. 22 | 23 | You should have received a copy of the GNU General Public License 24 | along with this program. If not, see . 25 | 26 | 27 | Contact: raronzen@gmail.com (Not always checked) 28 | 29 | Project homepage with schematics: 30 | http://code.google.com/p/arduinoshiftreglcd/ 31 | 32 | 33 | Acknowledgements: 34 | Based very much on the "official" LiquidCrystal library for the Arduino: 35 | http://arduino.cc/en/Reference/Libraries 36 | And also the improved version (with examples CustomChar1 and SerialDisplay) from LadyAda: 37 | http://web.alfredstate.edu/weimandn/arduino/LiquidCrystal_library/LiquidCrystal_index.html 38 | And inspired by this schematics from an unknown author 39 | (thanks to mircho on the arduino playground forum!): 40 | http://www.scienceprog.com/interfacing-lcd-to-atmega-using-two-wires/ 41 | And FINALLY a proper initialization routine for HD44780 compatible LCD's from 42 | http://web.alfredstate.edu/weimandn/lcd/lcd_initialization/lcd_initialization_index.html 43 | 44 | 45 | 46 | Modified to work serially with an 8-bit shift register and an LCD. 47 | 2-wire and 3-wire mode can use either a latched or unlatched shiftregister. 48 | (Preferably, though not neccessarily, unlatched for 2-wire mode) 49 | 50 | 51 | 52 | CONNECTION DESCRIPTION 53 | 54 | ShiftRegLCD 55 | 56 | Requires 2 or 3 pins from the Arduino. 57 | Arduino outputs to shift register data, clock and (optional) LCD Enable inputs. 58 | 59 | SR output: 60 | Bit #0 - N/C - not connected. Must be 0 in 2-wire mode. 61 | Bit #1 - LCD backlight control bit, or unused. 62 | Do not connect directly! Use a driver/ transistor! 63 | Bit #2 - connects to RS (Register Select) on the LCD 64 | Bits #3-6 - connects to LCD data inputs D4 - D7. 65 | Bit #7 - 2-wire mode: To cathode of the diode in the diode-resistor AND "gate" 66 | 3-wire mode: Unused 67 | 68 | LCD RW-pin hardwired to LOW (only writing to LCD). 69 | Busy Flag (BF, data bit D7) is not read. 70 | 71 | 72 | USAGE: 73 | 74 | ShiftRegLCD LCDobjectvariablename(Datapin, Clockpin, Enablepin or TWO_WIRE [, Lines [, Font]]]) 75 | 76 | where: 77 | Lines : Optional. Sets display nr of (logical) lines (1 or 2). 78 | Font : Optional. Sets small (normal 5x8 pix) or large (5x10 pix) font. 79 | Enablepin: Can be replaced by constant TWO_WIRE, if using only 2 wires. 80 | 81 | 82 | History 83 | 2012.02.15 raron - Fixed clear() and home() delay timings. 84 | 2012.02.09 raron - Final fixup on this version; a proper LCD initialization! 85 | 2012.02.07 raron - Cleaned up setCursor(), but no 16x4 yet. For that see 86 | the next version of this library (ShiftRegLCD123). 87 | 2012.01.31 raron - Updated for Arduino 1.0, thanks to suggestions 88 | from florian.fida. Also (I think) debugged setCursor method, but 89 | haven't coped for 16x4 LCD's yet since I don't specify LCD size. 90 | 2011.07.02 raron - Fixed a minor flaw in setCursor function. No functional 91 | change, just a bit more memory efficient. 92 | Thanks to CapnBry (from google code and github) who noticed it. 93 | URL to his version of shiftregLCD: 94 | https://github.com/CapnBry/HeaterMeter/commit/c6beba1b46b092ab0b33bcbd0a30a201fd1f28c1 95 | 2009.07.30 raron - minor corrections to the comments. 96 | Fixed timing to datasheet safe. Fixed keyword highlights. 97 | 2009.07.28 Mircho / raron - a new modification to the schematics, and a 98 | more streamlined interface 99 | 2009.07.27 raron - Thanks to an excellent suggestion from mircho from Arduino 100 | playground forum, the number of wires now required is only two! 101 | 2009.07.25 raron - Fixed comments. I really messed up the comments before 102 | posting this, so I had to fix it. 103 | Renamed a function, but no improvements or functional changes. 104 | 2009.07.23 raron - Incorporated some proper initialization routines 105 | inspired (lets say copy-paste-tweaked) from LiquidCrystal 106 | library improvements from LadyAda. 107 | 2009.05.23 raron - first version, but based mostly (as in almost verbatim) 108 | on the "official" LiquidCrystal library. 109 | */ 110 | 111 | #include "ShiftRegLCD.h" 112 | #include 113 | #include 114 | #include 115 | 116 | #if defined(ARDUINO) && ARDUINO >= 100 117 | #include "Arduino.h" 118 | #else 119 | #include "WProgram.h" 120 | #endif 121 | 122 | // Assuming 1 line 8 pixel high font 123 | ShiftRegLCD::ShiftRegLCD(uint8_t srdata, uint8_t srclock, uint8_t enable) 124 | { 125 | init(srdata, srclock, enable, 1, 0); 126 | } 127 | // Set nr. of lines, assume 8 pixel high font 128 | ShiftRegLCD::ShiftRegLCD(uint8_t srdata, uint8_t srclock, uint8_t enable, uint8_t lines) 129 | { 130 | init(srdata, srclock, enable, lines, 0); 131 | } 132 | // Set nr. of lines and font 133 | ShiftRegLCD::ShiftRegLCD(uint8_t srdata, uint8_t srclock, uint8_t enable, uint8_t lines, uint8_t font) 134 | { 135 | init(srdata, srclock, enable, lines, font); 136 | } 137 | 138 | 139 | void ShiftRegLCD::init(uint8_t srdata, uint8_t srclock, uint8_t enable, uint8_t lines, uint8_t font) 140 | { 141 | _two_wire = 0; 142 | _srdata_pin = srdata; 143 | _srclock_pin = srclock; 144 | _enable_pin = enable; 145 | _numlines = lines; 146 | _backlight = 0; // Backlight default off 147 | 148 | if (enable == TWO_WIRE) 149 | { 150 | _enable_pin = _srdata_pin; 151 | _two_wire = 1; 152 | } 153 | 154 | pinMode(_srclock_pin, OUTPUT); 155 | pinMode(_srdata_pin, OUTPUT); 156 | pinMode(_enable_pin, OUTPUT); 157 | 158 | digitalWrite(_srclock_pin, LOW); 159 | digitalWrite(_srdata_pin, LOW); 160 | digitalWrite(_enable_pin, LOW); 161 | 162 | if (lines>1) 163 | _displayfunction = LCD_4BITMODE | LCD_2LINE; 164 | else 165 | _displayfunction = LCD_4BITMODE | LCD_1LINE; 166 | 167 | 168 | // For some 1-line displays you can select a 10 pixel high font 169 | if (font != 0 && lines == 1) 170 | _displayfunction |= LCD_5x10DOTS; 171 | else 172 | _displayfunction |= LCD_5x8DOTS; 173 | 174 | // SEE PAGE 45/46 FOR INITIALIZATION SPECIFICATION! 175 | // according to datasheet, we need at least 40ms after power rises above 2.7V 176 | // before sending commands. Arduino can turn on way before 4.5V so we'll wait 177 | // more. 178 | // Also check this -excellent- guide on HD44780-based LCD initializations: 179 | // http://web.alfredstate.edu/weimandn/lcd/lcd_initialization/lcd_initialization_index.html 180 | // (From which this initialization and comments are taken from) 181 | // --------------------------------------------------------------------------- 182 | 183 | // Step 1. Power on, then delay > 100 ms 184 | 185 | // For some reason, delay() breaks execution of the rest of this library here 186 | //delay(200); 187 | for (int i=0; i<200; ++i) delayMicroseconds(1000); // Waits 200 ms just in case. 188 | 189 | // Step 2. Instruction 0011b (3h), then delay > 4.1 ms 190 | init4bits(LCD_FUNCTIONSET | LCD_8BITMODE); 191 | delayMicroseconds(4500); // wait more than 4.1ms 192 | 193 | // Step 3. Instruction 0011b (3h), then delay > 100 us 194 | // NOTE: send()/command() and init4bits() functions adds 60 us delay 195 | init4bits(LCD_FUNCTIONSET | LCD_8BITMODE); 196 | delayMicroseconds(40); 197 | 198 | // Step 4. Instruction 0011b (3h), then delay > 100 us 199 | init4bits(LCD_FUNCTIONSET | LCD_8BITMODE); 200 | delayMicroseconds(40); 201 | 202 | // Step 5. Instruction 0010b (2h), then delay > 100 us (speculation) 203 | // This is where display is set to 4-bit interface 204 | init4bits(LCD_FUNCTIONSET | LCD_4BITMODE); 205 | delayMicroseconds(40); 206 | 207 | // Step 6. Instruction 0010b (2h), then 1000b (8h), then delay > 53 us or check BF 208 | // Set nr. of logical lines (not neccesarily physical lines) and font size 209 | // (usually 2 lines and small font) 210 | command(LCD_FUNCTIONSET | _displayfunction); 211 | //delayMicroseconds(60); 212 | 213 | // Step 7. Instruction 0000b (0h), then 1000b (8h) then delay > 53 us or check BF 214 | command(LCD_DISPLAYCONTROL); 215 | //delayMicroseconds(60); 216 | 217 | // Step 8. Instruction 0000b (0h), then 0001b (1h) then delay > 3 ms or check BF 218 | // This is a Clear display instruction and takes a lot of time 219 | command(LCD_CLEARDISPLAY); 220 | delayMicroseconds(3000); 221 | 222 | // Step 9. Instruction 0000b (0h), then 0110b (6h), then delay > 53 us or check BF 223 | // Initialize to default text direction (for romance languages) 224 | _displaymode = LCD_ENTRYLEFT | LCD_ENTRYSHIFTDECREMENT; 225 | command(LCD_DISPLAYCONTROL | _displaymode); 226 | //delayMicroseconds(60); 227 | 228 | // Step 10. Not really a step, but initialization has ended. Except display is turned off. 229 | 230 | // Step 11. Instruction 0000b (0h), then 1100b (0Ch), then delay > 53 us or check BF 231 | // Turn display on, no cursor, no cursor blinking. 232 | _displaycontrol = LCD_DISPLAYON | LCD_CURSOROFF | LCD_BLINKOFF; 233 | command(LCD_DISPLAYCONTROL | _displaycontrol); 234 | //delayMicroseconds(60); 235 | } 236 | 237 | 238 | // For sending data via the shiftregister 239 | void ShiftRegLCD::send(uint8_t value, uint8_t mode) 240 | { 241 | uint8_t val1, val2; 242 | if (_two_wire) shiftOut(_srdata_pin, _srclock_pin, MSBFIRST, 0x00); // clear shiftregister 243 | mode = mode ? SR_RS_BIT : 0; // RS bit; LOW: command. HIGH: character. 244 | mode = _backlight ? mode | SR_BACKLIGHT : mode; // Backlight 245 | val1 = mode | SR_EN_BIT | ((value >> 1) & 0x78); // upper nibble 246 | val2 = mode | SR_EN_BIT | ((value << 3) & 0x78); // lower nibble 247 | shiftOut(_srdata_pin, _srclock_pin, MSBFIRST, val1); 248 | digitalWrite(_enable_pin, HIGH); 249 | delayMicroseconds(1); // enable pulse must be >450ns 250 | digitalWrite( _enable_pin, LOW ); 251 | if (_two_wire) shiftOut(_srdata_pin, _srclock_pin, MSBFIRST, 0x00); // clear shiftregister 252 | shiftOut(_srdata_pin, _srclock_pin, MSBFIRST, val2); 253 | digitalWrite(_enable_pin, HIGH); 254 | delayMicroseconds(1); // enable pulse must be >450ns 255 | digitalWrite(_enable_pin, LOW); 256 | // commands need > 37us to settle (for normal LCD with clock = 270 kHz) 257 | // Since LCD's may be slow (190 kHz clock = 53 us), we'll wait 60 us 258 | delayMicroseconds(60); 259 | } 260 | 261 | // For sending data when initializing the display to 4-bit 262 | void ShiftRegLCD::init4bits(uint8_t value) 263 | { 264 | uint8_t val1; 265 | if (_two_wire ) shiftOut (_srdata_pin, _srclock_pin, MSBFIRST, 0x00); // clear shiftregister 266 | val1 = SR_EN_BIT | ((value >> 1) & 0x78); 267 | shiftOut(_srdata_pin, _srclock_pin, MSBFIRST, val1); 268 | digitalWrite(_enable_pin, HIGH); 269 | delayMicroseconds(1); // enable pulse must be >450ns 270 | digitalWrite(_enable_pin, LOW); 271 | // commands need > 37us to settle (for normal LCD with clock = 270 kHz) 272 | // Since LCD's may be slow (190 kHz clock = 53 us), we'll wait 60 us 273 | delayMicroseconds(60); 274 | } 275 | 276 | 277 | void ShiftRegLCD::command(uint8_t value) 278 | { 279 | send(value, LOW); 280 | } 281 | 282 | #if defined(ARDUINO) && ARDUINO >= 100 283 | size_t ShiftRegLCD::write(uint8_t value) 284 | #else 285 | void ShiftRegLCD::write(uint8_t value) 286 | #endif 287 | { 288 | send(value, HIGH); 289 | } 290 | 291 | 292 | // ********** high level commands, for the user! ********** 293 | 294 | // Backlight On / Off control 295 | void ShiftRegLCD::backlightOn() 296 | { 297 | _backlight = 1; 298 | // Just to force a write to the shift register 299 | command(LCD_DISPLAYCONTROL | LCD_DISPLAYON); 300 | } 301 | 302 | void ShiftRegLCD::backlightOff() 303 | { 304 | _backlight = 0; 305 | // Just to force a write to the shift register 306 | command(LCD_DISPLAYCONTROL | LCD_DISPLAYON); 307 | } 308 | 309 | void ShiftRegLCD::clear() 310 | { 311 | command(LCD_CLEARDISPLAY); // clear display, set cursor position to zero 312 | delayMicroseconds(3000); // this command takes a long time! 313 | } 314 | 315 | void ShiftRegLCD::home() 316 | { 317 | command(LCD_RETURNHOME); // set cursor position to zero 318 | delayMicroseconds(3000); // this command takes a long time! 319 | } 320 | 321 | void ShiftRegLCD::setCursor(uint8_t col, uint8_t row) 322 | { 323 | // TODO: include support for 16x4 LCD's 324 | const uint8_t row_offsets[] = { 0x00, 0x40, 0x14, 0x54 }; 325 | if ( row >= _numlines ) 326 | row = _numlines-1; // we count rows starting w/0 327 | command(LCD_SETDDRAMADDR | (col + row_offsets[row])); 328 | } 329 | 330 | 331 | // Turn the display on/off (quickly) 332 | void ShiftRegLCD::noDisplay() { 333 | _displaycontrol &= ~LCD_DISPLAYON; 334 | command(LCD_DISPLAYCONTROL | _displaycontrol); 335 | } 336 | void ShiftRegLCD::display() { 337 | _displaycontrol |= LCD_DISPLAYON; 338 | command(LCD_DISPLAYCONTROL | _displaycontrol); 339 | } 340 | 341 | // Turns the underline cursor on/off 342 | void ShiftRegLCD::noCursor() { 343 | _displaycontrol &= ~LCD_CURSORON; 344 | command(LCD_DISPLAYCONTROL | _displaycontrol); 345 | } 346 | void ShiftRegLCD::cursor() { 347 | _displaycontrol |= LCD_CURSORON; 348 | command(LCD_DISPLAYCONTROL | _displaycontrol); 349 | } 350 | 351 | // Turn on and off the blinking cursor 352 | void ShiftRegLCD::noBlink() { 353 | _displaycontrol &= ~LCD_BLINKON; 354 | command(LCD_DISPLAYCONTROL | _displaycontrol); 355 | } 356 | void ShiftRegLCD::blink() { 357 | _displaycontrol |= LCD_BLINKON; 358 | command(LCD_DISPLAYCONTROL | _displaycontrol); 359 | } 360 | 361 | // These commands scroll the display without changing the RAM 362 | void ShiftRegLCD::scrollDisplayLeft(void) { 363 | command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVELEFT); 364 | } 365 | void ShiftRegLCD::scrollDisplayRight(void) { 366 | command(LCD_CURSORSHIFT | LCD_DISPLAYMOVE | LCD_MOVERIGHT); 367 | } 368 | 369 | // This is for text that flows Left to Right 370 | void ShiftRegLCD::shiftLeft(void) { 371 | _displaymode |= LCD_ENTRYLEFT; 372 | command(LCD_ENTRYMODESET | _displaymode); 373 | } 374 | 375 | // This is for text that flows Right to Left 376 | void ShiftRegLCD::shiftRight(void) { 377 | _displaymode &= ~LCD_ENTRYLEFT; 378 | command(LCD_ENTRYMODESET | _displaymode); 379 | } 380 | 381 | // This will 'right justify' text from the cursor 382 | void ShiftRegLCD::shiftIncrement(void) { 383 | _displaymode |= LCD_ENTRYSHIFTINCREMENT; 384 | command(LCD_ENTRYMODESET | _displaymode); 385 | } 386 | 387 | // This will 'left justify' text from the cursor 388 | void ShiftRegLCD::shiftDecrement(void) { 389 | _displaymode &= ~LCD_ENTRYSHIFTINCREMENT; 390 | command(LCD_ENTRYMODESET | _displaymode); 391 | } 392 | 393 | // Allows us to fill the first 8 CGRAM locations with custom characters 394 | void ShiftRegLCD::createChar(uint8_t location, uint8_t charmap[]) { 395 | location &= 0x7; // we only have 8 locations 0-7 396 | command(LCD_SETCGRAMADDR | location << 3); 397 | for (int i=0; i<8; i++) write(charmap[i]); 398 | command(LCD_SETDDRAMADDR); // Reset display to display text (from pos. 0) 399 | } 400 | 401 | -------------------------------------------------------------------------------- /libraries/HMC5883L/HMC5883L.cpp: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - HMC5883L I2C device class 2 | // Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B) 3 | // 6/12/2012 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // 2012-06-12 - fixed swapped Y/Z axes 8 | // 2011-08-22 - small Doxygen comment fixes 9 | // 2011-07-31 - initial release 10 | 11 | /* ============================================ 12 | I2Cdev device library code is placed under the MIT license 13 | Copyright (c) 2012 Jeff Rowberg 14 | 15 | Permission is hereby granted, free of charge, to any person obtaining a copy 16 | of this software and associated documentation files (the "Software"), to deal 17 | in the Software without restriction, including without limitation the rights 18 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | copies of the Software, and to permit persons to whom the Software is 20 | furnished to do so, subject to the following conditions: 21 | 22 | The above copyright notice and this permission notice shall be included in 23 | all copies or substantial portions of the Software. 24 | 25 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 26 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 28 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 30 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 31 | THE SOFTWARE. 32 | =============================================== 33 | */ 34 | 35 | #include "HMC5883L.h" 36 | 37 | /** Default constructor, uses default I2C address. 38 | * @see HMC5883L_DEFAULT_ADDRESS 39 | */ 40 | HMC5883L::HMC5883L() { 41 | devAddr = HMC5883L_DEFAULT_ADDRESS; 42 | } 43 | 44 | /** Specific address constructor. 45 | * @param address I2C address 46 | * @see HMC5883L_DEFAULT_ADDRESS 47 | * @see HMC5883L_ADDRESS 48 | */ 49 | HMC5883L::HMC5883L(uint8_t address) { 50 | devAddr = address; 51 | } 52 | 53 | /** Power on and prepare for general usage. 54 | * This will prepare the magnetometer with default settings, ready for single- 55 | * use mode (very low power requirements). Default settings include 8-sample 56 | * averaging, 15 Hz data output rate, normal measurement bias, a,d 1090 gain (in 57 | * terms of LSB/Gauss). Be sure to adjust any settings you need specifically 58 | * after initialization, especially the gain settings if you happen to be seeing 59 | * a lot of -4096 values (see the datasheet for mor information). 60 | */ 61 | void HMC5883L::initialize() { 62 | // write CONFIG_A register 63 | I2Cdev::writeByte(devAddr, HMC5883L_RA_CONFIG_A, 64 | (HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) | 65 | (HMC5883L_RATE_15 << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) | 66 | (HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1))); 67 | 68 | // write CONFIG_B register 69 | setGain(HMC5883L_GAIN_1090); 70 | 71 | // write MODE register 72 | setMode(HMC5883L_MODE_SINGLE); 73 | } 74 | 75 | /** Verify the I2C connection. 76 | * Make sure the device is connected and responds as expected. 77 | * @return True if connection is valid, false otherwise 78 | */ 79 | bool HMC5883L::testConnection() { 80 | if (I2Cdev::readBytes(devAddr, HMC5883L_RA_ID_A, 3, buffer) == 3) { 81 | return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3'); 82 | } 83 | return false; 84 | } 85 | 86 | // CONFIG_A register 87 | 88 | /** Get number of samples averaged per measurement. 89 | * @return Current samples averaged per measurement (0-3 for 1/2/4/8 respectively) 90 | * @see HMC5883L_AVERAGING_8 91 | * @see HMC5883L_RA_CONFIG_A 92 | * @see HMC5883L_CRA_AVERAGE_BIT 93 | * @see HMC5883L_CRA_AVERAGE_LENGTH 94 | */ 95 | uint8_t HMC5883L::getSampleAveraging() { 96 | I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer); 97 | return buffer[0]; 98 | } 99 | /** Set number of samples averaged per measurement. 100 | * @param averaging New samples averaged per measurement setting(0-3 for 1/2/4/8 respectively) 101 | * @see HMC5883L_RA_CONFIG_A 102 | * @see HMC5883L_CRA_AVERAGE_BIT 103 | * @see HMC5883L_CRA_AVERAGE_LENGTH 104 | */ 105 | void HMC5883L::setSampleAveraging(uint8_t averaging) { 106 | I2Cdev::writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging); 107 | } 108 | /** Get data output rate value. 109 | * The Table below shows all selectable output rates in continuous measurement 110 | * mode. All three channels shall be measured within a given output rate. Other 111 | * output rates with maximum rate of 160 Hz can be achieved by monitoring DRDY 112 | * interrupt pin in single measurement mode. 113 | * 114 | * Value | Typical Data Output Rate (Hz) 115 | * ------+------------------------------ 116 | * 0 | 0.75 117 | * 1 | 1.5 118 | * 2 | 3 119 | * 3 | 7.5 120 | * 4 | 15 (Default) 121 | * 5 | 30 122 | * 6 | 75 123 | * 7 | Not used 124 | * 125 | * @return Current rate of data output to registers 126 | * @see HMC5883L_RATE_15 127 | * @see HMC5883L_RA_CONFIG_A 128 | * @see HMC5883L_CRA_RATE_BIT 129 | * @see HMC5883L_CRA_RATE_LENGTH 130 | */ 131 | uint8_t HMC5883L::getDataRate() { 132 | I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer); 133 | return buffer[0]; 134 | } 135 | /** Set data output rate value. 136 | * @param rate Rate of data output to registers 137 | * @see getDataRate() 138 | * @see HMC5883L_RATE_15 139 | * @see HMC5883L_RA_CONFIG_A 140 | * @see HMC5883L_CRA_RATE_BIT 141 | * @see HMC5883L_CRA_RATE_LENGTH 142 | */ 143 | void HMC5883L::setDataRate(uint8_t rate) { 144 | I2Cdev::writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate); 145 | } 146 | /** Get measurement bias value. 147 | * @return Current bias value (0-2 for normal/positive/negative respectively) 148 | * @see HMC5883L_BIAS_NORMAL 149 | * @see HMC5883L_RA_CONFIG_A 150 | * @see HMC5883L_CRA_BIAS_BIT 151 | * @see HMC5883L_CRA_BIAS_LENGTH 152 | */ 153 | uint8_t HMC5883L::getMeasurementBias() { 154 | I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer); 155 | return buffer[0]; 156 | } 157 | /** Set measurement bias value. 158 | * @param bias New bias value (0-2 for normal/positive/negative respectively) 159 | * @see HMC5883L_BIAS_NORMAL 160 | * @see HMC5883L_RA_CONFIG_A 161 | * @see HMC5883L_CRA_BIAS_BIT 162 | * @see HMC5883L_CRA_BIAS_LENGTH 163 | */ 164 | void HMC5883L::setMeasurementBias(uint8_t bias) { 165 | I2Cdev::writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias); 166 | } 167 | 168 | // CONFIG_B register 169 | 170 | /** Get magnetic field gain value. 171 | * The table below shows nominal gain settings. Use the "Gain" column to convert 172 | * counts to Gauss. Choose a lower gain value (higher GN#) when total field 173 | * strength causes overflow in one of the data output registers (saturation). 174 | * The data output range for all settings is 0xF800-0x07FF (-2048 - 2047). 175 | * 176 | * Value | Field Range | Gain (LSB/Gauss) 177 | * ------+-------------+----------------- 178 | * 0 | +/- 0.88 Ga | 1370 179 | * 1 | +/- 1.3 Ga | 1090 (Default) 180 | * 2 | +/- 1.9 Ga | 820 181 | * 3 | +/- 2.5 Ga | 660 182 | * 4 | +/- 4.0 Ga | 440 183 | * 5 | +/- 4.7 Ga | 390 184 | * 6 | +/- 5.6 Ga | 330 185 | * 7 | +/- 8.1 Ga | 230 186 | * 187 | * @return Current magnetic field gain value 188 | * @see HMC5883L_GAIN_1090 189 | * @see HMC5883L_RA_CONFIG_B 190 | * @see HMC5883L_CRB_GAIN_BIT 191 | * @see HMC5883L_CRB_GAIN_LENGTH 192 | */ 193 | uint8_t HMC5883L::getGain() { 194 | I2Cdev::readBits(devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer); 195 | return buffer[0]; 196 | } 197 | /** Set magnetic field gain value. 198 | * @param gain New magnetic field gain value 199 | * @see getGain() 200 | * @see HMC5883L_RA_CONFIG_B 201 | * @see HMC5883L_CRB_GAIN_BIT 202 | * @see HMC5883L_CRB_GAIN_LENGTH 203 | */ 204 | void HMC5883L::setGain(uint8_t gain) { 205 | // use this method to guarantee that bits 4-0 are set to zero, which is a 206 | // requirement specified in the datasheet; it's actually more efficient than 207 | // using the I2Cdev.writeBits method 208 | I2Cdev::writeByte(devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1)); 209 | } 210 | 211 | // MODE register 212 | 213 | /** Get measurement mode. 214 | * In continuous-measurement mode, the device continuously performs measurements 215 | * and places the result in the data register. RDY goes high when new data is 216 | * placed in all three registers. After a power-on or a write to the mode or 217 | * configuration register, the first measurement set is available from all three 218 | * data output registers after a period of 2/fDO and subsequent measurements are 219 | * available at a frequency of fDO, where fDO is the frequency of data output. 220 | * 221 | * When single-measurement mode (default) is selected, device performs a single 222 | * measurement, sets RDY high and returned to idle mode. Mode register returns 223 | * to idle mode bit values. The measurement remains in the data output register 224 | * and RDY remains high until the data output register is read or another 225 | * measurement is performed. 226 | * 227 | * @return Current measurement mode 228 | * @see HMC5883L_MODE_CONTINUOUS 229 | * @see HMC5883L_MODE_SINGLE 230 | * @see HMC5883L_MODE_IDLE 231 | * @see HMC5883L_RA_MODE 232 | * @see HMC5883L_MODEREG_BIT 233 | * @see HMC5883L_MODEREG_LENGTH 234 | */ 235 | uint8_t HMC5883L::getMode() { 236 | I2Cdev::readBits(devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer); 237 | return buffer[0]; 238 | } 239 | /** Set measurement mode. 240 | * @param newMode New measurement mode 241 | * @see getMode() 242 | * @see HMC5883L_MODE_CONTINUOUS 243 | * @see HMC5883L_MODE_SINGLE 244 | * @see HMC5883L_MODE_IDLE 245 | * @see HMC5883L_RA_MODE 246 | * @see HMC5883L_MODEREG_BIT 247 | * @see HMC5883L_MODEREG_LENGTH 248 | */ 249 | void HMC5883L::setMode(uint8_t newMode) { 250 | // use this method to guarantee that bits 7-2 are set to zero, which is a 251 | // requirement specified in the datasheet; it's actually more efficient than 252 | // using the I2Cdev.writeBits method 253 | I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, mode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); 254 | mode = newMode; // track to tell if we have to clear bit 7 after a read 255 | } 256 | 257 | // DATA* registers 258 | 259 | /** Get 3-axis heading measurements. 260 | * In the event the ADC reading overflows or underflows for the given channel, 261 | * or if there is a math overflow during the bias measurement, this data 262 | * register will contain the value -4096. This register value will clear when 263 | * after the next valid measurement is made. Note that this method automatically 264 | * clears the appropriate bit in the MODE register if Single mode is active. 265 | * @param x 16-bit signed integer container for X-axis heading 266 | * @param y 16-bit signed integer container for Y-axis heading 267 | * @param z 16-bit signed integer container for Z-axis heading 268 | * @see HMC5883L_RA_DATAX_H 269 | */ 270 | void HMC5883L::getHeading(int16_t *x, int16_t *y, int16_t *z) { 271 | I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer); 272 | if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); 273 | *x = (((int16_t)buffer[0]) << 8) | buffer[1]; 274 | *y = (((int16_t)buffer[4]) << 8) | buffer[5]; 275 | *z = (((int16_t)buffer[2]) << 8) | buffer[3]; 276 | } 277 | /** Get X-axis heading measurement. 278 | * @return 16-bit signed integer with X-axis heading 279 | * @see HMC5883L_RA_DATAX_H 280 | */ 281 | int16_t HMC5883L::getHeadingX() { 282 | // each axis read requires that ALL axis registers be read, even if only 283 | // one is used; this was not done ineffiently in the code by accident 284 | I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer); 285 | if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); 286 | return (((int16_t)buffer[0]) << 8) | buffer[1]; 287 | } 288 | /** Get Y-axis heading measurement. 289 | * @return 16-bit signed integer with Y-axis heading 290 | * @see HMC5883L_RA_DATAY_H 291 | */ 292 | int16_t HMC5883L::getHeadingY() { 293 | // each axis read requires that ALL axis registers be read, even if only 294 | // one is used; this was not done ineffiently in the code by accident 295 | I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer); 296 | if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); 297 | return (((int16_t)buffer[4]) << 8) | buffer[5]; 298 | } 299 | /** Get Z-axis heading measurement. 300 | * @return 16-bit signed integer with Z-axis heading 301 | * @see HMC5883L_RA_DATAZ_H 302 | */ 303 | int16_t HMC5883L::getHeadingZ() { 304 | // each axis read requires that ALL axis registers be read, even if only 305 | // one is used; this was not done ineffiently in the code by accident 306 | I2Cdev::readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer); 307 | if (mode == HMC5883L_MODE_SINGLE) I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); 308 | return (((int16_t)buffer[2]) << 8) | buffer[3]; 309 | } 310 | 311 | // STATUS register 312 | 313 | /** Get data output register lock status. 314 | * This bit is set when this some but not all for of the six data output 315 | * registers have been read. When this bit is set, the six data output registers 316 | * are locked and any new data will not be placed in these register until one of 317 | * three conditions are met: one, all six bytes have been read or the mode 318 | * changed, two, the mode is changed, or three, the measurement configuration is 319 | * changed. 320 | * @return Data output register lock status 321 | * @see HMC5883L_RA_STATUS 322 | * @see HMC5883L_STATUS_LOCK_BIT 323 | */ 324 | bool HMC5883L::getLockStatus() { 325 | I2Cdev::readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer); 326 | return buffer[0]; 327 | } 328 | /** Get data ready status. 329 | * This bit is set when data is written to all six data registers, and cleared 330 | * when the device initiates a write to the data output registers and after one 331 | * or more of the data output registers are written to. When RDY bit is clear it 332 | * shall remain cleared for 250 us. DRDY pin can be used as an alternative to 333 | * the status register for monitoring the device for measurement data. 334 | * @return Data ready status 335 | * @see HMC5883L_RA_STATUS 336 | * @see HMC5883L_STATUS_READY_BIT 337 | */ 338 | bool HMC5883L::getReadyStatus() { 339 | I2Cdev::readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer); 340 | return buffer[0]; 341 | } 342 | 343 | // ID_* registers 344 | 345 | /** Get identification byte A 346 | * @return ID_A byte (should be 01001000, ASCII value 'H') 347 | */ 348 | uint8_t HMC5883L::getIDA() { 349 | I2Cdev::readByte(devAddr, HMC5883L_RA_ID_A, buffer); 350 | return buffer[0]; 351 | } 352 | /** Get identification byte B 353 | * @return ID_A byte (should be 00110100, ASCII value '4') 354 | */ 355 | uint8_t HMC5883L::getIDB() { 356 | I2Cdev::readByte(devAddr, HMC5883L_RA_ID_B, buffer); 357 | return buffer[0]; 358 | } 359 | /** Get identification byte C 360 | * @return ID_A byte (should be 00110011, ASCII value '3') 361 | */ 362 | uint8_t HMC5883L::getIDC() { 363 | I2Cdev::readByte(devAddr, HMC5883L_RA_ID_C, buffer); 364 | return buffer[0]; 365 | } 366 | --------------------------------------------------------------------------------