David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

quadcopter.cpp@9:f1bd96708a21, 2016-04-02 (annotated)

Committer:
ivo_david_michelle
Date:
Sat Apr 02 14:54:46 2016 +0000
Revision:
9:f1bd96708a21
Parent:
8:326e7009ce0c
Child:
10:e7d1801e966a
implemented printing and sensor reading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 6:6f3ffd97d808 1 #include "quadcopter.h"
ivo_david_michelle 9:f1bd96708a21 2 #include "sensor.h"
ivo_david_michelle 9:f1bd96708a21 3 #include "Adafruit_9DOF.h"
ivo_david_michelle 9:f1bd96708a21 4 #include "Serial_base.h"
ivo_david_michelle 9:f1bd96708a21 5
ivo_david_michelle 9:f1bd96708a21 6
ivo_david_michelle 6:6f3ffd97d808 7 // Date constructor
ivo_david_michelle 6:6f3ffd97d808 8 Quadcopter::Quadcopter()
ivo_david_michelle 6:6f3ffd97d808 9 {
ivo_david_michelle 8:326e7009ce0c 10 setState(0.0,0.0,0.0);
ivo_david_michelle 9:f1bd96708a21 11
ivo_david_michelle 9:f1bd96708a21 12 dof_ = Adafruit_9DOF();
ivo_david_michelle 9:f1bd96708a21 13 accel_ = Adafruit_LSM303_Accel_Unified(30301);
ivo_david_michelle 9:f1bd96708a21 14 mag_ = Adafruit_LSM303_Mag_Unified(30302);
ivo_david_michelle 9:f1bd96708a21 15 gyro_ = Adafruit_L3GD20_Unified(20);
ivo_david_michelle 9:f1bd96708a21 16 // pc.printf("Entering control loop\n\r");
ivo_david_michelle 9:f1bd96708a21 17 }
ivo_david_michelle 9:f1bd96708a21 18
ivo_david_michelle 9:f1bd96708a21 19
ivo_david_michelle 9:f1bd96708a21 20
ivo_david_michelle 9:f1bd96708a21 21 void Quadcopter::initAllSensors()
ivo_david_michelle 9:f1bd96708a21 22 {
ivo_david_michelle 9:f1bd96708a21 23 print("Initializing Sensors with class method");
ivo_david_michelle 9:f1bd96708a21 24
ivo_david_michelle 9:f1bd96708a21 25 initSensors(accel_, mag_, gyro_,offsetAngRate_); // IMU
ivo_david_michelle 6:6f3ffd97d808 26 }
ivo_david_michelle 9:f1bd96708a21 27
ivo_david_michelle 9:f1bd96708a21 28
ivo_david_michelle 9:f1bd96708a21 29 void Quadcopter::readSensorValues()
ivo_david_michelle 9:f1bd96708a21 30 {
ivo_david_michelle 9:f1bd96708a21 31 accel_.getEvent(&accel_event_);
ivo_david_michelle 9:f1bd96708a21 32 if (dof_.accelGetOrientation(&accel_event_, &orientation_)) {
ivo_david_michelle 9:f1bd96708a21 33 /* 'orientation' should have valid .roll and .pitch fields */
ivo_david_michelle 9:f1bd96708a21 34 //s_com->print(("Roll: "));
ivo_david_michelle 9:f1bd96708a21 35 //s_com->print(orientation.roll);
ivo_david_michelle 9:f1bd96708a21 36 //s_com->print(("; "));
ivo_david_michelle 9:f1bd96708a21 37 //s_com->print(("Pitch: "));
ivo_david_michelle 9:f1bd96708a21 38 //s_com->print(orientation.pitch);
ivo_david_michelle 9:f1bd96708a21 39 //s_com->print((";\t"));
ivo_david_michelle 9:f1bd96708a21 40 }
ivo_david_michelle 9:f1bd96708a21 41 /* Calculate the heading using the magnetometer */
ivo_david_michelle 9:f1bd96708a21 42 mag_.getEvent(&mag_event_);
ivo_david_michelle 9:f1bd96708a21 43 if (dof_.magGetOrientation(SENSOR_AXIS_Z, &mag_event_, &orientation_)) {
ivo_david_michelle 9:f1bd96708a21 44 /* 'orientation' should have valid .heading data now */
ivo_david_michelle 9:f1bd96708a21 45 //s_com->print(("Heading: "));
ivo_david_michelle 9:f1bd96708a21 46 //s_com->print(orientation.heading);
ivo_david_michelle 9:f1bd96708a21 47 // s_com->print((";\r\n"));
ivo_david_michelle 9:f1bd96708a21 48 }
ivo_david_michelle 9:f1bd96708a21 49
ivo_david_michelle 9:f1bd96708a21 50
ivo_david_michelle 9:f1bd96708a21 51
ivo_david_michelle 9:f1bd96708a21 52 }
ivo_david_michelle 9:f1bd96708a21 53
ivo_david_michelle 9:f1bd96708a21 54
ivo_david_michelle 6:6f3ffd97d808 55 // Date member function
ivo_david_michelle 8:326e7009ce0c 56 void Quadcopter::setState(double phi, double theta, double psi)
ivo_david_michelle 6:6f3ffd97d808 57 {
ivo_david_michelle 8:326e7009ce0c 58 state_.phi = phi;
ivo_david_michelle 8:326e7009ce0c 59 state_.theta = theta;
ivo_david_michelle 8:326e7009ce0c 60 state_.psi = psi;
ivo_david_michelle 9:f1bd96708a21 61 }
ivo_david_michelle 9:f1bd96708a21 62
ivo_david_michelle 9:f1bd96708a21 63 void Quadcopter::print(char * myString){
ivo_david_michelle 9:f1bd96708a21 64
ivo_david_michelle 9:f1bd96708a21 65 (*pcPntr_).printf(myString);
ivo_david_michelle 9:f1bd96708a21 66 (*pcPntr_).printf("\n\r");
ivo_david_michelle 9:f1bd96708a21 67
ivo_david_michelle 9:f1bd96708a21 68 }