Kim Youngsik / Mbed 2 deprecated 1TEST_Flight_Protocol_v1

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of 1TEST_Flight_Protocol by Kim Youngsik

Diff: ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp

Revision:
3:cf5a27438655
Parent:
2:3fe595ba6a7e
diff -r 3fe595ba6a7e -r cf5a27438655 ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp
--- a/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp	Mon May 22 06:10:49 2017 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.cpp	Thu Aug 31 05:29:09 2017 +0000
@@ -145,8 +145,9 @@
         gain_dx100[i] = (int)eeprom_read(EEPROM_GAIN_D_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_D_DOWN[i]);
         gain_ix100[i] = (int)eeprom_read(EEPROM_GAIN_I_UP[i]) * 256 + eeprom_read(EEPROM_GAIN_I_DOWN[i]);
     }
-    calibrate_gap_roll = (((int)eeprom_read(EEPROM_AHRS_ROLL_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_ROLL_GAP_DOWN))  - 32767)/100.0;
-    calibrate_gap_pitch = (((int)eeprom_read(EEPROM_AHRS_PITCH_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_PITCH_GAP_DOWN))  - 32767)/100.0;
+    cal_accel_bias[0] = (((int)eeprom_read(EEPROM_AHRS_ROLL_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_ROLL_GAP_DOWN))  - 32767)/10000.0;
+    cal_accel_bias[1] = (((int)eeprom_read(EEPROM_AHRS_PITCH_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_PITCH_GAP_DOWN))  - 32767)/10000.0;
+    cal_accel_bias[2] = (((int)eeprom_read(EEPROM_AHRS_YAW_GAP_UP) * 256 + eeprom_read(EEPROM_AHRS_YAW_GAP_DOWN))  - 32767)/10000.0;
 
     // --------- Sensor ----------- ///
     mag_x_avg = (float)eeprom_read(EEPROM_AHRS_YAW_X_GAP_1) * 256 + eeprom_read(EEPROM_AHRS_YAW_X_GAP_2) - 32767 ;
@@ -681,13 +682,18 @@
     }
 }
 
-void ROBOFRIEN_GUI::attitude_calibrate(float rollAngle, float pitchAngle){
-    calibrate_gap_roll += rollAngle;
-    calibrate_gap_pitch += pitchAngle;
-    eeprom_write(EEPROM_AHRS_ROLL_GAP_UP, (((unsigned long)(calibrate_gap_roll * 100 + 32767) / 256)));
-    eeprom_write(EEPROM_AHRS_ROLL_GAP_DOWN, (((unsigned long)(calibrate_gap_roll * 100 + 32767) % 256)));
-    eeprom_write(EEPROM_AHRS_PITCH_GAP_UP, (((unsigned long)(calibrate_gap_pitch * 100 + 32767) / 256)));
-    eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(calibrate_gap_pitch * 100 + 32767) % 256)));
+void ROBOFRIEN_GUI::attitude_calibrate(float in_acc_bias_x, float in_acc_bias_y, float in_acc_bias_z){
+//    calibrate_gap_roll += rollAngle;
+//    calibrate_gap_pitch += pitchAngle;
+    cal_accel_bias[0] = in_acc_bias_x;
+    cal_accel_bias[1] = in_acc_bias_y;
+    cal_accel_bias[2] = (in_acc_bias_z);
+    eeprom_write(EEPROM_AHRS_ROLL_GAP_UP, (((unsigned long)(in_acc_bias_x * 10000 + 32767) / 256)));
+    eeprom_write(EEPROM_AHRS_ROLL_GAP_DOWN, (((unsigned long)(in_acc_bias_x * 10000 + 32767) % 256)));
+    eeprom_write(EEPROM_AHRS_PITCH_GAP_UP, (((unsigned long)(in_acc_bias_y * 10000 + 32767) / 256)));
+    eeprom_write(EEPROM_AHRS_PITCH_GAP_DOWN, (((unsigned long)(in_acc_bias_y * 10000 + 32767) % 256)));
+    eeprom_write(EEPROM_AHRS_YAW_GAP_UP, (((unsigned long)(in_acc_bias_z * 10000 + 32767) / 256)));
+    eeprom_write(EEPROM_AHRS_YAW_GAP_DOWN, (((unsigned long)(in_acc_bias_z * 10000 + 32767) % 256)));
     eeprom_refresh();
 }