Revision 4:1b985e622d26, committed 2019-03-20

Comitter:

AlexQian

Date:

Wed Mar 20 10:44:31 2019 +0000

Parent:

3:46535ec6d8b1

Child:

5:6a93542b8922

Commit message:

Fly_test

Changed in this revision

--- a/MPU6050.h	Mon Mar 18 03:39:29 2019 +0000
+++ b/MPU6050.h	Wed Mar 20 10:44:31 2019 +0000
@@ -153,7 +153,7 @@
 int Ascale = AFS_2G;
 
 //Set up I2C, (SDA,SCL)
-I2C i2c(PB_7,PB_6);
+
 
 //DigitalOut myled(LED1);
    
@@ -185,18 +185,21 @@
 int lastUpdate = 0, firstUpdate = 0, Now = 0;     // used to calculate integration interval                               // used to calculate integration interval
 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};            // vector to hold quaternion
 
-class MPU6050 {
+class mpu6050 {
  
     protected:
+    I2C i2c;
+    Timer t;
  
     public:
   //===================================================================================================================
 //====== Set of useful function to access acceleratio, gyroscope, and temperature data
 //===================================================================================================================
 
-    MPU6050(PinName SDA,PinName SCL)
+    mpu6050(PinName SDA,PinName SCL):i2c(SDA,SCL)
 {
-    I2C i2c(SDA,SCL);
+    t.start();
+    i2c.frequency(400000);
 }
     void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
 {
@@ -772,7 +775,7 @@
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count1;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
+    if (delt_t > 50) { // update LCD once per half-second independent of read rate
 
 //    pc.printf("ax = %f", 1000*ax); 
 //    pc.printf(" ay = %f", 1000*ay); 

--- a/main.cpp	Mon Mar 18 03:39:29 2019 +0000
+++ b/main.cpp	Wed Mar 20 10:44:31 2019 +0000
@@ -1,32 +1,31 @@
 #define HIGH 1
 #define LOW 0
 #include "mbed.h"
-#include "nRF24L01P.h"
-#include "MPU6050.h"
 #include <string>
 typedef bool boolean;
 typedef std::string String;
+#include "nRF24L01P.h"
+#include "MPU6050.h"
 
 char rxdata[12] ;
-float Yaw,Pitch,Roll,Power;
+float Yaw;
+float Pitch;
+float Roll;
+int Power;
 int Yaw_t;
 int Pitch_t;
 int Roll_t;
-int Power_t;
 long CycleCount;
 long ReceiveCount;
 
 #define NRF24_TRANSFER_SIZE 12
 nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PB_6,PB_5,PB_7);
-Serial Serial_1(PA_2,PA_3);
+Serial Serial_2(PA_2,PA_3);
+mpu6050 mpu6050_I2C_1(PB_9,PB_8);
 DigitalOut myDigitalOutPA_7(PA_7);
 DigitalOut myDigitalOutPA_6(PA_6);
-MPU6050 mpu6050(PB_9,PB_8);
+
 int main() {
-Serial_1.baud(9600);
-Serial_1.printf("Init\n");
-mpu6050.Init();
-
 nrf24_PB_15.powerUp();
 nrf24_PB_15.setTransferSize( NRF24_TRANSFER_SIZE );
 nrf24_PB_15.setReceiveMode();
@@ -34,31 +33,30 @@
 nrf24_PB_15.setTxAddress(12345ull);
 nrf24_PB_15.enable();
 
+Serial_2.baud(9600);
 
+mpu6050_I2C_1.Init();
 
 CycleCount = 0;
 ReceiveCount = 0;
-Serial_1.printf("Ready to Receive\n");
 while (true) {
 CycleCount = CycleCount + 1;
-
-mpu6050.receiveData(&Yaw,&Pitch,&Roll);  //读取传感器姿态 
-Serial_1.printf("Sensor Data: Yaw, Pitch, Roll: %.2f %.2f %.2f\n\r", Yaw, Pitch, Roll);
-
+mpu6050_I2C_1.receiveData(&Yaw,&Pitch,&Roll);
+Serial_2.printf("Yaw=%.2f, Pitch=%.2f, Roll=%.2f \n",Yaw,Pitch,Roll);
 if (nrf24_PB_15.readable()) {
 ReceiveCount = ReceiveCount + 1;
 myDigitalOutPA_7.write((ReceiveCount % 2 == 0));
 nrf24_PB_15.read( NRF24L01P_PIPE_P0, rxdata, NRF24_TRANSFER_SIZE);
 if (rxdata[0] == 82) {
-Power_t = rxdata[1];
+Power = rxdata[1];
 Yaw_t = rxdata[2] - 45;
 Pitch_t = rxdata[3] - 45;
 Roll_t = rxdata[4] - 45;
-Serial_1.printf("Target: Power=%d, Yaw=%d, Pitch=%d, Roll=%d \n",Power_t,Yaw_t,Pitch_t,Roll_t);
+Serial_2.printf("Power=%d, Yaw=%d, Pitch=%d, Roll=%d \n",Power,Yaw_t,Pitch_t,Roll_t);
 }
 }
 myDigitalOutPA_6.write((CycleCount % 2 == 0));
-wait(0.01);
+wait(0.05);
 }
 
 }
\ No newline at end of file