iforce2d Chris
/
04_I2C_MPU6050
YMFC-AL implementation in mbed.
Revision 1:411d267f9d32, committed 2019-10-04
- Comitter:
- iforce2d
- Date:
- Fri Oct 04 17:04:41 2019 +0000
- Parent:
- 0:f76c26307f9a
- Commit message:
- First commit
Changed in this revision
diff -r f76c26307f9a -r 411d267f9d32 PPM.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PPM.cpp Fri Oct 04 17:04:41 2019 +0000 @@ -0,0 +1,27 @@ +#include "mbed.h" +#include "PPM.h" + +PPM::PPM(PinName pin): ppm(pin) +{ + for (int i = 0; i < NUM_CHANNELS; i++) + channels[i] = 1500; + currentChannel = 0; + timer.start(); + ppm.rise( callback(this, &PPM::rise) ); +} + +void PPM::rise() +{ + uint16_t time = timer.read_us(); + timer.reset(); + + if ( time > 2500 ) + { + currentChannel = 0; + } + else if ( currentChannel < NUM_CHANNELS ) + { + channels[currentChannel] = time; + currentChannel++; + } +} \ No newline at end of file
diff -r f76c26307f9a -r 411d267f9d32 PPM.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PPM.h Fri Oct 04 17:04:41 2019 +0000 @@ -0,0 +1,20 @@ +#ifndef PPM_IN +#define PPM_IN + +class PPM +{ + public: + static const uint8_t NUM_CHANNELS = 6; + uint16_t channels[NUM_CHANNELS]; + + PPM(PinName pin); + + void rise(); + + protected: + InterruptIn ppm; + Timer timer; + uint8_t currentChannel; +}; + +#endif \ No newline at end of file
diff -r f76c26307f9a -r 411d267f9d32 main.cpp --- a/main.cpp Thu Feb 14 17:24:48 2013 +0000 +++ b/main.cpp Fri Oct 04 17:04:41 2019 +0000 @@ -1,25 +1,483 @@ #include "mbed.h" - -// Read temperature from LM75BD +#include "PPM.h" + +I2C i2c(PB_11, PB_10); // sda, scl +Serial pc(PA_9, PA_10, 115200); // tx, rx +DigitalOut led_green(PB_3); +DigitalOut led_red(PB_4); +PPM ppm(PA_0); + +DigitalOut motorPin1(PA_8); +DigitalOut motorPin2(PA_11); +DigitalOut motorPin3(PB_6); +DigitalOut motorPin4(PB_7); +DigitalOut motorPin5(PB_8); +DigitalOut motorPin6(PB_9); + +DigitalOut buzzer(PA_12); + +#define ROLL 0 +#define PITCH 1 +#define YAW 2 + +#define RC_ROLL (ppm.channels[0]) +#define RC_PITCH (ppm.channels[1]) +#define RC_THROTTLE (ppm.channels[2]) +#define RC_YAW (ppm.channels[3]) +#define RC_DEADBAND_US 8 + +#define GYRO_CALIB_SAMPLES 4000 +#define MPU6050 (0x68 << 1) + +float pid_p_gain_roll = 1.3; //Gain setting for the roll P-controller +float pid_i_gain_roll = 0.04; //Gain setting for the roll I-controller +float pid_d_gain_roll = 18.0; //Gain setting for the roll D-controller +int pid_max_roll = 400; //Maximum output of the PID-controller (+/-) + +float pid_p_gain_pitch = pid_p_gain_roll; //Gain setting for the pitch P-controller. +float pid_i_gain_pitch = pid_i_gain_roll; //Gain setting for the pitch I-controller. +float pid_d_gain_pitch = pid_d_gain_roll; //Gain setting for the pitch D-controller. +int pid_max_pitch = pid_max_roll; //Maximum output of the PID-controller (+/-) + +float pid_p_gain_yaw = 4.0; //Gain setting for the pitch P-controller. //4.0 +float pid_i_gain_yaw = 0.02; //Gain setting for the pitch I-controller. //0.02 +float pid_d_gain_yaw = 0.0; //Gain setting for the pitch D-controller. +int pid_max_yaw = 400; //Maximum output of the PID-controller (+/-) + +int cal_int; +double gyro_cal[3]; +int16_t acc_raw[3], gyro_raw[3]; +float acc_smoothed[3], gyro_smoothed[3]; + +Timer armingTimer; + +Timer timer; + +float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll; +float acc_total_vector; + +float roll_level_adjust, pitch_level_adjust; +float pid_i_mem_roll, pid_roll_setpoint, pid_output_roll, pid_last_roll_d_error; +float pid_i_mem_pitch, pid_pitch_setpoint, pid_output_pitch, pid_last_pitch_d_error; +float pid_i_mem_yaw, pid_yaw_setpoint, pid_output_yaw, pid_last_yaw_d_error; + +int constrain( int val, int lower, int upper ) +{ + if ( val < lower ) return lower; + if ( val > upper ) return upper; + return val; +} + +void beepStart() +{ + buzzer = 0; +} + +void beepStop() +{ + buzzer = 1; +} + +void beep(int ms) +{ + beepStart(); + wait_ms(ms); + beepStop(); +} + +void setIMURegisters() +{ + char cmd[2]; + + cmd[0] = 0x6B; //We want to write to the PWR_MGMT_1 register (6B hex) + cmd[1] = 0x00; //Set the register bits as 00000000 to activate the gyro + i2c.write(MPU6050, cmd, 2); + + cmd[0] = 0x1B; //We want to write to the GYRO_CONFIG register (1B hex) + cmd[1] = 0x08; //Set the register bits as 00001000 (500dps full scale) + i2c.write(MPU6050, cmd, 2); + + cmd[0] = 0x1C; //We want to write to the ACCEL_CONFIG register (1A hex) + cmd[1] = 0x10; //Set the register bits as 00010000 (+/- 8g full scale range) + i2c.write(MPU6050, cmd, 2); + + //Let's perform a random register check to see if the values are written correct + cmd[0] = 0x1B; //Start reading at register 0x1B + i2c.write(MPU6050, cmd, 1); + i2c.read(MPU6050, cmd, 1); + if (cmd[0] != 0x08) { //Check if the value is 0x08 + led_red = 0; //Turn on the warning led + pc.printf("Gyro init failed! (%d)\n", cmd[0]); + while (true) + wait(1); //Stay in this loop for ever + } + + cmd[0] = 0x1A; //We want to write to the CONFIG register (1A hex) + cmd[1] = 0x03; //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz) + i2c.write(MPU6050, cmd, 2); +} + +void readIMU(bool calibrated = true) +{ + char cmd[14]; -I2C i2c(p28, p27); + cmd[0] = 0x3B; //Start reading at register 43h and auto increment with every read. + i2c.write(MPU6050, cmd, 1); + i2c.read(MPU6050, cmd, 14); //Read 14 bytes from the gyro. + + acc_raw[0] = (cmd[0] << 8) | cmd[1]; //Add the low and high byte to the acc_x variable. + acc_raw[1] = (cmd[2] << 8) | cmd[3]; //Add the low and high byte to the acc_y variable. + acc_raw[2] = (cmd[4] << 8) | cmd[5]; //Add the low and high byte to the acc_z variable. + + gyro_raw[0] = (cmd[8] << 8) | cmd[9]; //Read high and low part of the angular data. + gyro_raw[1] = (cmd[10] << 8) | cmd[11]; //Read high and low part of the angular data. + gyro_raw[2] = (cmd[12] << 8) | cmd[13]; //Read high and low part of the angular data. + + if ( calibrated ) { + for (int i = 0; i < 3; i++) + gyro_raw[i] -= gyro_cal[i]; + } +} + +void calibrateGyro() +{ + for (int i = 0; i < 3; i++) + gyro_cal[i] = 0; + + for (int s = 0; s < GYRO_CALIB_SAMPLES; s++) { //Take GYRO_CALIB_SAMPLES readings for calibration. + if (s % 50 == 0) + led_green = 1 - led_green; //Change the led status to indicate calibration. + readIMU(false); //Read the gyro output. + for (int i = 0; i < 3; i++) + gyro_cal[i] += gyro_raw[i]; //Ad roll value to gyro_roll_cal. + wait_us(3); //Wait 3 milliseconds before the next loop. + } + + for (int i = 0; i < 3; i++) + gyro_cal[i] /= (float)GYRO_CALIB_SAMPLES; //Divide the roll total by GYRO_CALIB_SAMPLES. +} -const int addr = 0x90; +bool checkArmStateChange(bool armed) +{ + static int armingState = 0; + + if ( RC_THROTTLE > 1100 ) + return false; + + if ( ! armed && RC_YAW > 1900 ) { + if ( armingState == 0 ) { + armingState = 1; + armingTimer.reset(); + armingTimer.start(); + pc.printf("Started arming\n"); + } + else { + uint16_t time = armingTimer.read_ms(); + if ( time > 2000 ) { + armed = true; + armingState = 0; + pc.printf("ARMED\n"); + //beep(300); + return true; + } + } + } + else if ( armed && RC_YAW < 1100 ) { + if ( armingState == 0 ) { + armingState = -1; + armingTimer.reset(); + armingTimer.start(); + pc.printf("Started disarming\n"); + } + else { + uint16_t time = armingTimer.read_ms(); + if ( time > 2000 ) { + armed = false; + armingState = 0; + pc.printf("DISARMED\n"); + //beep(50); + //wait_ms(50); + //beep(50); + return false; + } + } + } + else { + armingState = 0; + armingTimer.reset(); + } + + return armed; +} + +bool getAutoLevelState() +{ + return ppm.channels[5] > 1600; +} + +int afterDeadband(int in) +{ + int lower = 1500 - RC_DEADBAND_US; + int upper = 1500 + RC_DEADBAND_US; + if ( in < lower ) + return in - lower; + if ( in > upper ) + return in - upper; + return 0; +} + +void resetPID() +{ + pid_i_mem_roll = 0; + pid_last_roll_d_error = 0; + pid_i_mem_pitch = 0; + pid_last_pitch_d_error = 0; + pid_i_mem_yaw = 0; + pid_last_yaw_d_error = 0; +} + +void calculatePID() +{ + //Roll calculations + float pid_error_temp = gyro_smoothed[ROLL] - pid_roll_setpoint; + pid_i_mem_roll += pid_i_gain_roll * pid_error_temp; + if(pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll; + else if(pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1; + + pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error); + if(pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll; + else if(pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1; + + pid_last_roll_d_error = pid_error_temp; + + //Pitch calculations + pid_error_temp = gyro_smoothed[PITCH] - pid_pitch_setpoint; + pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp; + if(pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch; + else if(pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1; + + pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error); + if(pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch; + else if(pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1; + + pid_last_pitch_d_error = pid_error_temp; + + //Yaw calculations + pid_error_temp = gyro_smoothed[YAW] - pid_yaw_setpoint; + pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp; + if(pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw; + else if(pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1; + + pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error); + if(pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw; + else if(pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1; + + pid_last_yaw_d_error = pid_error_temp; +} int main() { - char cmd[2]; - while (1) { - cmd[0] = 0x01; - cmd[1] = 0x00; - i2c.write(addr, cmd, 2); - - wait(0.5); - - cmd[0] = 0x00; - i2c.write(addr, cmd, 1); - i2c.read(addr, cmd, 2); - - float tmp = (float((cmd[0]<<8)|cmd[1]) / 256.0); - printf("Temp = %.2f\n", tmp); + + i2c.frequency(400000); + + buzzer = 1; + led_red = 1; + led_green = 1; + + motorPin1 = 0; + motorPin2 = 0; + motorPin3 = 0; + motorPin4 = 0; + motorPin5 = 0; + motorPin6 = 0; + + armingTimer.reset(); + + beep(50); + + pc.printf("setIMURegisters\n"); + setIMURegisters(); + + pc.printf("calibrateGyro\n"); + calibrateGyro(); + + pc.printf("Finished gyro calibration (%f, %f, %f)\n", gyro_cal[0], gyro_cal[1], gyro_cal[2]); + + timer.reset(); + timer.start(); + + for (int i = 0; i < 3; i++) + gyro_smoothed[i] = 0; + angle_pitch = 0; + angle_roll = 0; + + beep(50); + wait_ms(50); + beep(50); + + bool armed = false; + bool armedLastTime = armed; + + bool autoLevel = getAutoLevelState(); + bool autoLevelLastTime = autoLevel; + + unsigned long oneShotCounter = 0; + + unsigned long loopCounter = 0; + + float initialAccUptake = 0.8; + + while (true) { + + //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]); + //pc.printf("Accel (%f, %f)\n", angle_pitch, angle_roll); + + armed = checkArmStateChange(armed); + led_green = ! armed; + + if ( ! armedLastTime && armed ) + resetPID(); + if ( armed != armedLastTime ) { + oneShotCounter = 20; + beepStart(); + } + armedLastTime = armed; + + autoLevel = getAutoLevelState(); + if ( autoLevel != autoLevelLastTime ) { + oneShotCounter = 4; + beepStart(); + } + autoLevelLastTime = autoLevel; + + if ( oneShotCounter > 0 ) { + if ( --oneShotCounter == 0 ) { + beepStop(); + } + } + + const float uptake = 0.3; + for (int i = 0; i < 3; i++) + gyro_smoothed[i] = (gyro_smoothed[i] * (1-uptake)) + ((gyro_raw[i] / 65.5) * uptake); //Gyro pid input is deg/sec. + + + //Gyro angle calculations + //0.0000611 = 1 / (250Hz / 65.5) + angle_pitch += gyro_raw[ROLL] * 0.0000611; //Calculate the traveled pitch angle and add this to the angle_pitch variable. + angle_roll += gyro_raw[PITCH] * 0.0000611; //Calculate the traveled roll angle and add this to the angle_roll variable. + + //0.000001066 = 0.0000611 * (3.142(PI) / 180degr) The Arduino sin function is in radians + float new_angle_pitch = angle_pitch + angle_roll * sin(gyro_raw[YAW] * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel. + angle_roll -= angle_pitch * sin(gyro_raw[YAW] * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel. + angle_pitch = new_angle_pitch; + + //Accelerometer angle calculations + acc_total_vector = sqrtf((acc_raw[0]*acc_raw[0])+(acc_raw[1]*acc_raw[1])+(acc_raw[2]*acc_raw[2])); //Calculate the total accelerometer vector. + + if (abs(acc_raw[PITCH]) < acc_total_vector) { //Prevent the asin function to produce a NaN + angle_pitch_acc = asin((float)acc_raw[PITCH]/acc_total_vector)* 57.296; //Calculate the pitch angle. + } + if (abs(acc_raw[ROLL]) < acc_total_vector) { //Prevent the asin function to produce a NaN + angle_roll_acc = asin((float)acc_raw[ROLL]/acc_total_vector)* -57.296; //Calculate the roll angle. + } + + //Place the MPU-6050 spirit level and note the values in the following two lines for calibration. + angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch. + angle_roll_acc -= 0.0; //Accelerometer calibration value for roll. + + initialAccUptake *= 0.98; + + float acc_uptake = 0.0004 + initialAccUptake; + angle_pitch = angle_pitch * (1-acc_uptake) + angle_pitch_acc * acc_uptake; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle. + angle_roll = angle_roll * (1-acc_uptake) + angle_roll_acc * acc_uptake; //Correct the drift of the gyro roll angle with the accelerometer roll angle. + + pitch_level_adjust = angle_pitch * 15; //Calculate the pitch angle correction + roll_level_adjust = angle_roll * 15; //Calculate the roll angle correction + + if ( ! autoLevel ){ //If the quadcopter is not in auto-level mode + pitch_level_adjust = 0; //Set the pitch angle correction to zero. + roll_level_adjust = 0; //Set the roll angle correcion to zero. + } + + + + //The PID set point in degrees per second is determined by the roll receiver input. + //In the case of deviding by 3 the max roll rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ). + pid_roll_setpoint = afterDeadband(RC_ROLL); + pid_roll_setpoint -= roll_level_adjust; //Subtract the angle correction from the standardized receiver roll input value. + pid_roll_setpoint /= 3.0; //Divide the setpoint for the PID roll controller by 3 to get angles in degrees. + + //The PID set point in degrees per second is determined by the pitch receiver input. + //In the case of deviding by 3 the max pitch rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ). + pid_pitch_setpoint = afterDeadband(RC_PITCH); + pid_pitch_setpoint -= pitch_level_adjust; //Subtract the angle correction from the standardized receiver pitch input value. + pid_pitch_setpoint /= 3.0; //Divide the setpoint for the PID pitch controller by 3 to get angles in degrees. + + //The PID set point in degrees per second is determined by the yaw receiver input. + //In the case of dividing by 3 the max yaw rate is aprox 164 degrees per second ( (500-8)/3 = 164d/s ). + pid_yaw_setpoint = 0; + //We need a little dead band of 16us for better results. + if (RC_THROTTLE > 1050) { //Do not yaw when turning off the motors. + pid_yaw_setpoint = afterDeadband(RC_YAW) / 3.0; + } + + calculatePID(); + + int pwm[6]; + + if (armed) { //The motors are started. + int throttle = RC_THROTTLE; + if (throttle > 1800) + throttle = 1800; //We need some room to keep full control at full throttle. + pwm[0] = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 1 (front-right - CCW) + pwm[1] = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW) + pwm[2] = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (rear-left - CCW) + pwm[3] = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 4 (front-left - CW) + pwm[4] = 1000; + pwm[5] = 1000; + + for (int i = 0; i < 6; i++) + pwm[i] = constrain( pwm[i], 1100, 2000); //Keep the motors running. + } + else { + for (int i = 0; i < 6; i++) + pwm[i] = 1000; //If start is not 2 keep a 1000us pulse for ess-1. + } + + + + + while ( timer.read_us() < 4000 ) + ; // wait + + motorPin1 = motorPin2 = motorPin3 = motorPin4 = 1; + timer.reset(); + + readIMU(); + + if ( loopCounter++ % 20 == 0 ) { + //pc.printf("%d %d %d %d\n", pwm[0], pwm[1], pwm[2], pwm[3]); + pc.printf("%f %f\n", angle_roll, angle_pitch ); + //pc.printf("%d\n", pwm[0] ); + //pc.printf("Gyro (%f, %f, %f)\n", gyro_smoothed[0], gyro_smoothed[1], gyro_smoothed[2]); + //pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]); + } + + bool doneOutput = false; + while ( ! doneOutput ) { + int now = timer.read_us(); + int c = 0; + if ( now >= pwm[0] ) { motorPin1 = 0; c++; } + if ( now >= pwm[1] ) { motorPin2 = 0; c++; } + if ( now >= pwm[2] ) { motorPin3 = 0; c++; } + if ( now >= pwm[3] ) { motorPin4 = 0; c++; } + if ( now >= pwm[4] ) { motorPin5 = 0; c++; } + if ( now >= pwm[5] ) { motorPin6 = 0; c++; } + doneOutput = c >= 6; + } + } -} \ No newline at end of file + + /*while (true) { + pc.printf("%d, %d, %d, %d, %d, %d\n", ppm.channels[0], ppm.channels[1], ppm.channels[2], ppm.channels[3], ppm.channels[4], ppm.channels[5]); + }*/ +}
diff -r f76c26307f9a -r 411d267f9d32 mbed.bld --- a/mbed.bld Thu Feb 14 17:24:48 2013 +0000 +++ b/mbed.bld Fri Oct 04 17:04:41 2019 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/0954ebd79f59 \ No newline at end of file +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file