//-------------------------------------------------------------------------- // klaus-peter neitzke // quadcopter flight code // 2025 3 inch spektrum // last modification 1 march 2025 // S1 (links) gedrueckt = 0 und S2 (rechts) gedrueckt = 0 // arduino duemilanove atmega328 // rc_pitch rechter knueppel -680 ... 680 // rc_roll rechter knueppel -680 ... 680 // rc_yaw linker knueppel -680 ... 680 // rc_lift linker knueppel 0 ... 1360 // rc_poti poti h 0 ... 100 // rc_s1 schalter d 0 = gedrueckt, 1= mitte, 2= gezogen // rc_s2 schalter e 0 = gedrueckt, 1= mitte, 2= gezogen #include #define MPU6050 0x68 #define DLPF_CONFIG 0x1A #define ACCEL_CONFIG 0x1C #define GYRO_CONFIG 0x1B #define ACCEL_XOUT_H 0x3B #define GYRO_XOUT_H 0x43 #define PWR_MGMT_1 0x6B #define toRead 6 byte reading[toRead]; #define accSens 3 // 0 = 2g, 1 = 4g, 2 = 8g, 3 = 16g // 16384 8192 4096 2048 #define gyroSens 3 // 0 = 250deg/s, 1 = 500deg/s, 2 = 1000deg/s, 3 = 2000deg/s // 131.072 65.536 32.768 16.384 #define filterFreq 0 // 0=256hz, 1=188hz, 2=98hz, 3=42hz, 4=20hz, 5=10hz, 6=5hz //-------------------------------------------------------------------------- // flight variables int rc_lift, rc_pitch, rc_yaw, rc_roll, rc_s1, rc_s2, rc_poti; int rc_lift_old; int m1, m2, m3, m4; unsigned long uart_time_old; unsigned long imu_time_old; unsigned int i, z[16]; float Pi, Div180Pi, rx_h; float pitch_g_sens, roll_g_sens, yaw_g_sens; float eop, eor, eoy, ip, ir, iy; float hx, hy, hz; float hx_est, hy_est, hz_est, hx_meas, hy_meas, hz_meas; //-------------------------------------------------------------------------- // setup procedure void setup() { // 0 to the speed controller analogWrite( 9, 0); analogWrite(11, 0); analogWrite(10, 0); analogWrite( 3, 0); delay(5000); uart_time_old= 0; imu_time_old= 0; Pi= 3.14159; Div180Pi= 180 / Pi; rx_h= 0.001; // 0.001 kalman weight for height vector h for (i= 0; i<=15; i= i+1) z[i]= 0; i= 0; rc_pitch= 0; rc_yaw= 0; rc_roll= 0; rc_lift= 0; rc_s1= 2; // init d switch = gezogen = motoren aus rc_s2= 0; // init e switch = gedrueckt rc_poti= 50; rc_lift_old= 0; pitch_g_sens= 0; roll_g_sens= 0; yaw_g_sens= 0; // initial best orientation hx= 0; hy= 0; hz= 1; // PID controller eop= 0; eor= 0; eoy= 0; ip= 0; ir= 0; iy= 0; Serial.begin(115200); // spectrum satellite Wire.begin(); writeTo(MPU6050, PWR_MGMT_1, 0); // wake up call writeTo(MPU6050, ACCEL_CONFIG, accSens << 3); // scaling accelerometer writeTo(MPU6050, GYRO_CONFIG, gyroSens << 3); // scaling gyroscope writeTo(MPU6050, DLPF_CONFIG, filterFreq); // digital low pass filter } //-------------------------------------------------------------------------- // read the dx9 satellite procedure void read_rc() { int v; unsigned long uart_time; unsigned int uart_dt; uart_time= micros(); uart_dt= int(uart_time - uart_time_old); uart_time_old= uart_time; if (uart_dt > 4000) i= 0; else i++; z[i]= Serial.read(); // 16 new rc bytes available if (i == 15) { // correct channel order 2 1 3 0 6 4 5 in the first 5 bits if ( ((z[ 6] & 248) == 16) && ((z[ 2] & 248) == 8) && ((z[10] & 248) == 24) && ((z[14] & 248) == 0) && ((z[12] & 248) == 48) && ((z[ 8] & 248) == 32) && ((z[ 4] & 248) == 40)) { rc_lift_old= rc_lift; rc_pitch= int(256*(z[ 6] & 7) + z[ 7]) - 1024; rc_roll= 1024 - int(256*(z[ 2] & 7) + z[ 3]); rc_yaw= 1024 - int(256*(z[10] & 7) + z[11]); rc_lift= 1704 - int(256*(z[14] & 7) + z[15]); rc_poti= int(256*(z[12] & 7) + z[13] - 344)/14; v= int(256*(z[ 8] & 7) + z[ 9]) - 1024; if (v > 200) rc_s1= 0; else if (v < -200) rc_s1= 2; else rc_s1= 1; v= int(256*(z[ 4] & 7) + z[ 5]) - 1024; if (v > 200) rc_s2= 0; else if (v < -200) rc_s2= 2; else rc_s2= 1; } // correct channel order i= 0; // important to avoid writing on forbidden area z[>15] } // 16 new rc bytes available } //-------------------------------------------------------------------------- // write to the mpu6050 sensor void writeTo(byte device, byte address, byte value) { Wire.beginTransmission(device); Wire.write(address); Wire.write(value); Wire.endTransmission(); } //-------------------------------------------------------------------------- // read from the mpu6050 sensor void readFrom(byte device, byte address, byte bytes, byte reading[]) { Wire.beginTransmission(device); Wire.write(address); Wire.endTransmission(); Wire.requestFrom(device, bytes); while (bytes > Wire.available()) { } for (int i = 0; i < bytes; i++) { reading[i] = Wire.read(); } } //-------------------------------------------------------------------------- // read the mpu6050 imu values void read_imu() { readFrom(MPU6050, ACCEL_XOUT_H, toRead, reading); hx_meas = -(float)((reading[0] << 8) | reading[1])/2048 + 0.00; hy_meas = -(float)((reading[2] << 8) | reading[3])/2048 - 0.01; hz_meas = (float)((reading[4] << 8) | reading[5])/2048 - 0.01; readFrom(MPU6050, GYRO_XOUT_H, toRead, reading); pitch_g_sens = (float)((reading[0] << 8) | reading[1])/16.384 + 1.4; roll_g_sens = -(float)((reading[2] << 8) | reading[3])/16.384 + 0.8; yaw_g_sens = -(float)((reading[4] << 8) | reading[5])/16.384 + 0.8; } //-------------------------------------------------------------------------- // calculate the normal vector based on kalman filter void calc_imu() { unsigned long imu_time; float imu_dt; float pitch, roll, yaw; float res; imu_time= micros(); imu_dt= float(imu_time - imu_time_old) / 1e6; imu_time_old= imu_time; if (imu_dt > 0) { pitch= pitch_g_sens * imu_dt / Div180Pi; roll = roll_g_sens * imu_dt / Div180Pi; yaw = yaw_g_sens * imu_dt / Div180Pi; // estimation vector h from old status hx_est= + hx - yaw * hy - roll * hz; hy_est= + yaw * hx + hy - pitch * hz; hz_est= + roll * hx + pitch * hy + hz; res= sqrt(sq(hx_est) + sq(hy_est) + sq(hz_est)); if (res > 0.01) { hx_est= hx_est / res; hy_est= hy_est / res; hz_est= hz_est / res; } // best info from estimation and measurement hx= (1-rx_h) * hx_est + rx_h * hx_meas; hy= (1-rx_h) * hy_est + rx_h * hy_meas; hz= (1-rx_h) * hz_est + rx_h * hz_meas; } } //-------------------------------------------------------------------------- // calculate the motor values void calc_mot() { int L, P, R, Y; float ae, ape; L= 5 * rc_lift / 4 - 2 * rc_lift_old / 4; //yaw ae= 0.2*(float)(rc_yaw) - yaw_g_sens; iy= iy + ae; ape= ae + 0.1*iy; Y= (int)(3*ape-1.5*eoy); eoy= ape; //pitch 0.25->45 deg, 0.2->30 deg ae= 0.2*(float)(rc_pitch) + 250*hy; ip= ip + ae; ape= ae + 0.01*ip - 0.25*pitch_g_sens; P= (int)(10.0*ape - 8.0*eop); eop= ape; //roll 0.25->45 deg, 0.2->30 deg ae= 0.2*(float)(rc_roll) + 250*hx; ir= ir + ae; ape= ae + 0.01*ir - 0.25*roll_g_sens; R= (int)(10.0*ape - 8.0*eor); eor= ape; m1= (L - P - R - Y) / 10; m2= (L + P - R + Y) / 10; m3= (L + P + R - Y) / 10; m4= (L - P + R + Y) / 10; } //-------------------------------------------------------------------------- // drive the motor 1 2 3 4 void run_mot() { if (rc_s1 == 2) // d gezogen = stop { // 0 lift to the speed controller analogWrite( 9, 128); analogWrite(11, 128); analogWrite(10, 128); analogWrite( 3, 128); // initial best orientation hx= 0; hy= 0; hz= 1; // pid controller eop= 0; eor= 0; eoy= 0; ip= 0; ir= 0; iy= 0; } else // d mitte oder gedrueckt = flug { m1= constrain(140 + m1, 140, 242); m2= constrain(140 + m2, 140, 242); m3= constrain(140 + m3, 140, 242); m4= constrain(140 + m4, 140, 242); analogWrite( 9, m1); analogWrite(11, m2); analogWrite(10, m3); analogWrite( 3, m4); } } //-------------------------------------------------------------------------- // global loop procedure void loop() { while (Serial.available() == 0) { read_imu(); calc_imu(); calc_mot(); run_mot(); delay(1); } read_rc(); } // end of program //--------------------------------------------------------------------------