Diff: quadcopter.cpp

Revision:

16:2be2aab63198

Parent:

15:90e07946186f

Child:

17:96d0c72e413e

--- a/quadcopter.cpp	Thu Apr 07 21:14:27 2016 +0000
+++ b/quadcopter.cpp	Thu Apr 07 22:04:03 2016 +0000
@@ -35,10 +35,14 @@
     // initSensors(accel_, mag_, gyro_, offsetAngRate_);  // IMU
     
     // prepare for communication with remote control   
+    rcTimer_.start();
     mrf_ = mrfPntr;  // RF tranceiver to link with handheld.
-    rcIterations_ = 100;
     rcLength_ = 250;
-    rcChannel_ = 3;
+    mrf_->SetChannel(3);  //Set the Channel. 0 is default, 15 is max
+    thrust_ = 0.5;
+    yaw_ = 0.5;
+    pitch_ = 0.5;
+    roll_ = 0.5;
 }
 
 
@@ -111,53 +115,24 @@
 }
 
     
-void Quadcopter::readRc() {
-    //write to class variables additional thrust double F_des_, desired state: phi, theta, psi.
-    
-    // RF tranceiver to link with handheld.
-
-    // read from joystick: 
-    
-    //write to class variables additional thrust double F_des_, desired state: phi, theta, psi. 
-    
+void Quadcopter::readRc() {    
+    long long id = -1;
     uint8_t zero = 0;
     uint8_t *rssi = &zero;
 
     uint8_t receive = 0;
     int result = 0;
 
-    //Set the Channel. 0 is default, 15 is max
-    mrf_->SetChannel(rcChannel_);
-
-    //Start the timer
-    rcTimer_.start();
-    
-    pc_->printf("START\r\n");
-    
-    // TODO: change default value?
-    float thrust = 0.5;
-    float yaw = 0.5;
-    float pitch = 0.5;
-    float roll = 0.5;
-
     char rxBuffer[rcLength_];
     
-    // TODO: stop doing iterations, just do while recieving from slave
-    for (int i = 0; i < rcIterations_; i++) {
-        receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
-        if (receive > 0) {
-            pc_->printf("RX: %s \r\n", rxBuffer);
-            result = sscanf(rxBuffer, "%f,%f,%f,%f", &thrust, &yaw, &pitch, &roll);
-            if (result != 4) {
-                pc_->printf("Parse failure\r\n");
-            } else {
-                pc_->printf("%f,%f,%f,%f\r\n", thrust, yaw, pitch, roll);
-            }
-        } else {
-            pc_->printf("Receive failure\r\n");
+    receive = rf_receive_rssi(*mrf_, rxBuffer, rssi, rcLength_ + 1);
+    if (receive > 0) {
+        result = sscanf(rxBuffer, "%lld,%f,%f,%f,%f", &id, &thrust_, &yaw_, &pitch_, &roll_);
+        if (result == 5) {
+            pc_->printf("%lld: %f,%f,%f,%f\r\n", id, thrust_, yaw_, pitch_, roll_);
         }
-        wait(0.2);  // TODO: change this value?
-    }
-     pc_->printf("END\r\n");    
+    } else {
+        pc_->printf("Receive failure\r\n");
+    }  
 }