ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Diff: quadcopter.cpp
- Revision:
- 41:d103f9aa44f0
- Parent:
- 40:09a59d5b7944
- Child:
- 42:d09dec5bb184
diff -r 09a59d5b7944 -r d103f9aa44f0 quadcopter.cpp --- a/quadcopter.cpp Wed May 04 17:28:48 2016 +0000 +++ b/quadcopter.cpp Wed May 04 18:46:06 2016 +0000 @@ -75,6 +75,14 @@ void Quadcopter::readSensorValues() { + // precomputed gyro offsets + double offset_gyro_x = -0.048261598984772; + double offset_gyro_y = 0.499745812182741; + double offset_gyro_z = 0.005005893401015; + double offset_acc_x = -0.233396269401444; + double offset_acc_y = 0.198868464759687; + double offset_acc_z = -0.485550588350806; + if (prev_kalman_time == 0) { prev_kalman_time = controlTimer->read(); return; @@ -82,15 +90,27 @@ accel_.getEvent(&accel_event_); // mag_.getEvent(&mag_event_); + // detract offsets + accel_event.acceleration.x -= offset_acc_x; + accel_event.acceleration.y -= offset_acc_y; + accel_event.acceleration.z -= offset_acc_z; + dof_.accelGetOrientation(&accel_event_, &orientation_); gyro_.getEvent(&gyro_event_); +/* old offset detraction 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; +*/ + + + gyro_event_.gyro.x -= offset_gyro_x; + gyro_event_.gyro.y -= offset_gyro_y; + gyro_event_.gyro.z -= offset_gyro_z; static int current_filter = 0; filters_.p[current_filter] = gyro_event_.gyro.x * M_PI / 180; @@ -162,12 +182,12 @@ prev_kalman_time = time; - /* RAW DATA + // RAW DATA pc_->printf("%f %f %f %f %f %f %f \r\n", prev_kalman_time, accel_event_.acceleration.x, accel_event_.acceleration.y, accel_event_.acceleration.z, gyro_event_.gyro.x, gyro_event_.gyro.y, gyro_event_.gyro.z); - */ + // static int count = 0; // if (count % 100 == 0) { // pc_->printf("%d\r\n", count);