Diff: quadcopter.cpp

Revision:

9:f1bd96708a21

Parent:

8:326e7009ce0c

Child:

10:e7d1801e966a

--- a/quadcopter.cpp	Sat Apr 02 13:47:58 2016 +0000
+++ b/quadcopter.cpp	Sat Apr 02 14:54:46 2016 +0000
@@ -1,16 +1,68 @@
 #include "quadcopter.h"
- 
+#include "sensor.h"
+#include "Adafruit_9DOF.h"
+#include "Serial_base.h"
+
+
 // Date constructor
 Quadcopter::Quadcopter()
 {
     setState(0.0,0.0,0.0);
-    
+
+    dof_ = Adafruit_9DOF();
+    accel_ = Adafruit_LSM303_Accel_Unified(30301);
+    mag_ = Adafruit_LSM303_Mag_Unified(30302);
+    gyro_ = Adafruit_L3GD20_Unified(20);
+    //  pc.printf("Entering control loop\n\r");
+}
+
+
+
+void Quadcopter::initAllSensors()
+{
+    print("Initializing Sensors with class method");
+
+    initSensors(accel_, mag_, gyro_,offsetAngRate_);  // IMU
 }
- 
+
+
+void Quadcopter::readSensorValues()
+{
+    accel_.getEvent(&accel_event_);
+    if (dof_.accelGetOrientation(&accel_event_, &orientation_)) {
+        /* 'orientation' should have valid .roll and .pitch fields */
+        //s_com->print(("Roll: "));
+        //s_com->print(orientation.roll);
+        //s_com->print(("; "));
+        //s_com->print(("Pitch: "));
+        //s_com->print(orientation.pitch);
+        //s_com->print((";\t"));
+    }
+    /* Calculate the heading using the magnetometer */
+    mag_.getEvent(&mag_event_);
+    if (dof_.magGetOrientation(SENSOR_AXIS_Z, &mag_event_, &orientation_)) {
+        /* 'orientation' should have valid .heading data now */
+        //s_com->print(("Heading: "));
+        //s_com->print(orientation.heading);
+        //  s_com->print((";\r\n"));
+    }
+
+
+
+}
+
+
 // Date member function
 void Quadcopter::setState(double phi, double theta, double psi)
 {
     state_.phi = phi;
     state_.theta = theta;
     state_.psi = psi;
-}
\ No newline at end of file
+}
+
+void Quadcopter::print(char * myString){
+    
+    (*pcPntr_).printf(myString);    
+    (*pcPntr_).printf("\n\r");
+
+    }
\ No newline at end of file