ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 16:2be2aab63198
- Parent:
- 15:90e07946186f
- Child:
- 17:96d0c72e413e
diff -r 90e07946186f -r 2be2aab63198 quadcopter.cpp --- 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"); + } }