![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 | } |