Diff: quadcopter.cpp

Revision:

24:e220fbb70ded

Parent:

23:04338a5ef404

Child:

25:d44610851105

diff -r 04338a5ef404 -r e220fbb70ded quadcopter.cpp
--- a/quadcopter.cpp	Tue Apr 12 22:54:16 2016 +0000
+++ b/quadcopter.cpp	Wed Apr 13 00:57:15 2016 +0000
@@ -2,7 +2,6 @@
 #include "sensor.h"
 #include "receiver.h"
 #include <string>
-#include <math.h>
 
 #ifndef M_PI
 #define M_PI           3.14159265358979323846
@@ -12,19 +11,17 @@
 
 // constructor
 Quadcopter::Quadcopter(Serial *pcPntr, MRF24J40 *mrfPntr)
-{   
+{
     pc_= pcPntr;  // enable printing
-    initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
-
-    m_= 1;
+    //initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMUm_= 1;
     g_= 9.81;
     l_= 0.25;
     gamma_= 1;
-    
-    
+
+
     // proportional attitude control gains
-    // TODO change gains so that joystick deflection never produces pwm duty cycle >10%. 
-    
+    // TODO change gains so that joystick deflection never produces pwm duty cycle >10%.
+
     kp_f_ =2.5;
     kp_phi_ = 0.2;
     kp_theta_ = 0.2;
@@ -43,10 +40,10 @@
     mag_ = Adafruit_LSM303_Mag_Unified(30302);
     gyro_ = Adafruit_L3GD20_Unified(20);
     //motor1_(p21);
-    
+
     // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
-    
-    // prepare for communication with remote control   
+
+    // prepare for communication with remote control
     rcTimer_.start();
     mrf_ = mrfPntr;  // RF tranceiver to link with handheld.
     rcLength_ = 250;
@@ -56,6 +53,8 @@
     pitch = 0.5;
     roll = 0.5;
     id = -1;
+    initial_offsets_ = (offset*) malloc(sizeof(offset));
+    initSensors(*this);  // IMU
 }
 
 
@@ -71,10 +70,12 @@
 
     gyro_.getEvent(&gyro_event_);
 
-    gyro_event_.gyro.x -= offsetAngRate_.x;
-    gyro_event_.gyro.y -= offsetAngRate_.y;
-    gyro_event_.gyro.z -= offsetAngRate_.z;
-
+    gyro_event_.gyro.x   -= initial_offsets_->gyro_x;
+    gyro_event_.gyro.y   -= initial_offsets_->gyro_y;
+    gyro_event_.gyro.z   -= initial_offsets_->gyro_z;
+    orientation_.roll    -= initial_offsets_->roll;
+    orientation_.pitch   -= initial_offsets_->pitch;
+    orientation_.heading -= initial_offsets_->heading;
 
     // measured values (will come from IMU/parameter class/Input to function later)
     // angles
@@ -85,15 +86,15 @@
     state_.p = gyro_event_.gyro.x;
     state_.q = gyro_event_.gyro.y;
     state_.r = gyro_event_.gyro.z;
-    
+
     // TODO print values to check what they are
     // TODO convert to Radians (*pi/180)
-    
+
     pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
     state_.phi = orientation_.roll * M_PI / 180;
-    state_.theta =orientation_.pitch * M_PI / 180;
-    state_.psi =orientation_.heading * M_PI / 180;
-    //pc_->printf("Converted Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
+    state_.theta = orientation_.pitch * M_PI / 180;
+    state_.psi = orientation_.heading * M_PI / 180;
+    //pc_->printf("Roll: %f\tPitch: %f\tYaw: %f\tVel x: %f\tVel y: %f\tVel z: %f\r\n", state_.phi, state_.theta, state_.psi, state_.p, state_.q, state_.r);
 }
 
 // Date member function
@@ -105,13 +106,10 @@
     goal->p = source->p;
     goal->q = source->q;
     goal->r = source->r;
-
 }
 
-
 void Quadcopter::controller()
 {
-
     // compute desired angles (in the case we decide not to set
     // the angles, but for instance the velocity with the Joystick
 
@@ -124,54 +122,73 @@
 
     //print("F: %f M_x: %f M_y: %f M_z: %f\n\r",  controlInput_.f, controlInput_.mz, controlInput_.my, controlInput_.mz);
     //        pc_->printf("F: %f\n\r",  F);
-    
+
     // set pwm values
     // make code faster by precomputing all the components that are used multiple times and hardcode 0.25/gamma...
     double zeroVeloPwm=0.1;
     motorPwm_.m1=zeroVeloPwm+ 0.25*controlInput_.f-0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
-    motorPwm_.m2=zeroVeloPwm +0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz; 
+    motorPwm_.m2=zeroVeloPwm +0.25*controlInput_.f-0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
     motorPwm_.m3=zeroVeloPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.my-0.25/gamma_*controlInput_.mz;
     motorPwm_.m4=zeroVeloPwm + 0.25*controlInput_.f+0.5/l_*controlInput_.mx+0.25/gamma_*controlInput_.mz;
-    
 }
 
 motors Quadcopter::getPwm()
 {
-
     return motorPwm_;
 }
 
 state Quadcopter::getState()
 {
-
     return state_;
 }
 
-    
-void Quadcopter::readRc() {    
+Adafruit_LSM303_Accel_Unified Quadcopter::getAccel()
+{
+    return accel_;
+}
+
+Adafruit_LSM303_Mag_Unified Quadcopter::getMag()
+{
+    return mag_;
+}
+
+Adafruit_L3GD20_Unified Quadcopter::getGyro()
+{
+    return gyro_;
+}
+
+offset* Quadcopter::getOffset()
+{
+    return initial_offsets_;
+}
+
+Adafruit_9DOF Quadcopter::getIMU()
+{
+    return dof_;
+}
+
+void Quadcopter::readRc()
+{
     uint8_t zero = 0;
     uint8_t *rssi = &zero;
-    
 
     uint8_t receive = 0;
 
     char rxBuffer[rcLength_];
-    
-    
+
+
     receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
     if (receive > 0) {
         sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust, &yaw, &pitch, &roll);
     } else {
         pc_->printf("Receive failure\r\n");
-    }  
-    
-    // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian). 
-    
+    }
+
+    // TODO convert to radians (starting point: full joystick deflection = +-10° (converted to radian).
+
     desiredState_.phi=roll-0.5;
     desiredState_.theta=pitch-0.5;
     desiredState_.psi=yaw-0.5;
     F_des_=thrust-0.5;
-    
-    
 }