Kim Youngsik
/
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.
Fork of
1TEST_Flight_Protocol
by 
Kim Youngsik
Revision 3:cf5a27438655, committed 2017-08-31
- Comitter:
- skyyoungsik
- Date:
- Thu Aug 31 05:29:09 2017 +0000
- Parent:
- 2:3fe595ba6a7e
- Commit message:
- ROBOFRIEN
Changed in this revision
--- 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();
}
--- a/ROBOFRIEN_GUI/ROBOFRIEN_GUI.h Mon May 22 06:10:49 2017 +0000
+++ b/ROBOFRIEN_GUI/ROBOFRIEN_GUI.h Thu Aug 31 05:29:09 2017 +0000
@@ -64,10 +64,11 @@
int32_t limit_roll_rate, limit_pitch_rate, limit_yaw_rate;
int gain_px100[20], gain_dx100[20], gain_ix100[20];
float mag_x_avg,mag_y_avg,mag_z_avg;
- float calibrate_gap_roll,calibrate_gap_pitch;
+// float calibrate_gap_roll,calibrate_gap_pitch;
+ float cal_accel_bias[3];
uint8_t DPN_Info;
int pwm_info[4];
- void attitude_calibrate(float rollAngle, float pitchAngle);
+ void attitude_calibrate(float , float , float);
void write_compass_setting_to_eeprom();
private:
void trans_configuration_data(int id_dest, int data_parse_num, int data_parse_detail_num);
--- a/ROBOFRIEN_SUB_FUNC/Config.h Mon May 22 06:10:49 2017 +0000 +++ b/ROBOFRIEN_SUB_FUNC/Config.h Thu Aug 31 05:29:09 2017 +0000 @@ -12,6 +12,7 @@ #define PIN_ULTRA_ECHO A9 #define AHRS_INTERRUPT_PIN A10 // use pin A10 on Arduino Uno & most boards + //// EEPROM ////// // DPN1 // #define EEPROM_MODEL_TYPE1 0 @@ -75,13 +76,16 @@ #define EEPROM_AHRS_ROLL_GAP_DOWN 75 #define EEPROM_AHRS_PITCH_GAP_UP 76 #define EEPROM_AHRS_PITCH_GAP_DOWN 77 +#define EEPROM_AHRS_YAW_GAP_UP 78 +#define EEPROM_AHRS_YAW_GAP_DOWN 79 -#define EEPROM_AHRS_YAW_X_GAP_1 78 -#define EEPROM_AHRS_YAW_X_GAP_2 79 -#define EEPROM_AHRS_YAW_Y_GAP_1 80 -#define EEPROM_AHRS_YAW_Y_GAP_2 81 -#define EEPROM_AHRS_YAW_Z_GAP_1 82 -#define EEPROM_AHRS_YAW_Z_GAP_2 83 + +#define EEPROM_AHRS_YAW_X_GAP_1 80 +#define EEPROM_AHRS_YAW_X_GAP_2 81 +#define EEPROM_AHRS_YAW_Y_GAP_1 82 +#define EEPROM_AHRS_YAW_Y_GAP_2 83 +#define EEPROM_AHRS_YAW_Z_GAP_1 84 +#define EEPROM_AHRS_YAW_Z_GAP_2 85 #define EEPROM_AHRS_DECLINATION_ANGLE_UP 86 #define EEPROM_AHRS_DECLINATION_ANGLE_DOWN 87
--- a/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.cpp Mon May 22 06:10:49 2017 +0000
+++ b/ROBOFRIEN_SUB_FUNC/ROBOFRIEN_LED.cpp Thu Aug 31 05:29:09 2017 +0000
@@ -42,7 +42,9 @@
}
/////////// RESET //////////////
- if( LedTimer.read_ms() > 4000000000){
+ if( LedTimer.read_ms() > 1000000000){
+ ex_headlight_timer = 0;
+ ex_sidelight_timer = 0;
LedTimer.reset();
}