File content as of revision 9:f1bd96708a21:

#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;
}

void Quadcopter::print(char * myString){
    
    (*pcPntr_).printf(myString);    
    (*pcPntr_).printf("\n\r");

    }